diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000000..cc55fefd25 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,19 @@ +--- +Checks: ' +bugprone-*, +-bugprone-integer-division, +-bugprone-narrowing-conversions, +performance-*, +clang-analyzer-*, +misc-*, +-misc-unused-parameters, +modernize-*, +-modernize-avoid-c-arrays, +-modernize-deprecated-headers, +-modernize-use-auto, +-modernize-use-using, +-modernize-use-nullptr, +-modernize-use-trailing-return-type, +' +CheckOptions: +... diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000000..cfc34e5848 --- /dev/null +++ b/.dockerignore @@ -0,0 +1,39 @@ +**/.git +.DS_Store +*.dylib +*.DSYM +*.d +*.pyc +*.pyo +.*.swp +.*.swo +.*.un~ +*.tmp +*.o +*.o-* +*.os +*.os-* +*.so +*.a + +venv/ +.venv/ + +notebooks +phone +massivemap +neos +installer +chffr/app2 +chffr/backend/env +selfdrive/nav +selfdrive/baseui +selfdrive/test/simulator2 +**/cache_data +xx/plus +xx/community +xx/projects +!xx/projects/eon_testing_master +!xx/projects/map3d +xx/ops +xx/junk diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000000..879e6eebca --- /dev/null +++ b/.editorconfig @@ -0,0 +1,11 @@ +root = true + +[*] +end_of_line = lf +insert_final_newline = true +trim_trailing_whitespace = true + +[{*.py, *.pyx, *.pxd}] +charset = utf-8 +indent_style = space +indent_size = 2 diff --git a/.gitignore b/.gitignore index 3e91531d08..370335e6a0 100644 --- a/.gitignore +++ b/.gitignore @@ -10,7 +10,6 @@ venv/ .overlay_init .overlay_consistent .sconsign.dblite -.vscode* model2.png a.out .hypothesis @@ -41,8 +40,9 @@ compile_commands.json compare_runtime*.html persist -board/obj/ -selfdrive/boardd/boardd +selfdrive/pandad/pandad +cereal/services.h +cereal/gen selfdrive/logcatd/logcatd selfdrive/mapd/default_speeds_by_region.json system/proclogd/proclogd @@ -53,16 +53,11 @@ selfdrive/car/tests/cars_dump system/camerad/camerad system/camerad/test/ae_gray_test selfdrive/modeld/_modeld -selfdrive/modeld/_navmodeld selfdrive/modeld/_dmonitoringmodeld /src/ -one notebooks -xx -yy hyperthneed -panda_jungle provisioning .coverage* diff --git a/.python-version b/.python-version new file mode 100644 index 0000000000..0c7d5f5f5d --- /dev/null +++ b/.python-version @@ -0,0 +1 @@ +3.11.4 diff --git a/Jenkinsfile b/Jenkinsfile index d716510bfe..321237716b 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -40,6 +40,7 @@ if [ -f /TICI ]; then rm -rf /tmp/tmp* rm -rf ~/.commacache rm -rf /dev/shm/* + rm -rf /dev/tmp/tmp* if ! systemctl is-active --quiet systemd-resolved; then echo "restarting resolved" @@ -182,7 +183,7 @@ node { deviceStage("onroad", "tici-needs-can", [], [ // TODO: ideally, this test runs in master-ci, but it takes 5+m to build it //["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR $SOURCE_DIR/scripts/retry.sh ./build_devel.sh"], - ["build openpilot", "cd selfdrive/manager && ./build.py"], + ["build openpilot", "cd system/manager && ./build.py"], ["check dirty", "release/check-dirty.sh"], ["onroad tests", "pytest selfdrive/test/test_onroad.py -s"], ["time to onroad", "pytest selfdrive/test/test_time_to_onroad.py"], @@ -190,53 +191,54 @@ node { }, 'HW + Unit Tests': { deviceStage("tici-hardware", "tici-common", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], - ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"], + ["build", "cd system/manager && ./build.py"], + ["test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"], ["test power draw", "pytest -s system/hardware/tici/tests/test_power_draw.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"], + ["test pigeond", "pytest system/ubloxd/tests/test_pigeond.py"], + ["test manager", "pytest system/manager/test/test_manager.py"], ]) }, 'loopback': { deviceStage("loopback", "tici-loopback", ["UNSAFE=1"], [ - ["build openpilot", "cd selfdrive/manager && ./build.py"], - ["test boardd loopback", "pytest selfdrive/boardd/tests/test_boardd_loopback.py"], + ["build openpilot", "cd system/manager && ./build.py"], + ["test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"], ]) }, 'camerad': { deviceStage("AR0231", "tici-ar0231", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], + ["build", "cd system/manager && ./build.py"], ["test camerad", "pytest system/camerad/test/test_camerad.py"], ["test exposure", "pytest system/camerad/test/test_exposure.py"], ]) deviceStage("OX03C10", "tici-ox03c10", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], + ["build", "cd system/manager && ./build.py"], ["test camerad", "pytest system/camerad/test/test_camerad.py"], ["test exposure", "pytest system/camerad/test/test_exposure.py"], ]) }, 'sensord': { deviceStage("LSM + MMC", "tici-lsmc", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], + ["build", "cd system/manager && ./build.py"], ["test sensord", "pytest system/sensord/tests/test_sensord.py"], ]) deviceStage("BMX + LSM", "tici-bmx-lsm", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], + ["build", "cd system/manager && ./build.py"], ["test sensord", "pytest system/sensord/tests/test_sensord.py"], ]) }, 'replay': { deviceStage("model-replay", "tici-replay", ["UNSAFE=1"], [ - ["build", "cd selfdrive/manager && ./build.py"], + ["build", "cd system/manager && ./build.py"], ["model replay", "selfdrive/test/process_replay/model_replay.py"], ]) }, 'tizi': { deviceStage("tizi", "tizi", ["UNSAFE=1"], [ - ["build openpilot", "cd selfdrive/manager && ./build.py"], - ["test boardd loopback", "SINGLE_PANDA=1 pytest selfdrive/boardd/tests/test_boardd_loopback.py"], - ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"], + ["build openpilot", "cd system/manager && ./build.py"], + ["test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"], + ["test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"], + ["test pandad", "pytest selfdrive/pandad/tests/test_pandad.py"], ["test amp", "pytest system/hardware/tici/tests/test_amplifier.py"], ["test hw", "pytest system/hardware/tici/tests/test_hardware.py"], ["test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py"], @@ -249,4 +251,4 @@ node { currentBuild.result = 'FAILED' throw e } -} \ No newline at end of file +} diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 7dafa9443b..0000000000 --- a/LICENSE +++ /dev/null @@ -1,7 +0,0 @@ -Copyright (c) 2018, Comma.ai, Inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/RELEASES.md b/RELEASES.md index 0a866008df..ea6524fa6f 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,16 @@ +Version 0.9.7 (2024-06-13) +======================== +* New driving model + * Inputs the past curvature for smoother and more accurate lateral control + * Simplified neural network architecture in the model's last layers + * Minor fixes to desire augmentation and weight decay +* New driver monitoring model + * Improved end-to-end bit for phone detection +* Adjust driving personality with the follow distance button +* Support for hybrid variants of supported Ford models +* Fingerprinting without the OBD-II port on all cars +* Improved fuzzy fingerprinting for Ford and Volkswagen + Version 0.9.6 (2024-02-27) ======================== * New driving model @@ -6,6 +19,9 @@ Version 0.9.6 (2024-02-27) * Directly outputs curvature for lateral control * New driver monitoring model * Trained on larger dataset +* Model path UI + * Shows where driving model wants to be + * Shows what model is seeing more clearly, but more jittery * AGNOS 9 * comma body streaming and controls over WebRTC * Improved fuzzy fingerprinting for many makes and models @@ -623,7 +639,7 @@ Version 0.5.13 (2019-05-31) * Reduce CPU utilization by 20% and improve stability * Temporarily remove mapd functionalities to improve stability * Add openpilot record-only mode for unsupported cars - * Synchronize controlsd to boardd to reduce latency + * Synchronize controlsd to pandad to reduce latency * Remove panda support for Subaru giraffe Version 0.5.12 (2019-05-16) @@ -959,7 +975,7 @@ Version 0.2.8 (2017-02-27) Version 0.2.7 (2017-02-08) =========================== * Better performance and pictures at night - * Fix ptr alignment issue in boardd + * Fix ptr alignment issue in pandad * Fix brake error light, fix crash if too cold Version 0.2.6 (2017-01-31) @@ -991,7 +1007,7 @@ Version 0.2.2 (2017-01-10) Version 0.2.1 (2016-12-14) =========================== * Performance improvements, removal of more numpy - * Fix boardd process priority + * Fix pandad process priority * Make counter timer reset on use of steering wheel Version 0.2 (2016-12-12) diff --git a/SConstruct b/SConstruct index b04e3903ed..944d650d74 100644 --- a/SConstruct +++ b/SConstruct @@ -67,7 +67,7 @@ AddOption('--pc-thneed', AddOption('--minimal', action='store_false', dest='extras', - default=os.path.islink(Dir('#rednose/').abspath), # minimal by default on release branch (where rednose is not a link) + default=os.path.exists(File('#.lfsconfig').abspath), # minimal by default on release branch (where there's no LFS) help='the minimum build to run openpilot. no tests, tools, etc.') ## Architecture name breakdown (arch) @@ -96,8 +96,6 @@ lenv = { rpath = lenv["LD_LIBRARY_PATH"].copy() if arch == "larch64": - lenv["LD_LIBRARY_PATH"] += ['/data/data/com.termux/files/usr/lib'] - cpppath = [ "#third_party/opencl/include", ] @@ -195,6 +193,7 @@ env = Environment( "-Wno-c99-designator", "-Wno-reorder-init-list", "-Wno-error=unused-but-set-variable", + "-Wno-vla-cxx-extension", ] + cflags + ccflags, CPPPATH=cpppath + [ @@ -210,6 +209,7 @@ env = Environment( "#third_party/qrcode", "#third_party", "#cereal", + "#msgq", "#opendbc/can", "#third_party/maplibre-native-qt/include", f"#third_party/maplibre-native-qt/{arch}/include" @@ -224,10 +224,9 @@ env = Environment( CFLAGS=["-std=gnu11"] + cflags, CXXFLAGS=["-std=c++1z"] + cxxflags, LIBPATH=libpath + [ - "#cereal", + "#msgq_repo", "#third_party", - "#opendbc/can", - "#selfdrive/boardd", + "#selfdrive/pandad", "#common", "#rednose/helpers", ], @@ -357,15 +356,13 @@ gpucommon = [_gpucommon] Export('common', 'gpucommon') -# Build cereal and messaging +# Build messaging (cereal + msgq + socketmaster + their dependencies) +SConscript(['msgq_repo/SConscript']) SConscript(['cereal/SConscript']) +Import('socketmaster', 'msgq') +messaging = [socketmaster, msgq, 'zmq', 'capnp', 'kj',] +Export('messaging') -cereal = [File('#cereal/libcereal.a')] -messaging = [File('#cereal/libmessaging.a')] -visionipc = [File('#cereal/libvisionipc.a')] -messaging_python = [File('#cereal/messaging/messaging_pyx.so')] - -Export('cereal', 'messaging', 'messaging_python', 'visionipc') # Build other submodules SConscript([ @@ -385,25 +382,22 @@ SConscript([ ]) if arch != "Darwin": SConscript([ - 'system/camerad/SConscript', 'system/sensord/SConscript', 'system/logcatd/SConscript', ]) +if arch == "larch64": + SConscript(['system/camerad/SConscript']) + # Build openpilot SConscript(['third_party/SConscript']) -SConscript(['selfdrive/boardd/SConscript']) -SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript']) -SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript']) -SConscript(['selfdrive/locationd/SConscript']) -SConscript(['selfdrive/navd/SConscript']) -SConscript(['selfdrive/modeld/SConscript']) -SConscript(['selfdrive/ui/SConscript']) +SConscript(['selfdrive/SConscript']) -if arch in ['x86_64', 'aarch64', 'Darwin'] and Dir('#tools/cabana/').exists() and GetOption('extras'): +if Dir('#tools/cabana/').exists() and GetOption('extras'): SConscript(['tools/replay/SConscript']) - SConscript(['tools/cabana/SConscript']) + if arch != "larch64": + SConscript(['tools/cabana/SConscript']) external_sconscript = GetOption('external_sconscript') if external_sconscript: diff --git a/SECURITY.md b/SECURITY.md new file mode 100644 index 0000000000..8b66082bf2 --- /dev/null +++ b/SECURITY.md @@ -0,0 +1,5 @@ +# Security Policy + +## Reporting a Vulnerability + +Suspected vulnerabilities can be reported to both `adeeb@comma.ai` and `security@comma.ai`. diff --git a/body/README.md b/body/README.md new file mode 100644 index 0000000000..641aaa41ee --- /dev/null +++ b/body/README.md @@ -0,0 +1,18 @@ +# comma body + +This the firmware for the comma body robotics dev kit. + + +Learn more at [commabody.com](https://commabody.com/). + +## building + +Compile: `scons` + +Flash bootstub and app: `board/recover.sh` # STM flasher should be connected to debug port, needs openocd + +Flash app through CAN bus with panda: + +`board/flash_base.sh` # base motherboard + +`board/flash_knee.sh` # knee motherboard diff --git a/body/SConstruct b/body/SConstruct new file mode 100644 index 0000000000..4c48fe5923 --- /dev/null +++ b/body/SConstruct @@ -0,0 +1 @@ +SConscript('board/SConscript') diff --git a/body/board/canloader.py b/body/board/canloader.py new file mode 100755 index 0000000000..624e6bf7a4 --- /dev/null +++ b/body/board/canloader.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +import os +import time +import argparse +import _thread +from panda import Panda, MCU_TYPE_F4 # pylint: disable=import-error +from panda.tests.pedal.canhandle import CanHandle # pylint: disable=import-error + + +def heartbeat_thread(p): + while True: + try: + p.send_heartbeat() + time.sleep(0.5) + except Exception: + continue + +def flush_panda(): + while(1): + if len(p.can_recv()) == 0: + break + +def flasher(p, addr, file): + p.can_send(addr, b"\xce\xfa\xad\xde\x1e\x0b\xb0\x0a", 0) + time.sleep(0.1) + print("flashing", file) + flush_panda() + code = open(file, "rb").read() + retries = 3 # How many times to retry on timeout error + while(retries+1>0): + try: + Panda.flash_static(CanHandle(p, 0), code, MCU_TYPE_F4) + except TimeoutError: + print("Timeout, trying again...") + retries -= 1 + else: + print("Successfully flashed") + break + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Flash body over can') + parser.add_argument("board", type=str, nargs='?', help="choose base or knee") + parser.add_argument("fn", type=str, nargs='?', help="flash file") + args = parser.parse_args() + + assert args.board in ["base", "knee"] + assert os.path.isfile(args.fn) + + addr = 0x250 if args.board == "base" else 0x350 + + p = Panda() + _thread.start_new_thread(heartbeat_thread, (p,)) + p.set_safety_mode(Panda.SAFETY_BODY) + + print("Flashing motherboard") + flasher(p, addr, args.fn) + + print("CAN flashing done") diff --git a/body/board/flash_base.sh b/body/board/flash_base.sh new file mode 100755 index 0000000000..b06818a5cb --- /dev/null +++ b/body/board/flash_base.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env sh +set -e + +scons -u + +./canloader.py base obj/body.bin.signed diff --git a/body/board/flash_knee.sh b/body/board/flash_knee.sh new file mode 100755 index 0000000000..c6a2c05ddc --- /dev/null +++ b/body/board/flash_knee.sh @@ -0,0 +1,6 @@ +#!/usr/bin/env sh +set -e + +scons -u + +./canloader.py knee obj/body.bin.signed diff --git a/body/board/recover.sh b/body/board/recover.sh new file mode 100755 index 0000000000..0a521e1a20 --- /dev/null +++ b/body/board/recover.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env sh +set -e + +FLASH_UTIL="openocd" + +scons -u + +$FLASH_UTIL -f interface/stlink.cfg -c "set CPUTAPID 0" -f target/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase obj/body.bin.signed 0x08004000" -c "flash write_image erase obj/bootstub.body.bin 0x08000000" -c "reset run" -c "shutdown" diff --git a/body/pyproject.toml b/body/pyproject.toml new file mode 100644 index 0000000000..bac2d7f081 --- /dev/null +++ b/body/pyproject.toml @@ -0,0 +1,7 @@ +# 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"] +line-length = 160 +target-version="py311" +flake8-implicit-str-concat.allow-multiline=false diff --git a/body/requirements.txt b/body/requirements.txt new file mode 100644 index 0000000000..0325804295 --- /dev/null +++ b/body/requirements.txt @@ -0,0 +1,5 @@ +cffi +scons +ruff +pre-commit +pycryptodome==3.9.8 diff --git a/cereal/README.md b/cereal/README.md new file mode 100644 index 0000000000..e3326aab0e --- /dev/null +++ b/cereal/README.md @@ -0,0 +1,60 @@ +# What is cereal? [![cereal tests](https://github.com/commaai/cereal/workflows/tests/badge.svg?event=push)](https://github.com/commaai/cereal/actions) [![codecov](https://codecov.io/gh/commaai/cereal/branch/master/graph/badge.svg)](https://codecov.io/gh/commaai/cereal) + +cereal is both a messaging spec for robotics systems as well as generic high performance IPC pub sub messaging with a single publisher and multiple subscribers. + +Imagine this use case: +* A sensor process reads gyro measurements directly from an IMU and publishes a `sensorEvents` packet +* A calibration process subscribes to the `sensorEvents` packet to use the IMU +* A localization process subscribes to the `sensorEvents` packet to use the IMU also + + +## Messaging Spec + +You'll find the message types in [log.capnp](log.capnp). It uses [Cap'n proto](https://capnproto.org/capnp-tool.html) and defines one struct called `Event`. + +All `Events` have a `logMonoTime` and a `valid`. Then a big union defines the packet type. + +### Best Practices + +- **All fields must describe quantities in SI units**, unless otherwise specified in the field name. +- In the context of the message they are in, field names should be completely unambiguous. +- All values should be easy to plot and be human-readable with minimal parsing. + +### Maintaining backwards-compatibility + +When making changes to the messaging spec you want to maintain backwards-compatibility, such that old logs can +be parsed with a new version of cereal. Adding structs and adding members to structs is generally safe, most other +things are not. Read more details [here](https://capnproto.org/language.html). + +### Custom forks + +Forks of [openpilot](https://github.com/commaai/openpilot) might want to add things to the messaging +spec, however this could conflict with future changes made in mainline cereal/openpilot. Rebasing against mainline openpilot +then means breaking backwards-compatibility with all old logs of your fork. So we added reserved events in +[custom.capnp](custom.capnp) that we will leave empty in mainline cereal/openpilot. **If you only modify those, you can ensure your +fork will remain backwards-compatible with all versions of mainline cereal/openpilot and your fork.** + +## Pub Sub Backends + +cereal supports two backends, one based on [zmq](https://zeromq.org/) and another called [msgq](messaging/msgq.cc), a custom pub sub based on shared memory that doesn't require the bytes to pass through the kernel. + +Example +--- +```python +import cereal.messaging as messaging + +# in subscriber +sm = messaging.SubMaster(['sensorEvents']) +while 1: + sm.update() + print(sm['sensorEvents']) + +``` + +```python +# in publisher +pm = messaging.PubMaster(['sensorEvents']) +dat = messaging.new_message('sensorEvents', size=1) +dat.sensorEvents[0] = {"gyro": {"v": [0.1, -0.1, 0.1]}} +pm.send('sensorEvents', dat) +``` diff --git a/cereal/SConscript b/cereal/SConscript index a5d9d6bea1..be5f161dea 100644 --- a/cereal/SConscript +++ b/cereal/SConscript @@ -1,13 +1,12 @@ -Import('env', 'envCython', 'arch', 'common') +Import('env', 'envCython', 'arch', 'common', 'msgq') import shutil cereal_dir = Dir('.') gen_dir = Dir('gen') -messaging_dir = Dir('messaging') +other_dir = Dir('#msgq') # Build cereal - schema_files = ['log.capnp', 'car.capnp', 'legacy.capnp', 'custom.capnp'] env.Command(["gen/c/include/c++.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS") env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s in schema_files], @@ -17,60 +16,16 @@ env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s # TODO: remove non shared cereal and messaging cereal_objects = env.SharedObject([f'gen/cpp/{s}.c++' for s in schema_files]) -env.Library('cereal', cereal_objects) +cereal = env.Library('cereal', cereal_objects) env.SharedLibrary('cereal_shared', cereal_objects) # Build messaging services_h = env.Command(['services.h'], ['services.py'], 'python3 ' + cereal_dir.path + '/services.py > $TARGET') +env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[msgq, 'zmq', common]) -messaging_objects = env.SharedObject([ - 'messaging/messaging.cc', - 'messaging/event.cc', - 'messaging/impl_zmq.cc', - 'messaging/impl_msgq.cc', - 'messaging/impl_fake.cc', - 'messaging/msgq.cc', - 'messaging/socketmaster.cc', -]) - -messaging_lib = env.Library('messaging', messaging_objects) -Depends('messaging/impl_zmq.cc', services_h) - -env.Program('messaging/bridge', ['messaging/bridge.cc'], LIBS=[messaging_lib, 'zmq', common]) -Depends('messaging/bridge.cc', services_h) - -envCython.Program('messaging/messaging_pyx.so', 'messaging/messaging_pyx.pyx', LIBS=envCython["LIBS"]+[messaging_lib, "zmq", common]) - - -# Build Vision IPC -vipc_sources = [ - 'visionipc/ipc.cc', - 'visionipc/visionipc_server.cc', - 'visionipc/visionipc_client.cc', - 'visionipc/visionbuf.cc', -] - -if arch == "larch64": - vipc_sources += ['visionipc/visionbuf_ion.cc'] -else: - vipc_sources += ['visionipc/visionbuf_cl.cc'] - -vipc_objects = env.SharedObject(vipc_sources) -vipc = env.Library('visionipc', vipc_objects) - - -vipc_frameworks = [] -vipc_libs = envCython["LIBS"] + [vipc, messaging_lib, common, "zmq"] -if arch == "Darwin": - vipc_frameworks.append('OpenCL') -else: - vipc_libs.append('OpenCL') -envCython.Program('visionipc/visionipc_pyx.so', 'visionipc/visionipc_pyx.pyx', - LIBS=vipc_libs, FRAMEWORKS=vipc_frameworks) -if GetOption('extras'): - env.Program('messaging/test_runner', ['messaging/test_runner.cc', 'messaging/msgq_tests.cc'], LIBS=[messaging_lib, common]) +socketmaster = env.SharedObject(['messaging/socketmaster.cc']) +socketmaster = env.Library('socketmaster', socketmaster) - env.Program('visionipc/test_runner', ['visionipc/test_runner.cc', 'visionipc/visionipc_tests.cc'], - LIBS=['pthread'] + vipc_libs, FRAMEWORKS=vipc_frameworks) +Export('cereal', 'socketmaster') diff --git a/cereal/car.capnp b/cereal/car.capnp index 3d7d737dcd..8d76f0ead8 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -96,7 +96,6 @@ struct CarEvent @0x9b1657f34caf3ad3 { fanMalfunction @91; cameraMalfunction @92; cameraFrameRate @110; - gpsMalfunction @94; processNotRunning @95; dashcamMode @96; controlsInitializing @98; @@ -116,6 +115,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { locationdPermanentError @118; paramsdTemporaryError @50; paramsdPermanentError @119; + actuatorsApiUnavailable @120; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; @@ -142,6 +142,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { noTargetDEPRECATED @25; brakeUnavailableDEPRECATED @2; plannerErrorDEPRECATED @32; + gpsMalfunctionDEPRECATED @94; } } @@ -154,6 +155,7 @@ struct CarState { # CAN health canValid @26 :Bool; # invalid counter/checksums canTimeout @40 :Bool; # CAN bus dropped out + canErrorCounter @48 :UInt32; # car speed vEgo @1 :Float32; # best estimate of speed @@ -219,6 +221,9 @@ struct CarState { fuelGauge @41 :Float32; # battery or fuel tank level from 0.0 to 1.0 charging @43 :Bool; + # process meta + cumLagMs @50 :Float32; + struct WheelSpeeds { # optional wheel speeds fl @0 :Float32; @@ -276,6 +281,7 @@ struct CarState { brakeLightsDEPRECATED @19 :Bool; steeringRateLimitedDEPRECATED @29 :Bool; canMonoTimesDEPRECATED @12: List(UInt64); + canRcvTimeoutDEPRECATED @49 :Bool; } # ******* radar state @ 20hz ******* @@ -323,14 +329,12 @@ struct CarControl { # Actuator commands as computed by controlsd actuators @6 :Actuators; + # moved to CarOutput + actuatorsOutputDEPRECATED @10 :Actuators; + leftBlinker @15: Bool; rightBlinker @16: Bool; - # Any car specific rate limits or quirks applied by - # the CarController are reflected in actuatorsOutput - # and matches what is sent to the car - actuatorsOutput @10 :Actuators; - orientationNED @13 :List(Float32); angularVelocity @14 :List(Float32); @@ -380,6 +384,7 @@ struct CarControl { leftLaneVisible @7: Bool; rightLaneDepart @8: Bool; leftLaneDepart @9: Bool; + leadDistanceBars @10: Int8; # 1-3: 1 is closest, 3 is farthest. some ports may utilize 2-4 bars instead enum VisualAlert { # these are the choices from the Honda @@ -418,6 +423,13 @@ struct CarControl { pitchDEPRECATED @9 :Float32; } +struct CarOutput { + # Any car specific rate limits or quirks applied by + # the CarController are reflected in actuatorsOutput + # and matches what is sent to the car + actuatorsOutput @0 :CarControl.Actuators; +} + # ****** car param ****** struct CarParams { @@ -427,7 +439,6 @@ struct CarParams { notCar @66 :Bool; # flag for non-car robotics platforms - enableGasInterceptor @2 :Bool; pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state? enableDsu @5 :Bool; # driving support unit enableBsm @56 :Bool; # blind spot monitoring @@ -597,6 +608,7 @@ struct CarParams { hyundaiCanfd @28; volkswagenMqbEvo @29; chryslerCusw @30; + psa @31; } enum SteerControlType { @@ -671,6 +683,7 @@ struct CarParams { gateway @1; # Integration at vehicle's CAN gateway } + enableGasInterceptorDEPRECATED @2 :Bool; enableCameraDEPRECATED @4 :Bool; enableApgsDEPRECATED @6 :Bool; steerRateCostDEPRECATED @33 :Float32; diff --git a/cereal/log.capnp b/cereal/log.capnp index 9f0ca64f8b..36ca692dcc 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -29,6 +29,7 @@ struct InitData { osVersion @18 :Text; dongleId @2 :Text; + bootlogId @22 :Text; deviceType @3 :DeviceType; version @4 :Text; @@ -57,6 +58,7 @@ struct InitData { tici @4; pc @5; tizi @6; + mici @7; } struct PandaInfo { @@ -197,6 +199,13 @@ struct Thumbnail { frameId @0 :UInt32; timestampEof @1 :UInt64; thumbnail @2 :Data; + encoding @3 :Encoding; + + enum Encoding { + unknown @0; + jpeg @1; + keyframe @2; + } } struct GPSNMEAData { @@ -250,7 +259,7 @@ struct SensorEventData { # android struct GpsLocation struct GpsLocationData { - # Contains GpsLocationFlags bits. + # Contains module-specific flags. flags @0 :UInt16; # Represents latitude in degrees. @@ -268,8 +277,8 @@ struct GpsLocationData { # Represents heading in degrees. bearingDeg @5 :Float32; - # Represents expected accuracy in meters. (presumably 1 sigma?) - accuracy @6 :Float32; + # Represents expected horizontal accuracy in meters. + horizontalAccuracy @6 :Float32; unixTimestampMillis @7 :Int64; @@ -287,6 +296,8 @@ struct GpsLocationData { # Represents velocity accuracy in m/s. (presumably 1 sigma?) speedAccuracy @12 :Float32; + hasFix @13 :Bool; + enum SensorSource { android @0; iOS @1; @@ -297,6 +308,7 @@ struct GpsLocationData { ublox @6; trimble @7; qcomdiag @8; + unicore @9; } } @@ -331,6 +343,8 @@ struct CanData { } struct DeviceState @0xa4d8b5af2aa492eb { + deviceType @45 :InitData.DeviceType; + networkType @22 :NetworkType; networkInfo @31 :NetworkInfo; networkStrength @24 :NetworkStrength; @@ -436,7 +450,6 @@ struct PandaState @0xa7649e2575e4591e { ignitionLine @2 :Bool; rxBufferOverflow @7 :UInt32; txBufferOverflow @8 :UInt32; - gmlanSendErrs @9 :UInt32; pandaType @10 :PandaType; ignitionCan @13 :Bool; faultStatus @15 :FaultStatus; @@ -484,7 +497,7 @@ struct PandaState @0xa7649e2575e4591e { interruptRateCan2 @3; interruptRateCan3 @4; interruptRateTach @5; - interruptRateGmlan @6; + interruptRateGmlanDEPRECATED @6; interruptRateInterrupts @7; interruptRateSpiDma @8; interruptRateSpiCs @9; @@ -570,6 +583,7 @@ struct PandaState @0xa7649e2575e4591e { gasInterceptorDetectedDEPRECATED @4 :Bool; startedSignalDetectedDEPRECATED @5 :Bool; hasGpsDEPRECATED @6 :Bool; + gmlanSendErrsDEPRECATED @9 :UInt32; fanSpeedRpmDEPRECATED @11 :UInt16; usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerModeDEPRECATED; safetyParamDEPRECATED @20 :Int16; @@ -681,6 +695,7 @@ struct ControlsState @0x97ff69c53601abf1 { active @36 :Bool; experimentalMode @64 :Bool; + personality @66 :LongitudinalPersonality; longControlState @30 :Car.CarControl.Actuators.LongControlState; vPid @2 :Float32; @@ -706,7 +721,6 @@ struct ControlsState @0x97ff69c53601abf1 { engageable @41 :Bool; # can OP be engaged? cumLagMs @15 :Float32; - canErrorCounter @57 :UInt32; lateralControlState :union { indiState @52 :LateralINDIState; @@ -851,6 +865,7 @@ struct ControlsState @0x97ff69c53601abf1 { steeringAngleDesiredDegDEPRECATED @29 :Float32; canMonoTimesDEPRECATED @21 :List(UInt64); desiredCurvatureRateDEPRECATED @62 :Float32; + canErrorCounterDEPRECATED @57 :UInt32; } # All SI units and in device frame @@ -898,8 +913,8 @@ struct ModelDataV2 { # Model perceived motion temporalPose @21 :Pose; - navEnabled @22 :Bool; - locationMonoTime @24 :UInt64; + navEnabledDEPRECATED @22 :Bool; + locationMonoTimeDEPRECATED @24 :UInt64; # e2e lateral planner lateralPlannerSolutionDEPRECATED @25: LateralPlannerSolution; @@ -1047,7 +1062,6 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { jerks @34 :List(Float32); solverExecutionTime @35 :Float32; - personality @36 :LongitudinalPersonality; enum LongitudinalPlanSource { cruise @0; @@ -1085,6 +1099,7 @@ struct LongitudinalPlan @0xe00b5b3eba12876c { eventsDEPRECATED @13 :List(Car.CarEvent); gpsTrajectoryDEPRECATED @12 :GpsTrajectory; gpsPlannerActiveDEPRECATED @19 :Bool; + personalityDEPRECATED @36 :LongitudinalPersonality; struct GpsTrajectory { x @0 :List(Float32); @@ -1125,29 +1140,6 @@ struct LateralPlan @0xe1e9318e2ae8b51e { u @1 :List(Float32); } - enum Desire { - none @0; - turnLeft @1; - turnRight @2; - laneChangeLeft @3; - laneChangeRight @4; - keepLeft @5; - keepRight @6; - } - - enum LaneChangeState { - off @0; - preLaneChange @1; - laneChangeStarting @2; - laneChangeFinishing @3; - } - - enum LaneChangeDirection { - none @0; - left @1; - right @2; - } - # deprecated curvatureDEPRECATED @22 :Float32; curvatureRateDEPRECATED @23 :Float32; @@ -2250,6 +2242,7 @@ struct Event { liveCalibration @19 :LiveCalibrationData; carState @22 :Car.CarState; carControl @23 :Car.CarControl; + carOutput @127 :Car.CarOutput; longitudinalPlan @24 :LongitudinalPlan; uiPlan @106 :UiPlan; ubloxGnss @34 :UbloxGnss; @@ -2268,7 +2261,6 @@ struct Event { liveLocationKalman @72 :LiveLocationKalman; modelV2 @75 :ModelDataV2; driverStateV2 @92 :DriverStateV2; - navModel @104 :NavModelData; # camera stuff, each camera state has a matching encode idx roadCameraState @2 :FrameData; @@ -2372,5 +2364,6 @@ struct Event { driverStateDEPRECATED @59 :DriverStateDEPRECATED; sensorEventsDEPRECATED @11 :List(SensorEventData); lateralPlanDEPRECATED @64 :LateralPlan; + navModelDEPRECATED @104 :NavModelData; } } diff --git a/cereal/maptile.capnp b/cereal/maptile.capnp new file mode 100644 index 0000000000..c8a23a1822 --- /dev/null +++ b/cereal/maptile.capnp @@ -0,0 +1,49 @@ +using Cxx = import "./include/c++.capnp"; +$Cxx.namespace("cereal"); + +@0xa086df597ef5d7a0; + +# Geometry +struct Point { + x @0: Float64; + y @1: Float64; + z @2: Float64; +} + +struct PolyLine { + points @0: List(Point); +} + +# Map features +struct Lane { + id @0 :Text; + + leftBoundary @1 :LaneBoundary; + rightBoundary @2 :LaneBoundary; + + leftAdjacentId @3 :Text; + rightAdjacentId @4 :Text; + + inboundIds @5 :List(Text); + outboundIds @6 :List(Text); + + struct LaneBoundary { + polyLine @0 :PolyLine; + startHeading @1 :Float32; # WRT north + } +} + +# Map tiles +struct TileSummary { + version @0 :Text; + updatedAt @1 :UInt64; # Millis since epoch + + level @2 :UInt8; + x @3 :UInt16; + y @4 :UInt16; +} + +struct MapTile { + summary @0 :TileSummary; + lanes @1 :List(Lane); +} diff --git a/cereal/messaging/__init__.py b/cereal/messaging/__init__.py index ddc0866849..4ba55cf7b9 100644 --- a/cereal/messaging/__init__.py +++ b/cereal/messaging/__init__.py @@ -1,7 +1,8 @@ # must be built with scons -from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \ +from msgq.ipc_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \ set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event -from .messaging_pyx import MultiplePublishersError, MessagingError +from msgq.ipc_pyx import MultiplePublishersError, IpcError +from msgq import fake_event_handle, pub_sock, sub_sock, drain_sock_raw, context import os import capnp @@ -13,27 +14,8 @@ from collections import deque from cereal import log from cereal.services import SERVICE_LIST -assert MultiplePublishersError -assert MessagingError -assert toggle_fake_events -assert set_fake_prefix -assert get_fake_prefix -assert delete_fake_prefix -assert wait_for_one_event - NO_TRAVERSAL_LIMIT = 2**64-1 -context = Context() - - -def fake_event_handle(endpoint: str, identifier: Optional[str] = None, override: bool = True, enable: bool = False) -> SocketEventHandle: - identifier = identifier or get_fake_prefix() - handle = SocketEventHandle(endpoint, identifier, override) - if override: - handle.enabled = enable - - return handle - def log_from_bytes(dat: bytes) -> capnp.lib.capnp._DynamicStructReader: with log.Event.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg: @@ -55,58 +37,10 @@ def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> return dat -def pub_sock(endpoint: str) -> PubSocket: - sock = PubSocket() - sock.connect(context, endpoint) - return sock - - -def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1", - conflate: bool = False, timeout: Optional[int] = None) -> SubSocket: - sock = SubSocket() - sock.connect(context, endpoint, addr.encode('utf8'), conflate) - - if timeout is not None: - sock.setTimeout(timeout) - - if poller is not None: - poller.registerSocket(sock) - return sock - - -def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]: - """Receive all message currently available on the queue""" - ret: List[bytes] = [] - while 1: - if wait_for_one and len(ret) == 0: - dat = sock.receive() - else: - dat = sock.receive(non_blocking=True) - - if dat is None: - break - - ret.append(dat) - - return ret - - def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]: """Receive all message currently available on the queue""" - ret: List[capnp.lib.capnp._DynamicStructReader] = [] - while 1: - if wait_for_one and len(ret) == 0: - dat = sock.receive() - else: - dat = sock.receive(non_blocking=True) - - if dat is None: # Timeout hit - break - - dat = log_from_bytes(dat) - ret.append(dat) - - return ret + msgs = drain_sock_raw(sock, wait_for_one=wait_for_one) + return [log_from_bytes(m) for m in msgs] # TODO: print when we drop packets? @@ -180,9 +114,8 @@ class SubMaster: self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq self.ignore_alive = [] if ignore_alive is None else ignore_alive self.ignore_valid = [] if ignore_valid is None else ignore_valid - if bool(int(os.getenv("SIMULATION", "0"))): - self.ignore_alive = services - self.ignore_average_freq = services + + self.simulation = bool(int(os.getenv("SIMULATION", "0"))) # if freq and poll aren't specified, assume the max to be conservative assert frequency is None or poll is None, "Do not specify 'frequency' - frequency of the polled service will be used." @@ -253,7 +186,7 @@ class SubMaster: self.valid[s] = msg.valid for s in self.data: - if SERVICE_LIST[s].frequency > 1e-5: + if SERVICE_LIST[s].frequency > 1e-5 and not self.simulation: # alive if delay is within 10x the expected frequency self.alive[s] = (cur_time - self.recv_time[s]) < (10. / SERVICE_LIST[s].frequency) @@ -273,7 +206,10 @@ class SubMaster: self.freq_ok[s] = avg_freq_ok or recent_freq_ok else: self.freq_ok[s] = True - self.alive[s] = True + if self.simulation: + self.alive[s] = self.seen[s] # alive is defined as seen when simulation flag set + else: + self.alive[s] = True def all_alive(self, service_list: Optional[List[str]] = None) -> bool: if service_list is None: diff --git a/cereal/messaging/bridge.cc b/cereal/messaging/bridge.cc index 4a5390caff..8619c1e226 100644 --- a/cereal/messaging/bridge.cc +++ b/cereal/messaging/bridge.cc @@ -8,8 +8,8 @@ typedef void (*sighandler_t)(int sig); #include "cereal/services.h" -#include "cereal/messaging/impl_msgq.h" -#include "cereal/messaging/impl_zmq.h" +#include "msgq/impl_msgq.h" +#include "msgq/impl_zmq.h" std::atomic do_exit = false; static void set_do_exit(int sig) { diff --git a/cereal/messaging/messaging.h b/cereal/messaging/messaging.h index 91106514af..f3850130e6 100644 --- a/cereal/messaging/messaging.h +++ b/cereal/messaging/messaging.h @@ -10,6 +10,7 @@ #include #include "cereal/gen/cpp/log.capnp.h" +#include "msgq/ipc.h" #ifdef __APPLE__ #define CLOCK_BOOTTIME CLOCK_MONOTONIC @@ -17,57 +18,6 @@ #define MSG_MULTIPLE_PUBLISHERS 100 -bool messaging_use_zmq(); - -class Context { -public: - virtual void * getRawContext() = 0; - static Context * create(); - virtual ~Context(){} -}; - -class Message { -public: - virtual void init(size_t size) = 0; - virtual void init(char * data, size_t size) = 0; - virtual void close() = 0; - virtual size_t getSize() = 0; - virtual char * getData() = 0; - virtual ~Message(){} -}; - - -class SubSocket { -public: - virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) = 0; - virtual void setTimeout(int timeout) = 0; - virtual Message *receive(bool non_blocking=false) = 0; - virtual void * getRawSocket() = 0; - static SubSocket * create(); - static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true); - virtual ~SubSocket(){} -}; - -class PubSocket { -public: - virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0; - virtual int sendMessage(Message *message) = 0; - virtual int send(char *data, size_t size) = 0; - virtual bool all_readers_updated() = 0; - static PubSocket * create(); - static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true); - static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true); - virtual ~PubSocket(){} -}; - -class Poller { -public: - virtual void registerSocket(SubSocket *socket) = 0; - virtual std::vector poll(int timeout) = 0; - static Poller * create(); - static Poller * create(std::vector sockets); - virtual ~Poller(){} -}; class SubMaster { public: diff --git a/cereal/messaging/socketmaster.cc b/cereal/messaging/socketmaster.cc index 3054b4f7db..cd697b1f51 100644 --- a/cereal/messaging/socketmaster.cc +++ b/cereal/messaging/socketmaster.cc @@ -7,6 +7,7 @@ #include "cereal/services.h" #include "cereal/messaging/messaging.h" + const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1"); static inline uint64_t nanos_since_boot() { diff --git a/cereal/services.py b/cereal/services.py index e79f2e062a..2ab28f6d52 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -1,24 +1,15 @@ #!/usr/bin/env python3 from typing import Optional -RESERVED_PORT = 8022 # sshd -STARTING_PORT = 8001 - - -def new_port(port: int): - port += STARTING_PORT - return port + 1 if port >= RESERVED_PORT else port - class Service: - def __init__(self, port: int, should_log: bool, frequency: float, decimation: Optional[int] = None): - self.port = port + def __init__(self, should_log: bool, frequency: float, decimation: Optional[int] = None): self.should_log = should_log self.frequency = frequency self.decimation = decimation -services: dict[str, tuple] = { +_services: dict[str, tuple] = { # service: (should_log, frequency, qlog decimation (optional)) # note: the "EncodeIdx" packets will still be in the log "gyroscope": (True, 104., 104), @@ -46,6 +37,7 @@ services: dict[str, tuple] = { "androidLog": (True, 0.), "carState": (True, 100., 10), "carControl": (True, 100., 10), + "carOutput": (True, 100., 10), "longitudinalPlan": (True, 20., 5), "procLog": (True, 0.5, 15), "gpsLocationExternal": (True, 10., 10), @@ -74,8 +66,6 @@ services: dict[str, tuple] = { "navInstruction": (True, 1., 10), "navRoute": (True, 0.), "navThumbnail": (True, 0.), - "navModel": (True, 2., 4.), - "mapRenderState": (True, 2., 1.), "uiPlan": (True, 20., 40.), "qRoadEncodeIdx": (False, 20.), "userFlag": (True, 0., 1), @@ -98,8 +88,8 @@ services: dict[str, tuple] = { "customReservedRawData1": (True, 0.), "customReservedRawData2": (True, 0.), } -SERVICE_LIST = {name: Service(new_port(idx), *vals) for - idx, (name, vals) in enumerate(services.items())} +SERVICE_LIST = {name: Service(*vals) for + idx, (name, vals) in enumerate(_services.items())} def build_header(): @@ -111,13 +101,13 @@ def build_header(): h += "#include \n" h += "#include \n" - h += "struct service { std::string name; int port; bool should_log; int frequency; int decimation; };\n" + h += "struct service { std::string name; bool should_log; int frequency; int decimation; };\n" h += "static std::map services = {\n" for k, v in SERVICE_LIST.items(): should_log = "true" if v.should_log else "false" decimation = -1 if v.decimation is None else v.decimation - h += ' { "%s", {"%s", %d, %s, %d, %d}},\n' % \ - (k, k, v.port, should_log, v.frequency, decimation) + h += ' { "%s", {"%s", %s, %d, %d}},\n' % \ + (k, k, should_log, v.frequency, decimation) h += "};\n" h += "#endif\n" diff --git a/cereal/visionipc/__init__.py b/cereal/visionipc/__init__.py deleted file mode 100644 index 7adfc58d14..0000000000 --- a/cereal/visionipc/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -from cereal.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name -assert VisionBuf -assert VisionIpcClient -assert VisionIpcServer -assert VisionStreamType -assert get_endpoint_name diff --git a/cereal/visionipc/ipc.h b/cereal/visionipc/ipc.h deleted file mode 100644 index 14bb61a521..0000000000 --- a/cereal/visionipc/ipc.h +++ /dev/null @@ -1,7 +0,0 @@ -#pragma once -#include - -int ipc_connect(const char* socket_path); -int ipc_bind(const char* socket_path); -int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds, - int *out_num_fds); diff --git a/codecov.yml b/codecov.yml new file mode 100644 index 0000000000..519e7d1a38 --- /dev/null +++ b/codecov.yml @@ -0,0 +1,13 @@ +comment: false +coverage: + status: + project: + default: + informational: true + patch: off + +ignore: + - "**/test_*.py" + - "selfdrive/test/**" + - "system/version.py" # codecov changes depending on if we are in a branch or not + - "tools" diff --git a/common/api/__init__.py b/common/api/__init__.py index 79875023a2..f3a7d83842 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -7,7 +7,7 @@ from openpilot.system.version import get_version API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') -class Api(): +class Api: def __init__(self, dongle_id): self.dongle_id = dongle_id with open(Paths.persist_root()+'/comma/id_rsa') as f: diff --git a/common/file_helpers.py b/common/file_helpers.py index dea298a529..29ad219c07 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -1,7 +1,6 @@ import os import tempfile import contextlib -from typing import Optional class CallbackReader: @@ -24,7 +23,7 @@ class CallbackReader: @contextlib.contextmanager -def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encoding: Optional[str] = None, newline: Optional[str] = None, +def atomic_write_in_dir(path: str, mode: str = 'w', buffering: int = -1, encoding: str = None, newline: str = None, overwrite: bool = False): """Write to a file atomically using a temporary file in the same directory as the destination file.""" dir_name = os.path.dirname(path) diff --git a/common/git.py b/common/git.py new file mode 100644 index 0000000000..4406bf96b1 --- /dev/null +++ b/common/git.py @@ -0,0 +1,42 @@ +from functools import cache +import subprocess +from openpilot.common.run import run_cmd, run_cmd_default + + +@cache +def get_commit(cwd: str = None, branch: str = "HEAD") -> str: + return run_cmd_default(["git", "rev-parse", branch], cwd=cwd) + + +@cache +def get_commit_date(cwd: str = None, commit: str = "HEAD") -> str: + return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit], cwd=cwd) + + +@cache +def get_short_branch(cwd: str = None) -> str: + return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=cwd) + + +@cache +def get_branch(cwd: str = None) -> str: + return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], cwd=cwd) + + +@cache +def get_origin(cwd: str = None) -> str: + try: + local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"], cwd=cwd) + tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"], cwd=cwd) + return run_cmd(["git", "config", "remote." + tracking_remote + ".url"], cwd=cwd) + except subprocess.CalledProcessError: # Not on a branch, fallback + return run_cmd_default(["git", "config", "--get", "remote.origin.url"], cwd=cwd) + + +@cache +def get_normalized_origin(cwd: str = None) -> str: + return get_origin(cwd) \ + .replace("git@", "", 1) \ + .replace(".git", "", 1) \ + .replace("https://", "", 1) \ + .replace(":", "/", 1) diff --git a/common/gpio.py b/common/gpio.py index 88a9479a60..66b2adf438 100644 --- a/common/gpio.py +++ b/common/gpio.py @@ -1,6 +1,5 @@ import os -from functools import lru_cache -from typing import Optional, List +from functools import cache def gpio_init(pin: int, output: bool) -> None: try: @@ -16,7 +15,7 @@ def gpio_set(pin: int, high: bool) -> None: except Exception as e: print(f"Failed to set gpio {pin} value: {e}") -def gpio_read(pin: int) -> Optional[bool]: +def gpio_read(pin: int) -> bool | None: val = None try: with open(f"/sys/class/gpio/gpio{pin}/value", 'rb') as f: @@ -36,8 +35,8 @@ def gpio_export(pin: int) -> None: except Exception: print(f"Failed to export gpio {pin}") -@lru_cache(maxsize=None) -def get_irq_action(irq: int) -> List[str]: +@cache +def get_irq_action(irq: int) -> list[str]: try: with open(f"/sys/kernel/irq/{irq}/actions") as f: actions = f.read().strip().split(',') @@ -45,7 +44,7 @@ def get_irq_action(irq: int) -> List[str]: except FileNotFoundError: return [] -def get_irqs_for_action(action: str) -> List[str]: +def get_irqs_for_action(action: str) -> list[str]: ret = [] with open("/proc/interrupts") as f: for l in f.readlines(): diff --git a/common/markdown.py b/common/markdown.py new file mode 100644 index 0000000000..f0f056d963 --- /dev/null +++ b/common/markdown.py @@ -0,0 +1,45 @@ +HTML_REPLACEMENTS = [ + (r'&', r'&'), + (r'"', r'"'), +] + +def parse_markdown(text: str, tab_length: int = 2) -> str: + lines = text.split("\n") + output: list[str] = [] + list_level = 0 + + def end_outstanding_lists(level: int, end_level: int) -> int: + while level > end_level: + level -= 1 + output.append("") + if level > 0: + output.append("") + return end_level + + for i, line in enumerate(lines): + if i + 1 < len(lines) and lines[i + 1].startswith("==="): # heading + output.append(f"

{line}

") + elif line.startswith("==="): + pass + elif line.lstrip().startswith("* "): # list + line_level = 1 + line.count(" " * tab_length, 0, line.index("*")) + if list_level >= line_level: + list_level = end_outstanding_lists(list_level, line_level) + else: + list_level += 1 + if list_level > 1: + output[-1] = output[-1].replace("", "") + output.append("
    ") + output.append(f"
  • {line.replace('*', '', 1).lstrip()}
  • ") + else: + list_level = end_outstanding_lists(list_level, 0) + if len(line) > 0: + output.append(line) + + end_outstanding_lists(list_level, 0) + output_str = "\n".join(output) + "\n" + + for (fr, to) in HTML_REPLACEMENTS: + output_str = output_str.replace(fr, to) + + return output_str diff --git a/common/mock/__init__.py b/common/mock/__init__.py new file mode 100644 index 0000000000..8c86bbd394 --- /dev/null +++ b/common/mock/__init__.py @@ -0,0 +1,50 @@ +""" +Utilities for generating mock messages for testing. +example in common/tests/test_mock.py +""" + + +import functools +import threading +from cereal.messaging import PubMaster +from cereal.services import SERVICE_LIST +from openpilot.common.mock.generators import generate_liveLocationKalman +from openpilot.common.realtime import Ratekeeper + + +MOCK_GENERATOR = { + "liveLocationKalman": generate_liveLocationKalman +} + + +def generate_messages_loop(services: list[str], done: threading.Event): + pm = PubMaster(services) + rk = Ratekeeper(100) + i = 0 + while not done.is_set(): + for s in services: + should_send = i % (100/SERVICE_LIST[s].frequency) == 0 + if should_send: + message = MOCK_GENERATOR[s]() + pm.send(s, message) + i += 1 + rk.keep_time() + + +def mock_messages(services: list[str] | str): + if isinstance(services, str): + services = [services] + + def decorator(func): + @functools.wraps(func) + def wrapper(*args, **kwargs): + done = threading.Event() + t = threading.Thread(target=generate_messages_loop, args=(services, done)) + t.start() + try: + return func(*args, **kwargs) + finally: + done.set() + t.join() + return wrapper + return decorator diff --git a/common/mock/generators.py b/common/mock/generators.py new file mode 100644 index 0000000000..40951faf85 --- /dev/null +++ b/common/mock/generators.py @@ -0,0 +1,20 @@ +from cereal import messaging + + +LOCATION1 = (32.7174, -117.16277) +LOCATION2 = (32.7558, -117.2037) + +LLK_DECIMATION = 10 +RENDER_FRAMES = 15 +DEFAULT_ITERATIONS = RENDER_FRAMES * LLK_DECIMATION + + +def generate_liveLocationKalman(location=LOCATION1): + msg = messaging.new_message('liveLocationKalman') + msg.liveLocationKalman.positionGeodetic = {'value': [*location, 0], 'std': [0., 0., 0.], 'valid': True} + msg.liveLocationKalman.positionECEF = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} + msg.liveLocationKalman.calibratedOrientationNED = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} + msg.liveLocationKalman.velocityCalibrated = {'value': [0., 0., 0.], 'std': [0., 0., 0.], 'valid': True} + msg.liveLocationKalman.status = 'valid' + msg.liveLocationKalman.gpsOK = True + return msg diff --git a/common/params.cc b/common/params.cc index aef2cbdecd..2330160173 100644 --- a/common/params.cc +++ b/common/params.cc @@ -89,6 +89,7 @@ private: std::unordered_map keys = { {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG}, + {"AlwaysOnDM", PERSISTENT}, {"ApiCache_Device", PERSISTENT}, {"ApiCache_NavDestinations", PERSISTENT}, {"AssistNowToken", PERSISTENT}, @@ -171,7 +172,6 @@ std::unordered_map keys = { {"Offroad_CarUnrecognized", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START}, {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START}, - {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START}, {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START}, {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START}, {"Offroad_NoFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, @@ -188,6 +188,7 @@ std::unordered_map keys = { {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReplayControlsState", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, + {"RouteCount", PERSISTENT}, {"SnoozeUpdate", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, {"SshEnabled", PERSISTENT}, {"TermsVersion", PERSISTENT}, @@ -206,7 +207,6 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, - {"VisionRadarToggle", PERSISTENT}, }; } // namespace @@ -273,7 +273,9 @@ int Params::put(const char* key, const char* value, size_t value_size) { } while (false); close(tmp_fd); - ::unlink(tmp_path.c_str()); + if (result != 0) { + ::unlink(tmp_path.c_str()); + } return result; } diff --git a/common/prefix.h b/common/prefix.h index f5abe14b2b..11d0bc1066 100644 --- a/common/prefix.h +++ b/common/prefix.h @@ -5,6 +5,7 @@ #include "common/params.h" #include "common/util.h" +#include "system/hardware/hw.h" class OpenpilotPrefix { public: @@ -25,6 +26,10 @@ public: system(util::string_format("rm %s -rf", real_path.c_str()).c_str()); unlink(param_path.c_str()); } + if (getenv("COMMA_CACHE") == nullptr) { + system(util::string_format("rm %s -rf", Path::download_cache_root().c_str()).c_str()); + } + system(util::string_format("rm %s -rf", Path::comma_home().c_str()).c_str()); system(util::string_format("rm %s -rf", msgq_path.c_str()).c_str()); unsetenv("OPENPILOT_PREFIX"); } diff --git a/common/prefix.py b/common/prefix.py index d027e3e5a1..3292acae86 100644 --- a/common/prefix.py +++ b/common/prefix.py @@ -2,14 +2,14 @@ import os import shutil import uuid -from typing import Optional from openpilot.common.params import Params +from openpilot.system.hardware import PC from openpilot.system.hardware.hw import Paths from openpilot.system.hardware.hw import DEFAULT_DOWNLOAD_CACHE_ROOT class OpenpilotPrefix: - def __init__(self, prefix: Optional[str] = None, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False): + def __init__(self, prefix: str = None, clean_dirs_on_exit: bool = True, shared_download_cache: bool = False): self.prefix = prefix if prefix else str(uuid.uuid4().hex[0:15]) self.msgq_path = os.path.join('/dev/shm', self.prefix) self.clean_dirs_on_exit = clean_dirs_on_exit @@ -46,7 +46,8 @@ class OpenpilotPrefix: shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True) os.remove(symlink_path) shutil.rmtree(self.msgq_path, ignore_errors=True) - shutil.rmtree(Paths.log_root(), ignore_errors=True) + if PC: + shutil.rmtree(Paths.log_root(), ignore_errors=True) if not os.environ.get("COMMA_CACHE", False): shutil.rmtree(Paths.download_cache_root(), ignore_errors=True) shutil.rmtree(Paths.comma_home(), ignore_errors=True) diff --git a/common/ratekeeper.h b/common/ratekeeper.h index b6e8ac66a6..e7323c6ec3 100644 --- a/common/ratekeeper.h +++ b/common/ratekeeper.h @@ -9,7 +9,7 @@ public: ~RateKeeper() {} bool keepTime(); bool monitorTime(); - inline double frame() const { return frame_; } + inline uint64_t frame() const { return frame_; } inline double remaining() const { return remaining_; } private: diff --git a/common/realtime.py b/common/realtime.py index a398146166..dd97ea3d78 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -3,7 +3,6 @@ import gc import os import time from collections import deque -from typing import Optional, List, Union from setproctitle import getproctitle @@ -13,7 +12,7 @@ from openpilot.system.hardware import PC # time step for each process DT_CTRL = 0.01 # controlsd DT_MDL = 0.05 # model -DT_TRML = 0.5 # thermald and manager +DT_HW = 0.5 # hardwared and manager DT_DMON = 0.05 # driver monitoring @@ -24,7 +23,7 @@ class Priority: CTRL_LOW = 51 # plannerd & radard # CORE 3 - # - boardd = 55 + # - pandad = 55 CTRL_HIGH = 53 @@ -33,12 +32,12 @@ def set_realtime_priority(level: int) -> None: os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) -def set_core_affinity(cores: List[int]) -> None: +def set_core_affinity(cores: list[int]) -> None: if not PC: os.sched_setaffinity(0, cores) -def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None: +def config_realtime_process(cores: int | list[int], priority: int) -> None: gc.disable() set_realtime_priority(priority) c = cores if isinstance(cores, list) else [cores, ] @@ -46,7 +45,7 @@ def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None class Ratekeeper: - def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None: + def __init__(self, rate: float, print_delay_threshold: float | None = 0.0) -> None: """Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative.""" self._interval = 1. / rate self._next_frame_time = time.monotonic() + self._interval diff --git a/common/run.py b/common/run.py new file mode 100644 index 0000000000..06deb6388d --- /dev/null +++ b/common/run.py @@ -0,0 +1,13 @@ +import subprocess + + +def run_cmd(cmd: list[str], cwd=None, env=None) -> str: + return subprocess.check_output(cmd, encoding='utf8', cwd=cwd, env=env).strip() + + +def run_cmd_default(cmd: list[str], default: str = "", cwd=None, env=None) -> str: + try: + return run_cmd(cmd, cwd=cwd, env=env) + except subprocess.CalledProcessError: + return default + diff --git a/common/spinner.py b/common/spinner.py index 43d4bb2cc2..dcf22641c4 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -3,7 +3,7 @@ import subprocess from openpilot.common.basedir import BASEDIR -class Spinner(): +class Spinner: def __init__(self): try: self.spinner_proc = subprocess.Popen(["./spinner"], diff --git a/common/stat_live.py b/common/stat_live.py index a91c1819bb..3901c448d8 100644 --- a/common/stat_live.py +++ b/common/stat_live.py @@ -1,6 +1,6 @@ import numpy as np -class RunningStat(): +class RunningStat: # tracks realtime mean and standard deviation without storing any data def __init__(self, priors=None, max_trackable=-1): self.max_trackable = max_trackable @@ -51,7 +51,7 @@ class RunningStat(): def params_to_save(self): return [self.M, self.S, self.n] -class RunningStatFilter(): +class RunningStatFilter: def __init__(self, raw_priors=None, filtered_priors=None, max_trackable=-1): self.raw_stat = RunningStat(raw_priors, -1) self.filtered_stat = RunningStat(filtered_priors, max_trackable) diff --git a/common/time.py b/common/time.py index f2e49eb546..5379faf22a 100644 --- a/common/time.py +++ b/common/time.py @@ -1,6 +1,15 @@ import datetime +from pathlib import Path -MIN_DATE = datetime.datetime(year=2024, month=1, day=28) +_MIN_DATE = datetime.datetime(year=2024, month=3, day=30) + +def min_date(): + # on systemd systems, the default time is the systemd build time + systemd_path = Path("/lib/systemd/systemd") + if systemd_path.exists(): + d = datetime.datetime.fromtimestamp(systemd_path.stat().st_mtime) + return d + datetime.timedelta(days=1) + return _MIN_DATE def system_time_valid(): - return datetime.datetime.now() > MIN_DATE + return datetime.datetime.now() > min_date() diff --git a/common/transformations/.gitignore b/common/transformations/.gitignore new file mode 100644 index 0000000000..a67290f09a --- /dev/null +++ b/common/transformations/.gitignore @@ -0,0 +1,2 @@ +transformations +transformations.cpp diff --git a/common/transformations/README.md b/common/transformations/README.md new file mode 100644 index 0000000000..42a060da80 --- /dev/null +++ b/common/transformations/README.md @@ -0,0 +1,70 @@ + +Reference Frames +------ +Many reference frames are used throughout. This +folder contains all helper functions needed to +transform between them. Generally this is done +by generating a rotation matrix and multiplying. + + +| Name | [x, y, z] | Units | Notes | +| :-------------: |:-------------:| :-----:| :----: | +| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. | +| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83.
    This is the global Mesh3D frame. | +| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. | +| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame.
    Relative to camera, **not imu.**
    ![img](http://upload.wikimedia.org/wikipedia/commons/thumb/2/2f/RPY_angles_of_airplanes.png/440px-RPY_angles_of_airplanes.png)| +| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in.
    More details below.
    | +| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road.
    More details below.
    | +| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. | +| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.| +| Normalized Camera | [u / focal, v / focal, 1] | / | | +| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. | +| Normalized Model | [u / focal, v / focal, 1] | / | | + + + + +Orientation Conventions +------ +Quaternions, rotation matrices and euler angles are three +equivalent representations of orientation and all three are +used throughout the code base. + +For euler angles the preferred convention is [roll, pitch, yaw] +which corresponds to rotations around the [x, y, z] axes. All +euler angles should always be in radians or radians/s unless +for plotting or display purposes. For quaternions the hamilton +notations is preferred which is [qw, qx, qy, qz]. All quaternions +should always be normalized with a strictly positive qw. **These +quaternions are a unique representation of orientation whereas euler angles +or rotation matrices are not.** + +To rotate from one frame into another with euler angles the +convention is to rotate around roll, then pitch and then yaw, +while rotating around the rotated axes, not the original axes. + + +Car frame +------ +Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different. + +The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects. + + +Calibrated frame +------ +It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame. + + +Example +------ +To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the +first position and orientation from Mesh3D one would do: +``` +ecef_from_local = rot_from_quat(quats_ecef[0]) +local_from_ecef = ecef_from_local.T +positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0]) +rotations_global = rot_from_quat(quats_ecef) +rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global) +eulers_local = euler_from_rot(rotations_local) +``` diff --git a/common/transformations/camera.py b/common/transformations/camera.py index c643cb5702..dc3ca5f388 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,55 +1,74 @@ +import itertools import numpy as np +from dataclasses import dataclass import openpilot.common.transformations.orientation as orient ## -- hardcoded hardware params -- -eon_f_focal_length = 910.0 -eon_d_focal_length = 650.0 -tici_f_focal_length = 2648.0 -tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame - -eon_f_frame_size = (1164, 874) -eon_d_frame_size = (816, 612) -tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208) - -# aka 'K' aka camera_frame_from_view_frame -eon_fcam_intrinsics = np.array([ - [eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2], - [0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2], - [0.0, 0.0, 1.0]]) -eon_intrinsics = eon_fcam_intrinsics # xx - -eon_dcam_intrinsics = np.array([ - [eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2], - [0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2], - [0.0, 0.0, 1.0]]) - -tici_fcam_intrinsics = np.array([ - [tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2], - [0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2], - [0.0, 0.0, 1.0]]) - -tici_dcam_intrinsics = np.array([ - [tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2], - [0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2], - [0.0, 0.0, 1.0]]) - -tici_ecam_intrinsics = tici_dcam_intrinsics - -# aka 'K_inv' aka view_frame_from_camera_frame -eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics) -eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx - -tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) -tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics) - - -FULL_FRAME_SIZE = tici_f_frame_size -FOCAL = tici_f_focal_length -fcam_intrinsics = tici_fcam_intrinsics - -W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] - +@dataclass(frozen=True) +class CameraConfig: + width: int + height: int + focal_length: float + + @property + def size(self): + return (self.width, self.height) + + @property + def intrinsics(self): + # aka 'K' aka camera_frame_from_view_frame + return np.array([ + [self.focal_length, 0.0, float(self.width)/2], + [0.0, self.focal_length, float(self.height)/2], + [0.0, 0.0, 1.0] + ]) + + @property + def intrinsics_inv(self): + # aka 'K_inv' aka view_frame_from_camera_frame + return np.linalg.inv(self.intrinsics) + +@dataclass(frozen=True) +class _NoneCameraConfig(CameraConfig): + width: int = 0 + height: int = 0 + focal_length: float = 0 + +@dataclass(frozen=True) +class DeviceCameraConfig: + fcam: CameraConfig + dcam: CameraConfig + ecam: CameraConfig + + def all_cams(self): + for cam in ['fcam', 'dcam', 'ecam']: + if not isinstance(getattr(self, cam), _NoneCameraConfig): + yield cam, getattr(self, cam) + +_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame +_os_fisheye = CameraConfig(2688, 1520, 567.0 / 2 * 3) +_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye) +_os_config = DeviceCameraConfig(CameraConfig(2688, 1520, 2648.0 * 2 / 3), _os_fisheye, _os_fisheye) +_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig()) + +DEVICE_CAMERAS = { + # A "device camera" is defined by a device type and sensor + + # sensor type was never set on eon/neo/two + ("neo", "unknown"): _neo_config, + # unknown here is AR0231, field was added with OX03C10 support + ("tici", "unknown"): _ar_ox_config, + + # before deviceState.deviceType was set, assume tici AR config + ("unknown", "ar0231"): _ar_ox_config, + ("unknown", "ox03c10"): _ar_ox_config, + + # simulator (emulates a tici) + ("pc", "unknown"): _ar_ox_config, +} +prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config))) +DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods}) # device/mesh : x->forward, y-> right, z->down # view : x->right, y->down, z->forward @@ -93,7 +112,7 @@ def roll_from_ke(m): -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) -def normalize(img_pts, intrinsics=fcam_intrinsics): +def normalize(img_pts, intrinsics): # normalizes image coordinates # accepts single pt or array of pts intrinsics_inv = np.linalg.inv(intrinsics) @@ -106,7 +125,7 @@ def normalize(img_pts, intrinsics=fcam_intrinsics): return img_pts_normalized[:, :2].reshape(input_shape) -def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf): +def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf): # denormalizes image coordinates # accepts single pt or array of pts img_pts = np.array(img_pts) @@ -123,7 +142,7 @@ 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): +def get_calib_from_vp(vp, intrinsics): vp_norm = normalize(vp, intrinsics) yaw_calib = np.arctan(vp_norm[0]) pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) diff --git a/common/transformations/model.py b/common/transformations/model.py index 7e40767f63..aaa12d776a 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,19 +1,11 @@ import numpy as np from openpilot.common.transformations.orientation import rot_from_euler -from openpilot.common.transformations.camera import ( - FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame, - eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics) +from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame # segnet SEGNET_SIZE = (512, 384) -def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE): - return np.array([[float(segnet_size[0]) / full_frame_size[0], 0.0], - [0.0, float(segnet_size[1]) / full_frame_size[1]]]) -segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx - - # MED model MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) @@ -63,16 +55,9 @@ calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3]) calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3]) # This function is verified to give similar results to xx.uncommon.utils.transform_img -def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray: - if tici and wide_camera: - cam_intrinsics = tici_ecam_intrinsics - elif tici: - cam_intrinsics = tici_fcam_intrinsics - else: - cam_intrinsics = eon_fcam_intrinsics - +def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray: calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel device_from_calib = rot_from_euler(device_from_calib_euler) - camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib + camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib warp_matrix: np.ndarray = camera_from_calib @ calib_from_model return warp_matrix diff --git a/common/transformations/orientation.py b/common/transformations/orientation.py index ce4378738d..86e6a6c347 100644 --- a/common/transformations/orientation.py +++ b/common/transformations/orientation.py @@ -1,5 +1,5 @@ import numpy as np -from typing import Callable +from collections.abc import Callable from openpilot.common.transformations.transformations import (ecef_euler_from_ned_single, euler2quat_single, diff --git a/rednose/__init__.py b/common/transformations/tests/__init__.py similarity index 100% rename from rednose/__init__.py rename to common/transformations/tests/__init__.py diff --git a/common/transformations/tests/test_coordinates.py b/common/transformations/tests/test_coordinates.py new file mode 100644 index 0000000000..11a6bf70ee --- /dev/null +++ b/common/transformations/tests/test_coordinates.py @@ -0,0 +1,104 @@ +import numpy as np + +import openpilot.common.transformations.coordinates as coord + +geodetic_positions = np.array([[37.7610403, -122.4778699, 115], + [27.4840915, -68.5867592, 2380], + [32.4916858, -113.652821, -6], + [15.1392514, 103.6976037, 24], + [24.2302229, 44.2835412, 1650]]) + +ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935], + [ 2068042.69652729, -5273435.40316622, 2927004.89190746], + [-2160412.60461669, -4932588.89873832, 3406542.29652851], + [-1458247.92550567, 5983060.87496612, 1654984.6099885 ], + [ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]]) + +ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613, 3884605.16002147], + [ 2068074.30639499, -5273413.78835412, 2927012.48741131], + [-2160344.53748176, -4932586.20092211, 3406636.2962545 ], + [-1458211.98517094, 5983151.11161276, 1655077.02698447], + [ 4167271.20055269, 4064398.22619263, 2602238.95265847]]) + + +ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436], + [10.699003365155221, 37.319278617604269, 4.1084100025050407], + [95.282646251726959, 61.266689955574428, -25.376506058505054], + [68.535769283630003, -56.285970011848889, -100.54840137956515], + [-33.066609321880179, 46.549821994306861, -84.062540548335591]]) + +ecef_init_batch = np.array([2068042.69652729, -5273435.40316622, 2927004.89190746]) +ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148, 2927074.04783672], + [ 2068103.31628647, -5273393.92275431, 2927102.08725987], + [ 2068108.49939636, -5273359.27047121, 2927045.07091581], + [ 2068075.12395611, -5273381.69432566, 2927041.08207992], + [ 2068060.72033399, -5273430.6061505, 2927094.54928305]]) + +ned_offsets_batch = np.array([[ 53.88103168, 43.83445935, -46.27488057], + [ 93.83378995, 71.57943024, -30.23113187], + [ 57.26725796, 89.05602684, 23.02265814], + [ 49.71775195, 49.79767572, 17.15351015], + [ 78.56272609, 18.53100158, -43.25290759]]) + + +class TestNED: + def test_small_distances(self): + start_geodetic = np.array([33.8042184, -117.888593, 0.0]) + local_coord = coord.LocalCoord.from_geodetic(start_geodetic) + + start_ned = local_coord.geodetic2ned(start_geodetic) + np.testing.assert_array_equal(start_ned, np.zeros(3,)) + + west_geodetic = start_geodetic + [0, -0.0005, 0] + west_ned = local_coord.geodetic2ned(west_geodetic) + assert np.abs(west_ned[0]) < 1e-3 + assert west_ned[1] < 0 + + southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0] + southwest_ned = local_coord.geodetic2ned(southwest_geodetic) + assert southwest_ned[0] < 0 + assert southwest_ned[1] < 0 + + def test_ecef_geodetic(self): + # testing single + np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9) + np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9) + np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4) + + np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9) + np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4) + np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9) + + + def test_ned(self): + for ecef_pos in ecef_positions: + converter = coord.LocalCoord.from_ecef(ecef_pos) + ecef_pos_moved = ecef_pos + [25, -25, 25] + ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved)) + np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9) + + for geo_pos in geodetic_positions: + converter = coord.LocalCoord.from_geodetic(geo_pos) + geo_pos_moved = geo_pos + np.array([0, 0, 10]) + geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10])) + np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6) + np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4) + + def test_ned_saved_results(self): + for i, ecef_pos in enumerate(ecef_positions): + converter = coord.LocalCoord.from_ecef(ecef_pos) + np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]), + ecef_positions_offset[i], + rtol=1e-9, atol=1e-4) + np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]), + ned_offsets[i], + rtol=1e-9, atol=1e-4) + + def test_ned_batch(self): + converter = coord.LocalCoord.from_ecef(ecef_init_batch) + np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch), + ned_offsets_batch, + rtol=1e-9, atol=1e-7) + np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch), + ecef_positions_offset_batch, + rtol=1e-9, atol=1e-7) diff --git a/common/transformations/tests/test_orientation.py b/common/transformations/tests/test_orientation.py new file mode 100644 index 0000000000..55fbc6581e --- /dev/null +++ b/common/transformations/tests/test_orientation.py @@ -0,0 +1,61 @@ +import numpy as np + +from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \ + rot2quat, quat2rot, \ + ned_euler_from_ecef + +eulers = np.array([[ 1.46520501, 2.78688383, 2.92780854], + [ 4.86909526, 3.60618161, 4.30648981], + [ 3.72175965, 2.68763705, 5.43895988], + [ 5.92306687, 5.69573614, 0.81100357], + [ 0.67838374, 5.02402037, 2.47106426]]) + +quats = np.array([[ 0.66855182, -0.71500939, 0.19539353, 0.06017818], + [ 0.43163717, 0.70013301, 0.28209145, 0.49389021], + [ 0.44121991, -0.08252646, 0.34257534, 0.82532207], + [ 0.88578382, -0.04515356, -0.32936046, 0.32383617], + [ 0.06578165, 0.61282835, 0.07126891, 0.78424163]]) + +ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935], + [ 2068042.69652729, -5273435.40316622, 2927004.89190746], + [-2160412.60461669, -4932588.89873832, 3406542.29652851], + [-1458247.92550567, 5983060.87496612, 1654984.6099885 ], + [ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]]) + +ned_eulers = np.array([[ 0.46806039, -0.4881889 , 1.65697808], + [-2.14525969, -0.36533066, 0.73813479], + [-1.39523364, -0.58540761, -1.77376356], + [-1.84220435, 0.61828016, -1.03310421], + [ 2.50450101, 0.36304151, 0.33136365]]) + + +class TestOrientation: + def test_quat_euler(self): + for i, eul in enumerate(eulers): + np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7) + np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6) + for i, eul in enumerate(eulers): + np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7) + np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6) + np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7) + np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6) + + def test_rot_euler(self): + for eul in eulers: + np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7) + for eul in eulers: + np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7) + np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7) + + def test_rot_quat(self): + for quat in quats: + np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7) + for quat in quats: + np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7) + np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7) + + def test_euler_ned(self): + for i in range(len(eulers)): + np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7) + #np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7) + # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7) diff --git a/common/util.cc b/common/util.cc index 8af9f5884c..5ffd6e4099 100644 --- a/common/util.cc +++ b/common/util.cc @@ -246,18 +246,13 @@ std::string random_string(std::string::size_type length) { return s; } -std::string dir_name(std::string const &path) { - size_t pos = path.find_last_of("/"); - if (pos == std::string::npos) return ""; - return path.substr(0, pos); -} - bool starts_with(const std::string &s1, const std::string &s2) { return strncmp(s1.c_str(), s2.c_str(), s2.size()) == 0; } -bool ends_with(const std::string &s1, const std::string &s2) { - return strcmp(s1.c_str() + (s1.size() - s2.size()), s2.c_str()) == 0; +bool ends_with(const std::string& s, const std::string& suffix) { + return s.size() >= suffix.size() && + strcmp(s.c_str() + (s.size() - suffix.size()), suffix.c_str()) == 0; } std::string check_output(const std::string& command) { @@ -276,20 +271,4 @@ std::string check_output(const std::string& command) { return result; } -struct tm get_time() { - time_t rawtime; - time(&rawtime); - - struct tm sys_time; - gmtime_r(&rawtime, &sys_time); - - return sys_time; -} - -bool time_valid(struct tm sys_time) { - int year = 1900 + sys_time.tm_year; - int month = 1 + sys_time.tm_mon; - return (year > 2023) || (year == 2023 && month >= 6); -} - } // namespace util diff --git a/common/util.h b/common/util.h index 0cbad5bd31..186873ac21 100644 --- a/common/util.h +++ b/common/util.h @@ -8,7 +8,6 @@ #include #include #include -#include #include #include #include @@ -45,10 +44,6 @@ int set_realtime_priority(int level); int set_core_affinity(std::vector cores); int set_file_descriptor_limit(uint64_t limit); -// ***** Time helpers ***** -struct tm get_time(); -bool time_valid(struct tm sys_time); - // ***** math helpers ***** // map x from [a1, a2] to [b1, b2] @@ -75,9 +70,8 @@ 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 dir_name(std::string const& path); bool starts_with(const std::string &s1, const std::string &s2); -bool ends_with(const std::string &s1, const std::string &s2); +bool ends_with(const std::string &s, const std::string &suffix); // ***** random helpers ***** int random_int(int min, int max); @@ -179,3 +173,10 @@ void update_max_atomic(std::atomic& max, T const& value) { T prev = max; while (prev < value && !max.compare_exchange_weak(prev, value)) {} } + +typedef struct Rect { + int x; + int y; + int w; + int h; +} Rect; diff --git a/common/utils.py b/common/utils.py new file mode 100644 index 0000000000..b9de020ee6 --- /dev/null +++ b/common/utils.py @@ -0,0 +1,11 @@ +class Freezable: + _frozen: bool = False + + def freeze(self): + if not self._frozen: + self._frozen = True + + def __setattr__(self, *args, **kwargs): + if self._frozen: + raise Exception("cannot modify frozen object") + super().__setattr__(*args, **kwargs) diff --git a/common/version.h b/common/version.h index 787bc897d7..177882e31d 100644 --- a/common/version.h +++ b/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.9.6" +#define COMMA_VERSION "0.9.7" diff --git a/conftest.py b/conftest.py new file mode 100644 index 0000000000..fc4931fb54 --- /dev/null +++ b/conftest.py @@ -0,0 +1,100 @@ +import contextlib +import gc +import os +import pytest +import random + +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.system.manager import manager +from openpilot.system.hardware import TICI, HARDWARE + + +def pytest_sessionstart(session): + # TODO: fix tests and enable test order randomization + if session.config.pluginmanager.hasplugin('randomly'): + session.config.option.randomly_reorganize = False + + +@pytest.hookimpl(hookwrapper=True, trylast=True) +def pytest_runtest_call(item): + # ensure we run as a hook after capturemanager's + if item.get_closest_marker("nocapture") is not None: + capmanager = item.config.pluginmanager.getplugin("capturemanager") + with capmanager.global_and_fixture_disabled(): + yield + else: + yield + + +@contextlib.contextmanager +def clean_env(): + starting_env = dict(os.environ) + yield + os.environ.clear() + os.environ.update(starting_env) + + +@pytest.fixture(scope="function", autouse=True) +def openpilot_function_fixture(request): + random.seed(0) + + with clean_env(): + # setup a clean environment for each test + with OpenpilotPrefix(shared_download_cache=request.node.get_closest_marker("shared_download_cache") is not None) as prefix: + prefix = os.environ["OPENPILOT_PREFIX"] + + yield + + # ensure the test doesn't change the prefix + assert "OPENPILOT_PREFIX" in os.environ and prefix == os.environ["OPENPILOT_PREFIX"] + + # cleanup any started processes + manager.manager_cleanup() + + # some processes disable gc for performance, re-enable here + if not gc.isenabled(): + gc.enable() + gc.collect() + +# If you use setUpClass, the environment variables won't be cleared properly, +# so we need to hook both the function and class pytest fixtures +@pytest.fixture(scope="class", autouse=True) +def openpilot_class_fixture(): + with clean_env(): + yield + + +@pytest.fixture(scope="function") +def tici_setup_fixture(openpilot_function_fixture): + """Ensure a consistent state for tests on-device. Needs the openpilot function fixture to run first.""" + HARDWARE.initialize_hardware() + HARDWARE.set_power_save(False) + os.system("pkill -9 -f athena") + + +@pytest.hookimpl(tryfirst=True) +def pytest_collection_modifyitems(config, items): + skipper = pytest.mark.skip(reason="Skipping tici test on PC") + for item in items: + if "tici" in item.keywords: + if not TICI: + item.add_marker(skipper) + else: + item.fixturenames.append('tici_setup_fixture') + + if "xdist_group_class_property" in item.keywords: + class_property_name = item.get_closest_marker('xdist_group_class_property').args[0] + class_property_value = getattr(item.cls, class_property_name) + item.add_marker(pytest.mark.xdist_group(class_property_value)) + + +@pytest.hookimpl(trylast=True) +def pytest_configure(config): + config_line = "xdist_group_class_property: group tests by a property of the class that contains them" + config.addinivalue_line("markers", config_line) + + config_line = "nocapture: don't capture test output" + config.addinivalue_line("markers", config_line) + + config_line = "shared_download_cache: share download cache between tests" + config.addinivalue_line("markers", config_line) diff --git a/docs/BOUNTIES.md b/docs/BOUNTIES.md new file mode 100644 index 0000000000..31e19e1206 --- /dev/null +++ b/docs/BOUNTIES.md @@ -0,0 +1,62 @@ +# [Bounties](https://github.com/orgs/commaai/projects/26/views/1) + +Get paid to improve openpilot! + +## Rules + +* code must be merged into openpilot master +* bounty eligibility is solely at our discretion +* once you open a PR, the bounty is locked to you until you stop working on it +* open a ticket at [comma.ai/support](https://comma.ai/support/shop-order) with links to your PRs to claim +* get an extra 20% if you redeem your bounty in [comma shop](https://comma.ai/shop) credit (including refunds on previous orders) +* for bounties >$100, the first PR gets a lock, which times out after a week of no progress + +We put up each bounty with the intention that it'll get merged, but occasionally the right resolution is to close the bounty, which only becomes clear once some effort is put in. +This is still valuable work, so we'll pay out $100 for getting any bounty closed with a good explanation. + +## Issue bounties + +We've tagged bounty-eligible issues across openpilot and the rest of our repos; check out all the open ones [here](https://github.com/orgs/commaai/projects/26/views/1). These bounties roughly work out like this: +* **$100** - a few hours of work for an experienced openpilot developer; a good intro for someone new to openpilot +* **$300** - a day of work for an experienced openpilot developer +* **$500** - a few days of work for an experienced openpilot developer +* **$1k+** - a week or two of work (could be less for the right person) + +New bounties can be proposed in the [**#contributing**](https://discord.com/channels/469524606043160576/1183173332531687454) channel in Discord. + +## Car bounties + +The car bounties only apply to cars that have a path to ship in openpilot release, which excludes unsupportable cars (e.g. Fords with a steering lockout) or cars that require extra hardware (Honda Accord with serial steering). + +#### Brand or platform port - $2000 +Example PR: [commaai/openpilot#23331](https://github.com/commaai/openpilot/pull/23331) + +This is for adding support for an entirely new brand or a substantially new ADAS platform within a brand (e.g. the Volkswagen PQ platform). + +#### Model port - $250 +Example PR: [commaai/openpilot#30245](https://github.com/commaai/openpilot/pull/30245) + +This is for porting a new car model that runs on a platform openpilot already supports. +In the average case, this is a few hours of work for an experienced software developer. + +This bounty also covers getting openpilot supported on a previously unsupported trim of an already supported car, e.g. the Chevy Bolt without ACC. + +#### Reverse Engineering a new Actuation Message - $300 + +This is for cars that are already supported, and it has three components: +* reverse a new steering, adaptive cruise, or AEB message +* merge the DBC definitions to [opendbc](http://github.com/commaai/opendbc) +* merge the openpilot code to use it and post a demo route + +The control doesn't have to be perfect, but it should generally do what it's supposed to do. + +### Specific Cars + +#### Rivian R1T or R1S - $3000 + +Get a Rivian driving with openpilot. +Requires a merged port with lateral control and at least a POC of longitudinal control. + +#### Chevy Bolt with SuperCruise - $2500 + +The Bolt is already supported on the trim with standard ACC. Get openpilot working on the trim with SuperCruise. It must be a normal install: no extra pandas or other hardware, no ECU reflashes, etc. The full bounty is for a port with lateral and longitudinal control. $1500 of the bounty can be claimed with a lateral-only port. diff --git a/docs/CARS.md b/docs/CARS.md index 6d3ee71659..3b163dcee3 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,29 +4,24 @@ A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 279 Supported Cars +# 288 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
     |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| |Acura|ILX 2016-19|AcuraWatch Plus|openpilot|25 mph|25 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Acura|RDX 2016-18|AcuraWatch Plus|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Acura|RDX 2019-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Buick|LaCrosse 2017-19[4](#footnotes)|Driver Confidence Package 2|openpilot|18 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Cadillac|Escalade 2017[4](#footnotes)|Driver Assist Package|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Cadillac|Escalade ESV 2016[4](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Cadillac|Escalade ESV 2019[4](#footnotes)|Adaptive Cruise Control (ACC) & LKAS|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|Q3 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chevrolet|Bolt EUV 2022-23|Premier or Premier Redline Trim without Super Cruise Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chevrolet|Bolt EV 2022-23|2LT Trim with Adaptive Cruise Control Package|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chevrolet|Equinox 2019-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chevrolet|Silverado 1500 2020-21|Safety Package II|openpilot available[1](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|openpilot available[1](#footnotes)|3 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Chevrolet|Volt 2017-18[4](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chrysler|Pacifica 2017-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chrysler|Pacifica 2019-20|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chrysler|Pacifica 2021-23|All|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -34,36 +29,45 @@ A supported vehicle is one that just works when you install a comma device. All |Chrysler|Pacifica Hybrid 2018|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Chrysler|Pacifica Hybrid 2019-23|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| +|CUPRA|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Dodge|Durango 2020-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Bronco Sport 2021-23|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Escape Plug-in Hybrid 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ford|Explorer 2020-23|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Explorer Hybrid 2020-23|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ford|Focus 2018[3](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Focus Hybrid 2018[3](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Kuga Hybrid 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Kuga Plug-in Hybrid 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ford|Maverick 2022|LARIAT Luxury|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Ford|Maverick 2023|Co-Pilot360 Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|G70 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|G70 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Maverick 2023-24|Co-Pilot360 Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Maverick Hybrid 2022|LARIAT Luxury|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|G70 2018|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|G70 2019-21|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|G70 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|G80 2017|All|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Genesis|G80 2018-19|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|G90 2017-18|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|GV60 (Advanced Trim) 2023[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|GV60 (Performance Trim) 2023[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|GV70 (2.5T Trim) 2022-23[6](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|GV70 (3.5T Trim) 2022-23[6](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Genesis|GV80 2023[6](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|GMC|Acadia 2018[4](#footnotes)|Adaptive Cruise Control (ACC)|openpilot|0 mph|7 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 OBD-II connector
    - 1 comma 3X
    - 2 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|G90 2017-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|GV60 (Advanced Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|GV60 (Performance Trim) 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|GV70 (2.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|GV70 (3.5T Trim) 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Genesis|GV80 2023[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai M connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |GMC|Sierra 1500 2020-21|Driver Alert Package II|openpilot available[1](#footnotes)|0 mph|6 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 GM connector
    - 1 comma 3X
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Accord 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Accord Hybrid 2018-22|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Civic 2016-18|Honda Sensing|openpilot|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[5](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Honda|Civic 2019-21|All|openpilot available[1](#footnotes)|0 mph|2 mph[4](#footnotes)|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Civic 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Civic Hatchback 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Civic Hatchback 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|CR-V 2015-16|Touring Trim|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|CR-V 2017-22|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Honda|CR-V Hybrid 2017-20|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Honda|CR-V Hybrid 2017-21|Honda Sensing|openpilot available[1](#footnotes)|0 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|e 2020|All|openpilot available[1](#footnotes)|0 mph|3 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Honda Bosch A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Fit 2018-20|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Honda|Freed 2020|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Honda Nidec connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -79,30 +83,30 @@ A supported vehicle is one that just works when you install a comma device. All |Hyundai|Azera Hybrid 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Azera Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Custin 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Elantra 2019|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Elantra 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Elantra GT 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Elantra Hybrid 2021-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Genesis 2015-16|Smart Cruise Control (SCC)|Stock|19 mph|37 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai J connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|i30 2017-19|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Ioniq 5 (with HDA II) 2022-23[6](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Ioniq 5 (without HDA II) 2022-23[6](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Ioniq 6 (with HDA II) 2023[6](#footnotes)|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Ioniq 5 (Southeast Asia only) 2022-23[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Ioniq 5 (with HDA II) 2022-23[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Ioniq 5 (without HDA II) 2022-23[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Ioniq 6 (with HDA II) 2023-24[5](#footnotes)|Highway Driving Assist II|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Electric 2019|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Electric 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Hybrid 2017-19|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Hybrid 2020-22|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Ioniq Plug-in Hybrid 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Kona 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai O connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Kona Electric (with HDA II, Korea only) 2023[6](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai R connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Kona Electric (with HDA II, Korea only) 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai R connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai I connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Santa Cruz 2022-23[6](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Santa Cruz 2022-24[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Santa Fe 2021-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Santa Fe Hybrid 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -110,49 +114,51 @@ A supported vehicle is one that just works when you install a comma device. All |Hyundai|Sonata 2018-19|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Sonata 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Sonata Hybrid 2020-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Staria 2023[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Tucson 2022[6](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Tucson 2023[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Staria 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Tucson 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Tucson 2022[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Tucson 2023-24[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Tucson Hybrid 2022-24[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Tucson Hybrid 2022-24[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Tucson Plug-in Hybrid 2024[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 FCA connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Carnival 2022-24[6](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Carnival (China only) 2023[6](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Carnival 2022-24[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Carnival (China only) 2023[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Ceed 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|EV6 (Southeast Asia only) 2022-23[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|EV6 (with HDA II) 2022-23[6](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|EV6 (without HDA II) 2022-23[6](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Forte 2023|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|EV6 (Southeast Asia only) 2022-24[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|EV6 (with HDA II) 2022-24[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai P connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|EV6 (without HDA II) 2022-24[5](#footnotes)|Highway Driving Assist|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai L connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Forte 2019-21|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|6 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Forte 2022-23|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|K5 2021-24|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|K5 Hybrid 2020-22|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|K8 Hybrid (with HDA II) 2023[6](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|K8 Hybrid (with HDA II) 2023[5](#footnotes)|Highway Driving Assist II|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai Q connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro EV 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro EV 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro EV 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro EV 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro EV 2023[6](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro EV 2023[5](#footnotes)|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Hybrid 2018|All|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Niro Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro Hybrid 2023[6](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro Plug-in Hybrid 2018-19|All|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro Plug-in Hybrid 2020|All|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro Plug-in Hybrid 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Niro Plug-in Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Plug-in Hybrid 2018-19|All|Stock|10 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Plug-in Hybrid 2020|Smart Cruise Control (SCC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Plug-in Hybrid 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai D connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Niro Plug-in Hybrid 2022|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai F connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Optima 2017|Advanced Smart Cruise Control|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai B connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Optima 2019-20|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai G connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Optima Hybrid 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Seltos 2021|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Sorento 2018|Advanced Smart Cruise Control & LKAS|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Sorento 2019|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai E connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Sorento 2021-23[6](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Sorento Hybrid 2021-23[6](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Sorento Plug-in Hybrid 2022-23[6](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Sportage 2023[6](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Kia|Sportage Hybrid 2023[6](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Sorento 2021-23[5](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Sorento Hybrid 2021-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Sorento Plug-in Hybrid 2022-23[5](#footnotes)|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai A connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Sportage 2023-24[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Kia|Sportage Hybrid 2023[5](#footnotes)|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai N connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Stinger 2018-20|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai C connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Stinger 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai K connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Kia|Telluride 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Hyundai H connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -177,9 +183,10 @@ A supported vehicle is one that just works when you install a comma device. All |Lexus|RX Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Lexus|RX Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Lexus|UX Hybrid 2019-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Lincoln|Aviator 2020-21|Co-Pilot360 Plus|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|MAN|eTGE 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|MAN|TGE 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Lincoln|Aviator 2020-23|Co-Pilot360 Plus|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Lincoln|Aviator Plug-in Hybrid 2020-23|Co-Pilot360 Plus|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Ford Q3 connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|MAN|eTGE 2020-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|MAN|TGE 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Mazda|CX-5 2022-24|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Mazda connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Mazda|CX-9 2021-23|All|Stock|0 mph|28 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 Mazda connector
    - 1 RJ45 cable (7 ft)
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Nissan|Altima 2019-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan B connector
    - 1 RJ45 cable (7 ft)
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -187,26 +194,27 @@ A supported vehicle is one that just works when you install a comma device. All |Nissan|Rogue 2018-20|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan A connector
    - 1 RJ45 cable (7 ft)
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Nissan|X-Trail 2017|ProPILOT Assist|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 Nissan A connector
    - 1 RJ45 cable (7 ft)
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Ram|1500 2019-24|Adaptive Cruise Control (ACC)|Stock|0 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Ram connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|SEAT|Ateca 2018|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Subaru|Ascent 2019-21|All[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Forester 2019-21|All[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Impreza 2017-19|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Impreza 2020-22|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Legacy 2020-22|All[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru B connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|Outback 2020-22|All[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru B connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|XV 2018-19|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Subaru|XV 2020-21|EyeSight Driver Assistance[7](#footnotes)|openpilot available[1,8](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || -|Škoda|Fabia 2022-23[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Škoda|Kamiq 2021-23[10,12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Škoda|Karoq 2019-23[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Škoda|Kodiaq 2017-23[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Škoda|Octavia 2015-19[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Škoda|Octavia RS 2016[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Škoda|Scala 2020-23[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Škoda|Superb 2015-22[12](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Subaru|Ascent 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Forester 2019-21|All[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Impreza 2017-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Impreza 2020-22|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Legacy 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru B connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|Outback 2020-22|All[6](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru B connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|XV 2018-19|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Subaru|XV 2020-21|EyeSight Driver Assistance[6](#footnotes)|openpilot available[1,7](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Subaru A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    Tools- 1 Pry Tool
    - 1 Socket Wrench 8mm or 5/16" (deep)
    || +|Škoda|Fabia 2022-23[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Škoda|Kamiq 2021-23[9,11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Škoda|Karoq 2019-23[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Škoda|Kodiaq 2017-23[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Škoda|Octavia 2015-19[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Škoda|Octavia RS 2016[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Škoda|Octavia Scout 2017-19[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Škoda|Scala 2020-23[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Škoda|Superb 2015-22[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Alphard 2019-20|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Alphard Hybrid 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Avalon 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -219,20 +227,20 @@ A supported vehicle is one that just works when you install a comma device. All |Toyota|C-HR 2021|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|C-HR Hybrid 2017-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|C-HR Hybrid 2021-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Camry 2018-20|All|Stock|0 mph[9](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Camry 2021-24|All|openpilot|0 mph[9](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Camry 2018-20|All|Stock|0 mph[8](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Camry 2021-24|All|openpilot|0 mph[8](#footnotes)|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Camry Hybrid 2018-20|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Camry Hybrid 2021-24|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Corolla 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Corolla 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Corolla Cross (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Corolla Cross Hybrid (Non-US only) 2020-22|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Corolla Hatchback 2019-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Corolla Hybrid 2020-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Corolla Hybrid (Non-US only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Corolla Hybrid (South America only) 2020-23|All|openpilot|17 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Highlander 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Highlander 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Highlander Hybrid 2017-19|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Highlander Hybrid 2020-23|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Mirai 2021|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Prius 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || @@ -240,69 +248,69 @@ A supported vehicle is one that just works when you install a comma device. All |Toyota|Prius 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Prius Prime 2017-20|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|Prius Prime 2021-22|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Prius v 2017|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 2023-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 Hybrid 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || |Toyota|RAV4 Hybrid 2023-24|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Crafter 2017-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|e-Crafter 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Grand California 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Jetta 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Jetta GLI 2021-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Passat 2015-22[11](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Volkswagen|T-Roc 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [14](#footnotes)|| -|Volkswagen|Taos 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || -|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,13](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 RJ45 cable (7 ft)
    - 1 Toyota A connector
    - 1 comma 3X
    - 1 comma power v2
    - 1 harness box
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Arteon Shooting Brake 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Atlas 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Atlas Cross Sport 2020-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|California 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Caravelle 2020|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|CC 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Crafter 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|e-Crafter 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|e-Golf 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf Alltrack 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf GTD 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf GTE 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf GTI 2015-21|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|31 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 angled mount (8 degrees)
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Jetta 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Jetta GLI 2021-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Passat 2015-22[10](#footnotes)|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Passat Alltrack 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Passat GTE 2015-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Polo 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Volkswagen|Polo GTI 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Volkswagen|T-Cross 2021|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    [13](#footnotes)|| +|Volkswagen|T-Roc 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Taos 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Teramont 2018-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Teramont Cross Sport 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Teramont X 2021-22|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Tiguan 2018-24|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Tiguan eHybrid 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || +|Volkswagen|Touran 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
    Parts- 1 J533 connector
    - 1 USB-C coupler
    - 1 comma 3X
    - 1 harness box
    - 1 long OBD-C cable
    - 1 mount
    - 1 right angle OBD-C cable (1.5 ft)
    Buy Here
    || ### Footnotes 1openpilot Longitudinal Control (Alpha) is available behind a toggle; the toggle is only available in non-release branches such as `devel` or `master-ci`.
    2By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. If the Driver Support Unit (DSU) is disconnected, openpilot ACC will replace stock ACC. NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).
    3Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in North and South America/Southeast Asia.
    -4Requires a community built ASCM harness. NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).
    -52019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
    -6Requires a CAN FD panda kit if not using comma 3X for this CAN FD car.
    -7In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.
    -8Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB.
    -9openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
    -10Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
    -11Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
    -12Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma 3X functionality.
    -13Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
    -14Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
    +42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
    +5Requires a CAN FD panda kit if not using comma 3X for this CAN FD car.
    +6In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.
    +7Enabling longitudinal control (alpha) will disable all EyeSight functionality, including AEB, LDW, and RAB.
    +8openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
    +9Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.
    +10Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.
    +11Some Škoda vehicles are equipped with heated windshields, which are known to block GPS signal needed for some comma 3X functionality.
    +12Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness are limited to using stock ACC.
    +13Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot in software, but doesn't yet have a harness available from the comma store.
    ## Community Maintained Cars Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). @@ -343,7 +351,9 @@ openpilot does not yet support these Toyota models due to a new message authenti * Toyota Venza 2021+ * Toyota Sequoia 2023+ * Toyota Tundra 2022+ +* Toyota Highlander 2024+ * Toyota Corolla Cross 2022+ (only US model) +* Toyota Camry 2025+ * Lexus NX 2022+ * Toyota bZ4x 2023+ * Subaru Solterra 2023+ diff --git a/docs/CONTRIBUTING.md b/docs/CONTRIBUTING.md index 30f4e0dfd3..755ca82220 100644 --- a/docs/CONTRIBUTING.md +++ b/docs/CONTRIBUTING.md @@ -62,4 +62,4 @@ A good pull request has all of the following: * Consider opting into driver camera uploads to improve the driver monitoring model. * Connect your device to Wi-Fi regularly, so that we can pull data for training better driving models. * Run the `nightly` branch and report issues. This branch is like `master` but it's built just like a release. -* Annotate images in the [comma10k dateset](https://github.com/commaai/comma10k). +* Annotate images in the [comma10k dataset](https://github.com/commaai/comma10k). diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000000..09103e206e --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,53 @@ +# Minimal makefile for Sphinx documentation +# + +OPENPILOT_ROOT = `git rev-parse --show-toplevel` + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +DOCSDIR = "$(OPENPILOT_ROOT)/docs" +SOURCEDIR = "$(OPENPILOT_ROOT)/build/docs" +DOCSBUILDDIR = "$(OPENPILOT_ROOT)/build/docs" +BUILDDIR = "$(OPENPILOT_ROOT)/build" + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(DOCSBUILDDIR)" $(SPHINXOPTS) $(O) + +clean: + @echo "Cleaning build folder..." + rm -rf "$(BUILDDIR)" + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @echo "Cleaning build folder..." + rm -rf "$(BUILDDIR)" + mkdir -p "$(DOCSBUILDDIR)" + + @echo "Copying docs & config to build folder..." + cp -a "$(DOCSDIR)" "$(BUILDDIR)" + cd "$(OPENPILOT_ROOT)" && \ + find . -type f \( -name "*.md" -o -name "*.rst" -o -name "*.png" -o -name "*.jpg" -o -name "*.svg" \) \ + -not -path "*/.*" \ + -not -path "./build/*" \ + -not -path "./docs/*" \ + -not -path "./xx/*" \ + -exec cp --parents "{}" ./build/docs/ \; + + @echo "Building rst files..." + sphinx-apidoc -o "$(DOCSBUILDDIR)" ../ \ + ../xx ../rednose_repo ../notebooks ../panda_jungle \ + ../third_party \ + ../panda/examples \ + ../scripts \ + ../selfdrive/modeld \ + ../selfdrive/debug \ + $(shell find .. -type d -name "*test* -not -path "**.venv**" \") + + @echo "Building html files..." + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(DOCSBUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/README.md b/docs/README.md new file mode 100644 index 0000000000..838b00e7ae --- /dev/null +++ b/docs/README.md @@ -0,0 +1,3 @@ +# openpilot-docs + +These docs are autogenerated from [this folder](https://github.com/commaai/openpilot/tree/master/docs) in the main openpilot repository. \ No newline at end of file diff --git a/docs/SAFETY.md b/docs/SAFETY.md new file mode 100644 index 0000000000..4b568728a7 --- /dev/null +++ b/docs/SAFETY.md @@ -0,0 +1,36 @@ +# Safety + +openpilot is an Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC) system. +Like other ACC and ALC systems, openpilot is a failsafe passive system and it requires the +driver to be alert and to pay attention at all times. + +In order to enforce driver alertness, openpilot includes a driver monitoring feature +that alerts the driver when distracted. + +However, even with an attentive driver, we must make further efforts for the system to be +safe. We repeat, **driver alertness is necessary, but not sufficient, for openpilot to be +used safely** and openpilot is provided with no warranty of fitness for any purpose. + +openpilot is developed in good faith to be compliant with FMVSS requirements and to follow +industry standards of safety for Level 2 Driver Assistance Systems. In particular, we observe +ISO26262 guidelines, including those from [pertinent documents](https://www.nhtsa.gov/sites/nhtsa.dot.gov/files/documents/13498a_812_573_alcsystemreport.pdf) +released by NHTSA. In addition, we impose strict coding guidelines (like [MISRA C : 2012](https://www.misra.org.uk/what-is-misra/)) +on parts of openpilot that are safety relevant. We also perform software-in-the-loop, +hardware-in-the-loop and in-vehicle tests before each software release. + +Following Hazard and Risk Analysis and FMEA, at a very high level, we have designed openpilot +ensuring two main safety requirements. + +1. The driver must always be capable to immediately retake manual control of the vehicle, + by stepping on the brake pedal or by pressing the cancel button. +2. The vehicle must not alter its trajectory too quickly for the driver to safely + react. This means that while the system is engaged, the actuators are constrained + to operate within reasonable limits[^1]. + +For additional safety implementation details, refer to [panda safety model](https://github.com/commaai/panda#safety-model). For vehicle specific implementation of the safety concept, refer to [panda/board/safety/](https://github.com/commaai/panda/tree/master/board/safety). + +**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or + not fully meeting the above requirements. + +[^1]: For these actuator limits we observe ISO11270 and ISO15622. Lateral limits described there translate to 0.9 seconds of maximum actuation to achieve a 1m lateral deviation. + diff --git a/docs/WORKFLOW.md b/docs/WORKFLOW.md new file mode 100644 index 0000000000..85ae8d86f4 --- /dev/null +++ b/docs/WORKFLOW.md @@ -0,0 +1,42 @@ +# openpilot development workflow + +Aside from the ML models, most tools used for openpilot development are in this repo. + +Most development happens on normal Ubuntu workstations, and not in cars or directly on comma devices. See the [setup guide](../tools) for getting your PC setup for openpilot development. + +## Quick start + +```bash +# get the latest stuff +git pull +git submodule update --init --recursive + +# update dependencies +tools/ubuntu_setup.sh + +# build everything +scons -j$(nproc) + +# build just the ui with either of these +scons -j8 selfdrive/ui/ +cd selfdrive/ui/ && scons -u -j8 + +# test everything +pytest . + +# test just logging services +cd system/loggerd && pytest . + +# run the linter +pre-commit run --all +``` + +## Testing + +### Automated Testing + +All PRs and commits are automatically checked by GitHub Actions. Check out `.github/workflows/` for what GitHub Actions runs. Any new tests should be added to GitHub Actions. + +### Code Style and Linting + +Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`. diff --git a/docs/_static/favicon.ico b/docs/_static/favicon.ico new file mode 100644 index 0000000000..976929954f Binary files /dev/null and b/docs/_static/favicon.ico differ diff --git a/docs/_static/logo.png b/docs/_static/logo.png new file mode 100644 index 0000000000..2699565085 Binary files /dev/null and b/docs/_static/logo.png differ diff --git a/docs/_static/robots.txt b/docs/_static/robots.txt new file mode 100644 index 0000000000..3bcd24fb5d --- /dev/null +++ b/docs/_static/robots.txt @@ -0,0 +1,2 @@ +User-agent: * +Sitemap: https://docs.comma.ai/sitemap.xml \ No newline at end of file diff --git a/docs/assets/icon-star-empty.svg b/docs/assets/icon-star-empty.svg new file mode 100644 index 0000000000..5d3c32d671 --- /dev/null +++ b/docs/assets/icon-star-empty.svg @@ -0,0 +1,56 @@ + + + + + + image/svg+xml + + + + + + + + diff --git a/docs/assets/icon-star-full.svg b/docs/assets/icon-star-full.svg new file mode 100644 index 0000000000..294db2b7f2 --- /dev/null +++ b/docs/assets/icon-star-full.svg @@ -0,0 +1,56 @@ + + + + + + image/svg+xml + + + + + + + + diff --git a/docs/assets/icon-star-half.svg b/docs/assets/icon-star-half.svg new file mode 100644 index 0000000000..ab905fddcb --- /dev/null +++ b/docs/assets/icon-star-half.svg @@ -0,0 +1,66 @@ + + + + + + image/svg+xml + + + + + + + + + + diff --git a/docs/assets/icon-youtube.svg b/docs/assets/icon-youtube.svg new file mode 100644 index 0000000000..4e2c9fdfa9 --- /dev/null +++ b/docs/assets/icon-youtube.svg @@ -0,0 +1,12 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/docs/c_docs.rst b/docs/c_docs.rst new file mode 100644 index 0000000000..3b89fe9874 --- /dev/null +++ b/docs/c_docs.rst @@ -0,0 +1,89 @@ +openpilot +========== + + +opendbc +------ +.. autodoxygenindex:: + :project: opendbc_can + + +cereal +------ + +messaging +^^^^^^^^^ +.. autodoxygenindex:: + :project: msgq_repo_msgq + +visionipc +^^^^^^^^^ +.. autodoxygenindex:: + :project: msgq_repo_msgq_visionipc + + +selfdrive +--------- + +camerad +^^^^^^^ +.. autodoxygenindex:: + :project: system_camerad_cameras + +locationd +^^^^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_locationd + +ui +^^ + +.. autodoxygenindex:: + :project: selfdrive_ui + +replay +"""""" +.. autodoxygenindex:: + :project: tools_replay + +qt +"" +.. autodoxygenindex:: + :project: selfdrive_ui_qt_offroad +.. autodoxygenindex:: + :project: selfdrive_ui_qt_maps + +proclogd +^^^^^^^^ +.. autodoxygenindex:: + :project: system_proclogd + +modeld +^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_modeld_transforms +.. autodoxygenindex:: + :project: selfdrive_modeld_models +.. autodoxygenindex:: + :project: selfdrive_modeld_runners + +common +^^^^^^ +.. autodoxygenindex:: + :project: common + +sensorsd +^^^^^^^^ +.. autodoxygenindex:: + :project: system_sensord_sensors + +pandad +^^^^^^ +.. autodoxygenindex:: + :project: selfdrive_pandad + + +rednose +------- +.. autodoxygenindex:: + :project: rednose_repo_rednose_helpers diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000000..9a8d646697 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,147 @@ +# type: ignore + +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +from os.path import exists + +from openpilot.common.basedir import BASEDIR +from openpilot.system.version import get_version + +sys.path.insert(0, os.path.abspath('.')) +sys.path.insert(0, os.path.abspath('..')) + +VERSION = get_version() + + +# -- Project information ----------------------------------------------------- + +project = 'openpilot docs' +copyright = '2021, comma.ai' # noqa: A001 +author = 'comma.ai' +version = VERSION +release = VERSION +language = 'en' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + 'sphinx.ext.autodoc', # Auto-generate docs + 'sphinx.ext.viewcode', # Add view code link to modules + 'sphinx_rtd_theme', # Read The Docs theme + 'myst_parser', # Markdown parsing + 'breathe', # Doxygen C/C++ integration + 'sphinx_sitemap', # sitemap generation for SEO +] + +myst_html_meta = { + "description": "openpilot docs", + "keywords": "op, openpilot, docs, documentation", + "robots": "all,follow", + "googlebot": "index,follow,snippet,archive", + "property=og:locale": "en_US", + "property=og:site_name": "docs.comma.ai", + "property=og:url": "https://docs.comma.ai", + "property=og:title": "openpilot Documentation", + "property=og:type": "website", + "property=og:image:type": "image/jpeg", + "property=og:image:width": "400", + "property=og:image": "https://docs.comma.ai/_static/logo.png", + "property=og:image:url": "https://docs.comma.ai/_static/logo.png", + "property=og:image:secure_url": "https://docs.comma.ai/_static/logo.png", + "property=og:description": "openpilot Documentation", + "property=twitter:card": "summary_large_image", + "property=twitter:logo": "https://docs.comma.ai/_static/logo.png", + "property=twitter:title": "openpilot Documentation", + "property=twitter:description": "openpilot Documentation" +} + +html_baseurl = 'https://docs.comma.ai/' +sitemap_filename = "sitemap.xml" + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + + +# -- c docs configuration --------------------------------------------------- + +# Breathe Configuration +# breathe_default_project = "c_docs" +breathe_build_directory = f"{BASEDIR}/build/docs/html/xml" +breathe_separate_member_pages = True +breathe_default_members = ('members', 'private-members', 'undoc-members') +breathe_domain_by_extension = { + "h": "cc", +} +breathe_implementation_filename_extensions = ['.c', '.cc'] +breathe_doxygen_config_options = {} +breathe_projects_source = {} + +# only document files that have accompanying .cc files next to them +print("searching for c_docs...") +for root, _, files in os.walk(BASEDIR): + found = False + breath_src = {} + breathe_srcs_list = [] + + for file in files: + ccFile = os.path.join(root, file)[:-2] + ".cc" + + if file.endswith(".h") and exists(ccFile): + f = os.path.join(root, file) + + parent_dir_abs = os.path.dirname(f) + parent_dir = parent_dir_abs[len(BASEDIR) + 1:] + parent_project = parent_dir.replace('/', '_') + print(f"\tFOUND: {f} in {parent_project}") + + breathe_srcs_list.append(file) + found = True + + if found: + breath_src[parent_project] = (parent_dir_abs, breathe_srcs_list) + breathe_projects_source.update(breath_src) + +print(f"breathe_projects_source: {breathe_projects_source.keys()}") + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = 'sphinx_rtd_theme' +html_show_copyright = True + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] +html_logo = '_static/logo.png' +html_favicon = '_static/favicon.ico' +html_theme_options = { + 'logo_only': False, + 'display_version': True, + 'vcs_pageview_mode': 'blob', + 'style_nav_header_background': '#000000', +} +html_extra_path = ['_static'] diff --git a/docs/index.md b/docs/index.md new file mode 100644 index 0000000000..0fb2617a5b --- /dev/null +++ b/docs/index.md @@ -0,0 +1,42 @@ +# openpilot Documentation + +```{include} README.md +``` + +```{toctree} +:caption: 'General' +:maxdepth: 4 + +CARS.md +CONTRIBUTING.md +INTEGRATION.md +LIMITATIONS.md +SAFETY.md +``` + +```{toctree} +:caption: 'Overview' +:maxdepth: 2 + +overview.rst +``` + +## API Documentation + +- {ref}`genindex` +- {ref}`modindex` +- {ref}`search` + +```{toctree} +:caption: 'Python API' +:maxdepth: 2 + +modules.rst +``` + +```{toctree} +:caption: 'C/C++ API' +:maxdepth: 4 + +c_docs.rst +``` \ No newline at end of file diff --git a/docs/overview.rst b/docs/overview.rst new file mode 100644 index 0000000000..8c552077f3 --- /dev/null +++ b/docs/overview.rst @@ -0,0 +1,72 @@ +openpilot +========= + +.. toctree:: + :maxdepth: 4 + + Debugging + system/loggerd/README.md + Driver Monitoring + Process Replay + +cereal +========= + +.. toctree:: + :maxdepth: 4 + + cereal/README.md + cereal/messaging/msgq.md + +models +========= + +.. toctree:: + :maxdepth: 4 + + models/README.md + +opendbc +========= + +.. toctree:: + :maxdepth: 4 + + opendbc/README.md + +panda +========= + +.. toctree:: + :maxdepth: 4 + + panda/README.md + panda/UPDATING.md + panda/board/README.md + panda/drivers/linux/README.md + panda/drivers/windows/README.md + + +rednose +========= +.. toctree:: + :maxdepth: 4 + + rednose_repo/README.md + + +tools +========= +.. toctree:: + :maxdepth: 4 + + tools/CTF.md + tools/joystick/README.md + tools/lib/README.md + tools/plotjuggler/README.md + tools/replay/README.md + tools/serial/README.md + Simulator + tools/ssh/README.md + Webcam + tools/cabana/README.md diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index a7f61411c8..9256f463af 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -82,7 +82,7 @@ function launch { tmux capture-pane -pq -S-1000 > /tmp/launch_log # start manager - cd selfdrive/manager + cd system/manager if [ ! -f $DIR/prebuilt ]; then ./build.py fi diff --git a/launch_env.sh b/launch_env.sh index 6859afb0d4..81578aff01 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="9.7" + export AGNOS_VERSION="10.1" fi export STAGING_ROOT="/data/safe_staging" diff --git a/msgq b/msgq new file mode 120000 index 0000000000..df09146f62 --- /dev/null +++ b/msgq @@ -0,0 +1 @@ +msgq_repo/msgq \ No newline at end of file diff --git a/msgq_repo/.dockerignore b/msgq_repo/.dockerignore new file mode 100644 index 0000000000..5b2d46270d --- /dev/null +++ b/msgq_repo/.dockerignore @@ -0,0 +1 @@ +.sconsign.dblite diff --git a/cereal/.gitignore b/msgq_repo/.gitignore similarity index 97% rename from cereal/.gitignore rename to msgq_repo/.gitignore index d6ec0036ce..6db3c7cd46 100644 --- a/cereal/.gitignore +++ b/msgq_repo/.gitignore @@ -5,6 +5,7 @@ __pycache__ .*.swp .*.swo *.os +*.so *.o *.a diff --git a/msgq_repo/README.md b/msgq_repo/README.md new file mode 100644 index 0000000000..dc10530e0a --- /dev/null +++ b/msgq_repo/README.md @@ -0,0 +1,54 @@ +# MSGQ: A lock free single producer multi consumer message queue + +## What is this library? +MSGQ is a generic high performance IPC pub sub system with a single publisher and multiple subscribers. MSGQ is designed to be a high performance replacement for ZMQ-like SUB/PUB patterns. It uses a ring buffer in shared memory to efficiently read and write data. Each read requires a copy. Writing can be done without a copy, as long as the size of the data is known in advance. While MSGQ is the core of this library, this library also allows replacing the MSGQ backend with ZMQ or a spoofed implementation that can be used for deterministic testing. This library also contains visionipc, an IPC system specifically for large contiguous buffers (like images/video). + +## Storage +The storage for the queue consists of an area of metadata, and the actual buffer. The metadata contains: + +1. A counter to the number of readers that are active +2. A pointer to the head of the queue for writing. From now on referred to as *write pointer* +3. A cycle counter for the writer. This counter is incremented when the writer wraps around +4. N pointers, pointing to the current read position for all the readers. From now on referred to as *read pointer* +5. N counters, counting the number of cycles for all the readers +6. N booleans, indicating validity for all the readers. From now on referred to as *validity flag* + +The counter and the pointer are both 32 bit values, packed into 64 bit so they can be read and written atomically. + +The data buffer is a ring buffer. All messages are prefixed by an 8 byte size field, followed by the data. A size of -1 indicates a wrap-around, and means the next message is stored at the beginning of the buffer. + + +## Writing +Writing involves the following steps: + +1. Check if the area that is to be written overlaps with any of the read pointers, mark those readers as invalid by clearing the validity flag. +2. Write the message +3. Increase the write pointer by the size of the message + +In case there is not enough space at the end of the buffer, a special empty message with a prefix of -1 is written. The cycle counter is incremented by one. In this case step 1 will check there are no read pointers pointing to the remainder of the buffer. Then another write cycle will start with the actual message. + +There always needs to be 8 bytes of empty space at the end of the buffer. By doing this there is always space to write the -1. + +## Reset reader +When the reader is lagging too much behind the read pointer becomes invalid and no longer points to the beginning of a valid message. To reset a reader to the current write pointer, the following steps are performed: + +1. Set valid flag +2. Set read cycle counter to that of the writer +3. Set read pointer to write pointer + +## Reading +Reading involves the following steps: + +1. Read the size field at the current read pointer +2. Read the validity flag +3. Copy the data out of the buffer +4. Increase the read pointer by the size of the message +5. Check the validity flag again + +Before starting the copy, the valid flag is checked. This is to prevent a race condition where the size prefix was invalid, and the read could read outside of the buffer. Make sure that step 1 and 2 are not reordered by your compiler or CPU. + +If a writer overwrites the data while it's being copied out, the data will be invalid. Therefore the validity flag is also checked after reading it. The order of step 4 and 5 does not matter. + +If at steps 2 or 5 the validity flag is not set, the reader is reset. Any data that was already read is discarded. After the reader is reset, the reading starts from the beginning. + +If a message with size -1 is encountered, step 3 and 4 are replaced by increasing the cycle counter and setting the read pointer to the beginning of the buffer. After that another read is performed. diff --git a/msgq_repo/SConscript b/msgq_repo/SConscript new file mode 100644 index 0000000000..147eb30425 --- /dev/null +++ b/msgq_repo/SConscript @@ -0,0 +1,48 @@ +Import('env', 'envCython', 'arch', 'common') + + +visionipc_dir = Dir('msgq/visionipc') +gen_dir = Dir('gen') + + +# Build msgq +msgq_objects = env.SharedObject([ + 'msgq/ipc.cc', + 'msgq/event.cc', + 'msgq/impl_zmq.cc', + 'msgq/impl_msgq.cc', + 'msgq/impl_fake.cc', + 'msgq/msgq.cc', +]) +msgq = env.Library('msgq', msgq_objects) +msgq_python = envCython.Program('msgq/ipc_pyx.so', 'msgq/ipc_pyx.pyx', LIBS=envCython["LIBS"]+[msgq, "zmq", common]) + +# Build Vision IPC +vipc_files = ['visionipc.cc', 'visionipc_server.cc', 'visionipc_client.cc', 'visionbuf.cc'] +vipc_sources = [f'{visionipc_dir.abspath}/{f}' for f in vipc_files] + +if arch == "larch64": + vipc_sources += [f'{visionipc_dir.abspath}/visionbuf_ion.cc'] +else: + vipc_sources += [f'{visionipc_dir.abspath}/visionbuf_cl.cc'] + +vipc_objects = env.SharedObject(vipc_sources) +visionipc = env.Library('visionipc', vipc_objects) + + +vipc_frameworks = [] +vipc_libs = envCython["LIBS"] + [visionipc, msgq, common, "zmq"] +if arch == "Darwin": + vipc_frameworks.append('OpenCL') +else: + vipc_libs.append('OpenCL') +envCython.Program(f'{visionipc_dir.abspath}/visionipc_pyx.so', f'{visionipc_dir.abspath}/visionipc_pyx.pyx', + LIBS=vipc_libs, FRAMEWORKS=vipc_frameworks) + +if GetOption('extras'): + env.Program('msgq/test_runner', ['msgq/test_runner.cc', 'msgq/msgq_tests.cc'], LIBS=[msgq, common]) + env.Program(f'{visionipc_dir.abspath}/test_runner', + [f'{visionipc_dir.abspath}/test_runner.cc', f'{visionipc_dir.abspath}/visionipc_tests.cc'], + LIBS=['pthread'] + vipc_libs, FRAMEWORKS=vipc_frameworks) + +Export('visionipc', 'msgq', 'msgq_python') diff --git a/msgq_repo/SConstruct b/msgq_repo/SConstruct new file mode 100644 index 0000000000..c1e7fc5217 --- /dev/null +++ b/msgq_repo/SConstruct @@ -0,0 +1,90 @@ +import os +import platform +import subprocess +import sysconfig +import numpy as np + +arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() +if platform.system() == "Darwin": + arch = "Darwin" + +common = '' + +cpppath = [ + f"#/", + '#msgq/', + '/usr/lib/include', + '/opt/homebrew/include', + sysconfig.get_paths()['include'], +] + +libpath = [ + '/opt/homebrew/lib', +] + +AddOption('--minimal', + action='store_false', + dest='extras', + default=True, + help='the minimum build. no tests, tools, etc.') + +AddOption('--asan', + action='store_true', + help='turn on ASAN') + +AddOption('--ubsan', + action='store_true', + help='turn on UBSan') + +ccflags = [] +ldflags = [] +if GetOption('ubsan'): + flags = [ + "-fsanitize=undefined", + "-fno-sanitize-recover=undefined", + ] + ccflags += flags + ldflags += flags +elif GetOption('asan'): + ccflags += ["-fsanitize=address", "-fno-omit-frame-pointer"] + ldflags += ["-fsanitize=address"] + +env = Environment( + ENV=os.environ, + CC='clang', + CXX='clang++', + CCFLAGS=[ + "-g", + "-fPIC", + "-O2", + "-Wunused", + "-Werror", + "-Wshadow", + "-Wno-vla-cxx-extension", + ] + ccflags, + LDFLAGS=ldflags, + LINKFLAGS=ldflags, + + CFLAGS="-std=gnu11", + CXXFLAGS="-std=c++1z", + CPPPATH=cpppath, + LIBPATH=libpath, + CYTHONCFILESUFFIX=".cpp", + tools=["default", "cython"] +) + +Export('env', 'arch', 'common') + +envCython = env.Clone(LIBS=[]) +envCython["CPPPATH"] += [np.get_include()] +envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"] +envCython["CCFLAGS"].remove('-Werror') +if arch == "Darwin": + envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] +else: + envCython["LINKFLAGS"] = ["-pthread", "-shared"] + +Export('envCython') + + +SConscript(['SConscript']) diff --git a/msgq_repo/codecov.yml b/msgq_repo/codecov.yml new file mode 100644 index 0000000000..83427c3ee8 --- /dev/null +++ b/msgq_repo/codecov.yml @@ -0,0 +1,8 @@ +comment: false +coverage: + status: + project: + default: + informational: true + patch: off + diff --git a/msgq_repo/msgq/.gitignore b/msgq_repo/msgq/.gitignore new file mode 100644 index 0000000000..6bd751773b --- /dev/null +++ b/msgq_repo/msgq/.gitignore @@ -0,0 +1 @@ +ipc_pyx.cpp diff --git a/msgq_repo/msgq/__init__.py b/msgq_repo/msgq/__init__.py new file mode 100644 index 0000000000..340a1254e3 --- /dev/null +++ b/msgq_repo/msgq/__init__.py @@ -0,0 +1,61 @@ +# must be built with scons +from msgq.ipc_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \ + set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event +from msgq.ipc_pyx import MultiplePublishersError, IpcError + +from typing import Optional, List + +assert MultiplePublishersError +assert IpcError +assert toggle_fake_events +assert set_fake_prefix +assert get_fake_prefix +assert delete_fake_prefix +assert wait_for_one_event + +NO_TRAVERSAL_LIMIT = 2**64-1 + +context = Context() + + +def fake_event_handle(endpoint: str, identifier: Optional[str] = None, override: bool = True, enable: bool = False) -> SocketEventHandle: + identifier = identifier or get_fake_prefix() + handle = SocketEventHandle(endpoint, identifier, override) + if override: + handle.enabled = enable + + return handle + +def pub_sock(endpoint: str) -> PubSocket: + sock = PubSocket() + sock.connect(context, endpoint) + return sock + + +def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1", + conflate: bool = False, timeout: Optional[int] = None) -> SubSocket: + sock = SubSocket() + sock.connect(context, endpoint, addr.encode('utf8'), conflate) + + if timeout is not None: + sock.setTimeout(timeout) + + if poller is not None: + poller.registerSocket(sock) + return sock + +def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]: + """Receive all message currently available on the queue""" + ret: List[bytes] = [] + while 1: + if wait_for_one and len(ret) == 0: + dat = sock.receive() + else: + dat = sock.receive(non_blocking=True) + + if dat is None: + break + + ret.append(dat) + + return ret diff --git a/cereal/messaging/event.cc b/msgq_repo/msgq/event.cc similarity index 99% rename from cereal/messaging/event.cc rename to msgq_repo/msgq/event.cc index a708de915a..be782e3b88 100644 --- a/cereal/messaging/event.cc +++ b/msgq_repo/msgq/event.cc @@ -13,7 +13,7 @@ #include #include -#include "cereal/messaging/event.h" +#include "msgq/event.h" #ifndef __APPLE__ #include diff --git a/cereal/messaging/event.h b/msgq_repo/msgq/event.h similarity index 100% rename from cereal/messaging/event.h rename to msgq_repo/msgq/event.h diff --git a/cereal/messaging/impl_fake.cc b/msgq_repo/msgq/impl_fake.cc similarity index 81% rename from cereal/messaging/impl_fake.cc rename to msgq_repo/msgq/impl_fake.cc index 178b8b7a01..a7607206f4 100644 --- a/cereal/messaging/impl_fake.cc +++ b/msgq_repo/msgq/impl_fake.cc @@ -1,4 +1,4 @@ -#include "cereal/messaging/impl_fake.h" +#include "msgq/impl_fake.h" void FakePoller::registerSocket(SubSocket *socket) { this->sockets.push_back(socket); diff --git a/cereal/messaging/impl_fake.h b/msgq_repo/msgq/impl_fake.h similarity index 95% rename from cereal/messaging/impl_fake.h rename to msgq_repo/msgq/impl_fake.h index 0ec8486a08..2c3b74dc9a 100644 --- a/cereal/messaging/impl_fake.h +++ b/msgq_repo/msgq/impl_fake.h @@ -11,8 +11,8 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/messaging/event.h" +#include "msgq/ipc.h" +#include "msgq/event.h" template class FakeSubSocket: public TSubSocket { diff --git a/cereal/messaging/impl_msgq.cc b/msgq_repo/msgq/impl_msgq.cc similarity index 88% rename from cereal/messaging/impl_msgq.cc rename to msgq_repo/msgq/impl_msgq.cc index 8f2c10a087..b23991351f 100644 --- a/cereal/messaging/impl_msgq.cc +++ b/msgq_repo/msgq/impl_msgq.cc @@ -5,8 +5,7 @@ #include #include -#include "cereal/services.h" -#include "cereal/messaging/impl_msgq.h" +#include "msgq/impl_msgq.h" volatile sig_atomic_t msgq_do_exit = 0; @@ -16,10 +15,6 @@ void sig_handler(int signal) { msgq_do_exit = 1; } -static bool service_exists(std::string path){ - return services.count(path) > 0; -} - MSGQContext::MSGQContext() { } @@ -58,10 +53,6 @@ int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string a assert(context); assert(address == "127.0.0.1"); - if (check_endpoint && !service_exists(std::string(endpoint))){ - std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl; - } - q = new msgq_queue_t; int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE); if (r != 0){ @@ -150,9 +141,10 @@ MSGQSubSocket::~MSGQSubSocket(){ int MSGQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){ assert(context); - if (check_endpoint && !service_exists(std::string(endpoint))){ - std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl; - } + // TODO + //if (check_endpoint && !service_exists(std::string(endpoint))){ + // std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl; + //} q = new msgq_queue_t; int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE); diff --git a/cereal/messaging/impl_msgq.h b/msgq_repo/msgq/impl_msgq.h similarity index 95% rename from cereal/messaging/impl_msgq.h rename to msgq_repo/msgq/impl_msgq.h index 68235f0c6e..4d9db1850f 100644 --- a/cereal/messaging/impl_msgq.h +++ b/msgq_repo/msgq/impl_msgq.h @@ -3,8 +3,8 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/messaging/msgq.h" +#include "msgq/ipc.h" +#include "msgq/msgq.h" #define MAX_POLLERS 128 diff --git a/cereal/messaging/impl_zmq.cc b/msgq_repo/msgq/impl_zmq.cc similarity index 90% rename from cereal/messaging/impl_zmq.cc rename to msgq_repo/msgq/impl_zmq.cc index 7da9df1b8f..434888eb8f 100644 --- a/cereal/messaging/impl_zmq.cc +++ b/msgq_repo/msgq/impl_zmq.cc @@ -5,11 +5,16 @@ #include #include -#include "cereal/services.h" -#include "cereal/messaging/impl_zmq.h" +#include "msgq/impl_zmq.h" +//FIXME: This is a hack to get the port number from the socket name, might have collisions static int get_port(std::string endpoint) { - return services.at(endpoint).port; + std::hash hasher; + size_t hash_value = hasher(endpoint); + int start_port = 8023; + int max_port = 65535; + int port = start_port + (hash_value % (max_port - start_port)); + return port; } ZMQContext::ZMQContext() { @@ -59,6 +64,7 @@ int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string ad int reconnect_ivl = 500; zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl)); + full_endpoint = "tcp://" + address + ":"; if (check_endpoint){ full_endpoint += std::to_string(get_port(endpoint)); diff --git a/cereal/messaging/impl_zmq.h b/msgq_repo/msgq/impl_zmq.h similarity index 97% rename from cereal/messaging/impl_zmq.h rename to msgq_repo/msgq/impl_zmq.h index 903875f630..718196e306 100644 --- a/cereal/messaging/impl_zmq.h +++ b/msgq_repo/msgq/impl_zmq.h @@ -4,7 +4,7 @@ #include #include -#include "cereal/messaging/messaging.h" +#include "msgq/ipc.h" #define MAX_POLLERS 128 diff --git a/cereal/messaging/messaging.cc b/msgq_repo/msgq/ipc.cc similarity index 93% rename from cereal/messaging/messaging.cc rename to msgq_repo/msgq/ipc.cc index 6b7fe8f901..f620f46fad 100644 --- a/cereal/messaging/messaging.cc +++ b/msgq_repo/msgq/ipc.cc @@ -1,10 +1,10 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/messaging/impl_zmq.h" -#include "cereal/messaging/impl_msgq.h" -#include "cereal/messaging/impl_fake.h" +#include "msgq/ipc.h" +#include "msgq/impl_zmq.h" +#include "msgq/impl_msgq.h" +#include "msgq/impl_fake.h" #ifdef __APPLE__ const bool MUST_USE_ZMQ = true; diff --git a/msgq_repo/msgq/ipc.h b/msgq_repo/msgq/ipc.h new file mode 100644 index 0000000000..1f0e3e06b5 --- /dev/null +++ b/msgq_repo/msgq/ipc.h @@ -0,0 +1,68 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + + + +#ifdef __APPLE__ +#define CLOCK_BOOTTIME CLOCK_MONOTONIC +#endif + +#define MSG_MULTIPLE_PUBLISHERS 100 + +bool messaging_use_zmq(); + +class Context { +public: + virtual void * getRawContext() = 0; + static Context * create(); + virtual ~Context(){} +}; + +class Message { +public: + virtual void init(size_t size) = 0; + virtual void init(char * data, size_t size) = 0; + virtual void close() = 0; + virtual size_t getSize() = 0; + virtual char * getData() = 0; + virtual ~Message(){} +}; + + +class SubSocket { +public: + virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) = 0; + virtual void setTimeout(int timeout) = 0; + virtual Message *receive(bool non_blocking=false) = 0; + virtual void * getRawSocket() = 0; + static SubSocket * create(); + static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true); + virtual ~SubSocket(){} +}; + +class PubSocket { +public: + virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0; + virtual int sendMessage(Message *message) = 0; + virtual int send(char *data, size_t size) = 0; + virtual bool all_readers_updated() = 0; + static PubSocket * create(); + static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true); + static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true); + virtual ~PubSocket(){} +}; + +class Poller { +public: + virtual void registerSocket(SubSocket *socket) = 0; + virtual std::vector poll(int timeout) = 0; + static Poller * create(); + static Poller * create(std::vector sockets); + virtual ~Poller(){} +}; \ No newline at end of file diff --git a/cereal/messaging/messaging.pxd b/msgq_repo/msgq/ipc.pxd similarity index 93% rename from cereal/messaging/messaging.pxd rename to msgq_repo/msgq/ipc.pxd index 97b9a22b0b..2c7ac963ec 100644 --- a/cereal/messaging/messaging.pxd +++ b/msgq_repo/msgq/ipc.pxd @@ -6,7 +6,7 @@ from libcpp.vector cimport vector from libcpp cimport bool -cdef extern from "cereal/messaging/impl_fake.h": +cdef extern from "msgq/impl_fake.h": cdef cppclass Event: @staticmethod int wait_for_one(vector[Event], int) except + @@ -34,7 +34,7 @@ cdef extern from "cereal/messaging/impl_fake.h": Event recv_ready() -cdef extern from "cereal/messaging/messaging.h": +cdef extern from "msgq/ipc.h": cdef cppclass Context: @staticmethod Context * create() diff --git a/cereal/messaging/messaging_pyx.pyx b/msgq_repo/msgq/ipc_pyx.pyx similarity index 89% rename from cereal/messaging/messaging_pyx.pyx rename to msgq_repo/msgq/ipc_pyx.pyx index 8216aeea99..d8797f3959 100644 --- a/cereal/messaging/messaging_pyx.pyx +++ b/msgq_repo/msgq/ipc_pyx.pyx @@ -10,22 +10,22 @@ from libc.string cimport strerror from cython.operator import dereference -from .messaging cimport Context as cppContext -from .messaging cimport SubSocket as cppSubSocket -from .messaging cimport PubSocket as cppPubSocket -from .messaging cimport Poller as cppPoller -from .messaging cimport Message as cppMessage -from .messaging cimport Event as cppEvent, SocketEventHandle as cppSocketEventHandle +from .ipc cimport Context as cppContext +from .ipc cimport SubSocket as cppSubSocket +from .ipc cimport PubSocket as cppPubSocket +from .ipc cimport Poller as cppPoller +from .ipc cimport Message as cppMessage +from .ipc cimport Event as cppEvent, SocketEventHandle as cppSocketEventHandle -class MessagingError(Exception): +class IpcError(Exception): def __init__(self, endpoint=None): suffix = f"with {endpoint.decode('utf-8')}" if endpoint else "" message = f"Messaging failure {suffix}: {strerror(errno.errno).decode('utf-8')}" super().__init__(message) -class MultiplePublishersError(MessagingError): +class MultiplePublishersError(IpcError): pass @@ -170,7 +170,7 @@ cdef class SubSocket: self.is_owner = True if self.socket == NULL: - raise MessagingError + raise IpcError def __dealloc__(self): if self.is_owner: @@ -190,7 +190,7 @@ cdef class SubSocket: if errno.errno == errno.EADDRINUSE: raise MultiplePublishersError(endpoint) else: - raise MessagingError(endpoint) + raise IpcError(endpoint) def setTimeout(self, int timeout): self.socket.setTimeout(timeout) @@ -219,7 +219,7 @@ cdef class PubSocket: def __cinit__(self): self.socket = cppPubSocket.create() if self.socket == NULL: - raise MessagingError + raise IpcError def __dealloc__(self): del self.socket @@ -231,7 +231,7 @@ cdef class PubSocket: if errno.errno == errno.EADDRINUSE: raise MultiplePublishersError(endpoint) else: - raise MessagingError(endpoint) + raise IpcError(endpoint) def send(self, bytes data): length = len(data) @@ -241,7 +241,7 @@ cdef class PubSocket: if errno.errno == errno.EADDRINUSE: raise MultiplePublishersError else: - raise MessagingError + raise IpcError def all_readers_updated(self): return self.socket.all_readers_updated() diff --git a/cereal/logger/logger.h b/msgq_repo/msgq/logger/logger.h similarity index 100% rename from cereal/logger/logger.h rename to msgq_repo/msgq/logger/logger.h diff --git a/cereal/messaging/msgq.cc b/msgq_repo/msgq/msgq.cc similarity index 99% rename from cereal/messaging/msgq.cc rename to msgq_repo/msgq/msgq.cc index af93bbf60b..fed2959bd3 100644 --- a/cereal/messaging/msgq.cc +++ b/msgq_repo/msgq/msgq.cc @@ -23,7 +23,7 @@ #include -#include "cereal/messaging/msgq.h" +#include "msgq/msgq.h" void sigusr2_handler(int signal) { assert(signal == SIGUSR2); diff --git a/cereal/messaging/msgq.h b/msgq_repo/msgq/msgq.h similarity index 98% rename from cereal/messaging/msgq.h rename to msgq_repo/msgq/msgq.h index 0a72a3864b..94e1849442 100644 --- a/cereal/messaging/msgq.h +++ b/msgq_repo/msgq/msgq.h @@ -6,7 +6,7 @@ #include #define DEFAULT_SEGMENT_SIZE (10 * 1024 * 1024) -#define NUM_READERS 12 +#define NUM_READERS 15 #define ALIGN(n) ((n + (8 - 1)) & -8) #define UNUSED(x) (void)x diff --git a/msgq_repo/msgq/msgq_tests.cc b/msgq_repo/msgq/msgq_tests.cc new file mode 100644 index 0000000000..02f17917a5 --- /dev/null +++ b/msgq_repo/msgq/msgq_tests.cc @@ -0,0 +1,424 @@ +#include "catch2/catch.hpp" +#include "msgq/msgq.h" + +TEST_CASE("ALIGN") +{ + REQUIRE(ALIGN(0) == 0); + REQUIRE(ALIGN(1) == 8); + REQUIRE(ALIGN(7) == 8); + REQUIRE(ALIGN(8) == 8); + REQUIRE(ALIGN(99999) == 100000); +} + +TEST_CASE("msgq_msg_init_size") +{ + const size_t msg_size = 30; + msgq_msg_t msg; + + msgq_msg_init_size(&msg, msg_size); + REQUIRE(msg.size == msg_size); + + msgq_msg_close(&msg); +} + +TEST_CASE("msgq_msg_init_data") +{ + const size_t msg_size = 30; + char *data = new char[msg_size]; + + for (size_t i = 0; i < msg_size; i++) + { + data[i] = i; + } + + msgq_msg_t msg; + msgq_msg_init_data(&msg, data, msg_size); + + REQUIRE(msg.size == msg_size); + REQUIRE(memcmp(msg.data, data, msg_size) == 0); + + delete[] data; + msgq_msg_close(&msg); +} + +TEST_CASE("msgq_init_subscriber") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q; + msgq_new_queue(&q, "test_queue", 1024); + REQUIRE(*q.num_readers == 0); + + q.reader_id = 1; + *q.read_valids[0] = false; + *q.read_pointers[0] = ((uint64_t)1 << 32); + + *q.write_pointer = 255; + + msgq_init_subscriber(&q); + REQUIRE(q.read_conflate == false); + REQUIRE(*q.read_valids[0] == true); + REQUIRE((*q.read_pointers[0] >> 32) == 0); + REQUIRE((*q.read_pointers[0] & 0xFFFFFFFF) == 255); +} + +TEST_CASE("msgq_msg_send first message") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q; + msgq_new_queue(&q, "test_queue", 1024); + msgq_init_publisher(&q); + + REQUIRE(*q.write_pointer == 0); + + size_t msg_size = 128; + + SECTION("Aligned message size") + { + } + SECTION("Unaligned message size") + { + msg_size--; + } + char *data = new char[msg_size]; + + for (size_t i = 0; i < msg_size; i++) + { + data[i] = i; + } + + msgq_msg_t msg; + msgq_msg_init_data(&msg, data, msg_size); + + msgq_msg_send(&msg, &q); + REQUIRE(*(int64_t *)q.data == msg_size); // Check size tag + REQUIRE(*q.write_pointer == 128 + sizeof(int64_t)); + REQUIRE(memcmp(q.data + sizeof(int64_t), data, msg_size) == 0); + + delete[] data; + msgq_msg_close(&msg); +} + +TEST_CASE("msgq_msg_send test wraparound") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q; + msgq_new_queue(&q, "test_queue", 1024); + msgq_init_publisher(&q); + + REQUIRE((*q.write_pointer & 0xFFFFFFFF) == 0); + REQUIRE((*q.write_pointer >> 32) == 0); + + const size_t msg_size = 120; + msgq_msg_t msg; + msgq_msg_init_size(&msg, msg_size); + + for (int i = 0; i < 8; i++) + { + msgq_msg_send(&msg, &q); + } + // Check 8th message was written at the beginning + REQUIRE((*q.write_pointer & 0xFFFFFFFF) == msg_size + sizeof(int64_t)); + + // Check cycle count + REQUIRE((*q.write_pointer >> 32) == 1); + + // Check wraparound tag + char *tag_location = q.data; + tag_location += 7 * (msg_size + sizeof(int64_t)); + REQUIRE(*(int64_t *)tag_location == -1); + + msgq_msg_close(&msg); +} + +TEST_CASE("msgq_msg_recv test wraparound") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q_pub, q_sub; + msgq_new_queue(&q_pub, "test_queue", 1024); + msgq_new_queue(&q_sub, "test_queue", 1024); + + msgq_init_publisher(&q_pub); + msgq_init_subscriber(&q_sub); + + REQUIRE((*q_pub.write_pointer >> 32) == 0); + REQUIRE((*q_sub.read_pointers[0] >> 32) == 0); + + const size_t msg_size = 120; + msgq_msg_t msg1; + msgq_msg_init_size(&msg1, msg_size); + + SECTION("Check cycle counter after reset") + { + for (int i = 0; i < 8; i++) + { + msgq_msg_send(&msg1, &q_pub); + } + + msgq_msg_t msg2; + msgq_msg_recv(&msg2, &q_sub); + REQUIRE(msg2.size == 0); // Reader had to reset + msgq_msg_close(&msg2); + } + SECTION("Check cycle counter while keeping up with writer") + { + for (int i = 0; i < 8; i++) + { + msgq_msg_send(&msg1, &q_pub); + + msgq_msg_t msg2; + msgq_msg_recv(&msg2, &q_sub); + REQUIRE(msg2.size > 0); + msgq_msg_close(&msg2); + } + } + + REQUIRE((*q_sub.read_pointers[0] >> 32) == 1); + msgq_msg_close(&msg1); +} + +TEST_CASE("msgq_msg_send test invalidation") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q_pub, q_sub; + msgq_new_queue(&q_pub, "test_queue", 1024); + msgq_new_queue(&q_sub, "test_queue", 1024); + + msgq_init_publisher(&q_pub); + msgq_init_subscriber(&q_sub); + *q_sub.write_pointer = (uint64_t)1 << 32; + + REQUIRE(*q_sub.read_valids[0] == true); + + SECTION("read pointer in tag") + { + *q_sub.read_pointers[0] = 0; + } + SECTION("read pointer in data section") + { + *q_sub.read_pointers[0] = 64; + } + SECTION("read pointer in wraparound section") + { + *q_pub.write_pointer = ((uint64_t)1 << 32) | 1000; // Writer is one cycle ahead + *q_sub.read_pointers[0] = 1020; + } + + msgq_msg_t msg; + msgq_msg_init_size(&msg, 128); + msgq_msg_send(&msg, &q_pub); + + REQUIRE(*q_sub.read_valids[0] == false); + + msgq_msg_close(&msg); +} + +TEST_CASE("msgq_init_subscriber init 2 subscribers") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t q1, q2; + msgq_new_queue(&q1, "test_queue", 1024); + msgq_new_queue(&q2, "test_queue", 1024); + + *q1.num_readers = 0; + + REQUIRE(*q1.num_readers == 0); + REQUIRE(*q2.num_readers == 0); + + msgq_init_subscriber(&q1); + REQUIRE(*q1.num_readers == 1); + REQUIRE(*q2.num_readers == 1); + REQUIRE(q1.reader_id == 0); + + msgq_init_subscriber(&q2); + REQUIRE(*q1.num_readers == 2); + REQUIRE(*q2.num_readers == 2); + REQUIRE(q2.reader_id == 1); +} + +TEST_CASE("Write 1 msg, read 1 msg", "[integration]") +{ + remove("/dev/shm/test_queue"); + const size_t msg_size = 128; + msgq_queue_t writer, reader; + + msgq_new_queue(&writer, "test_queue", 1024); + msgq_new_queue(&reader, "test_queue", 1024); + + msgq_init_publisher(&writer); + msgq_init_subscriber(&reader); + + // Build 128 byte message + msgq_msg_t outgoing_msg; + msgq_msg_init_size(&outgoing_msg, msg_size); + + for (size_t i = 0; i < msg_size; i++) + { + outgoing_msg.data[i] = i; + } + + REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); + + msgq_msg_t incoming_msg1; + REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); + REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); + + // Verify that there are no more messages + msgq_msg_t incoming_msg2; + REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); + + msgq_msg_close(&outgoing_msg); + msgq_msg_close(&incoming_msg1); + msgq_msg_close(&incoming_msg2); +} + +TEST_CASE("Write 2 msg, read 2 msg - conflate = false", "[integration]") +{ + remove("/dev/shm/test_queue"); + const size_t msg_size = 128; + msgq_queue_t writer, reader; + + msgq_new_queue(&writer, "test_queue", 1024); + msgq_new_queue(&reader, "test_queue", 1024); + + msgq_init_publisher(&writer); + msgq_init_subscriber(&reader); + + // Build 128 byte message + msgq_msg_t outgoing_msg; + msgq_msg_init_size(&outgoing_msg, msg_size); + + for (size_t i = 0; i < msg_size; i++) + { + outgoing_msg.data[i] = i; + } + + REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); + REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); + + msgq_msg_t incoming_msg1; + REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); + REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); + + msgq_msg_t incoming_msg2; + REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == msg_size); + REQUIRE(memcmp(incoming_msg2.data, outgoing_msg.data, msg_size) == 0); + + msgq_msg_close(&outgoing_msg); + msgq_msg_close(&incoming_msg1); + msgq_msg_close(&incoming_msg2); +} + +TEST_CASE("Write 2 msg, read 2 msg - conflate = true", "[integration]") +{ + remove("/dev/shm/test_queue"); + const size_t msg_size = 128; + msgq_queue_t writer, reader; + + msgq_new_queue(&writer, "test_queue", 1024); + msgq_new_queue(&reader, "test_queue", 1024); + + msgq_init_publisher(&writer); + msgq_init_subscriber(&reader); + reader.read_conflate = true; + + // Build 128 byte message + msgq_msg_t outgoing_msg; + msgq_msg_init_size(&outgoing_msg, msg_size); + + for (size_t i = 0; i < msg_size; i++) + { + outgoing_msg.data[i] = i; + } + + REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); + REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size); + + msgq_msg_t incoming_msg1; + REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size); + REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0); + + // Verify that there are no more messages + msgq_msg_t incoming_msg2; + REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0); + + msgq_msg_close(&outgoing_msg); + msgq_msg_close(&incoming_msg1); + msgq_msg_close(&incoming_msg2); +} + +TEST_CASE("1 publisher, 1 slow subscriber", "[integration]") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t writer, reader; + + msgq_new_queue(&writer, "test_queue", 1024); + msgq_new_queue(&reader, "test_queue", 1024); + + msgq_init_publisher(&writer); + msgq_init_subscriber(&reader); + + int n_received = 0; + int n_skipped = 0; + + for (uint64_t i = 0; i < 1e5; i++) + { + msgq_msg_t outgoing_msg; + msgq_msg_init_data(&outgoing_msg, (char *)&i, sizeof(uint64_t)); + msgq_msg_send(&outgoing_msg, &writer); + msgq_msg_close(&outgoing_msg); + + if (i % 10 == 0) + { + msgq_msg_t msg1; + msgq_msg_recv(&msg1, &reader); + + if (msg1.size == 0) + { + n_skipped++; + } + else + { + n_received++; + } + msgq_msg_close(&msg1); + } + } + + // TODO: verify these numbers by hand + REQUIRE(n_received == 8572); + REQUIRE(n_skipped == 1428); +} + +TEST_CASE("1 publisher, 2 subscribers", "[integration]") +{ + remove("/dev/shm/test_queue"); + msgq_queue_t writer, reader1, reader2; + + msgq_new_queue(&writer, "test_queue", 1024); + msgq_new_queue(&reader1, "test_queue", 1024); + msgq_new_queue(&reader2, "test_queue", 1024); + + msgq_init_publisher(&writer); + msgq_init_subscriber(&reader1); + msgq_init_subscriber(&reader2); + + for (uint64_t i = 0; i < 1024 * 3; i++) + { + msgq_msg_t outgoing_msg; + msgq_msg_init_data(&outgoing_msg, (char *)&i, sizeof(uint64_t)); + msgq_msg_send(&outgoing_msg, &writer); + + msgq_msg_t msg1, msg2; + msgq_msg_recv(&msg1, &reader1); + msgq_msg_recv(&msg2, &reader2); + + REQUIRE(msg1.size == sizeof(uint64_t)); + REQUIRE(msg2.size == sizeof(uint64_t)); + REQUIRE(*(uint64_t *)msg1.data == i); + REQUIRE(*(uint64_t *)msg2.data == i); + + msgq_msg_close(&outgoing_msg); + msgq_msg_close(&msg1); + msgq_msg_close(&msg2); + } +} diff --git a/cereal/visionipc/test_runner.cc b/msgq_repo/msgq/test_runner.cc similarity index 100% rename from cereal/visionipc/test_runner.cc rename to msgq_repo/msgq/test_runner.cc diff --git a/selfdrive/athena/__init__.py b/msgq_repo/msgq/tests/__init__.py similarity index 100% rename from selfdrive/athena/__init__.py rename to msgq_repo/msgq/tests/__init__.py diff --git a/msgq_repo/msgq/tests/test_fake.py b/msgq_repo/msgq/tests/test_fake.py new file mode 100644 index 0000000000..b5ed297ab1 --- /dev/null +++ b/msgq_repo/msgq/tests/test_fake.py @@ -0,0 +1,192 @@ +import os +import unittest +import multiprocessing +import platform +import msgq +from parameterized import parameterized_class +from typing import Optional + +WAIT_TIMEOUT = 5 + + +@unittest.skipIf(platform.system() == "Darwin", "Events not supported on macOS") +class TestEvents(unittest.TestCase): + + def test_mutation(self): + handle = msgq.fake_event_handle("carState") + event = handle.recv_called_event + + self.assertFalse(event.peek()) + event.set() + self.assertTrue(event.peek()) + event.clear() + self.assertFalse(event.peek()) + + del event + + def test_wait(self): + handle = msgq.fake_event_handle("carState") + event = handle.recv_called_event + + event.set() + try: + event.wait(WAIT_TIMEOUT) + self.assertTrue(event.peek()) + except RuntimeError: + self.fail("event.wait() timed out") + + def test_wait_multiprocess(self): + handle = msgq.fake_event_handle("carState") + event = handle.recv_called_event + + def set_event_run(): + event.set() + + try: + p = multiprocessing.Process(target=set_event_run) + p.start() + event.wait(WAIT_TIMEOUT) + self.assertTrue(event.peek()) + except RuntimeError: + self.fail("event.wait() timed out") + + p.kill() + + def test_wait_zero_timeout(self): + handle = msgq.fake_event_handle("carState") + event = handle.recv_called_event + + try: + event.wait(0) + self.fail("event.wait() did not time out") + except RuntimeError: + self.assertFalse(event.peek()) + + +@unittest.skipIf(platform.system() == "Darwin", "FakeSockets not supported on macOS") +@unittest.skipIf("ZMQ" in os.environ, "FakeSockets not supported on ZMQ") +@parameterized_class([{"prefix": None}, {"prefix": "test"}]) +class TestFakeSockets(unittest.TestCase): + prefix: Optional[str] = None + + def setUp(self): + msgq.toggle_fake_events(True) + if self.prefix is not None: + msgq.set_fake_prefix(self.prefix) + else: + msgq.delete_fake_prefix() + + def tearDown(self): + msgq.toggle_fake_events(False) + msgq.delete_fake_prefix() + + def test_event_handle_init(self): + handle = msgq.fake_event_handle("controlsState", override=True) + + self.assertFalse(handle.enabled) + self.assertGreaterEqual(handle.recv_called_event.fd, 0) + self.assertGreaterEqual(handle.recv_ready_event.fd, 0) + + def test_non_managed_socket_state(self): + # non managed socket should have zero state + _ = msgq.pub_sock("ubloxGnss") + + handle = msgq.fake_event_handle("ubloxGnss", override=False) + + self.assertFalse(handle.enabled) + self.assertEqual(handle.recv_called_event.fd, 0) + self.assertEqual(handle.recv_ready_event.fd, 0) + + def test_managed_socket_state(self): + # managed socket should not change anything about the state + handle = msgq.fake_event_handle("ubloxGnss") + handle.enabled = True + + expected_enabled = handle.enabled + expected_recv_called_fd = handle.recv_called_event.fd + expected_recv_ready_fd = handle.recv_ready_event.fd + + _ = msgq.pub_sock("ubloxGnss") + + self.assertEqual(handle.enabled, expected_enabled) + self.assertEqual(handle.recv_called_event.fd, expected_recv_called_fd) + self.assertEqual(handle.recv_ready_event.fd, expected_recv_ready_fd) + + def test_sockets_enable_disable(self): + carState_handle = msgq.fake_event_handle("ubloxGnss", enable=True) + recv_called = carState_handle.recv_called_event + recv_ready = carState_handle.recv_ready_event + + pub_sock = msgq.pub_sock("ubloxGnss") + sub_sock = msgq.sub_sock("ubloxGnss") + + try: + carState_handle.enabled = True + recv_ready.set() + pub_sock.send(b"test") + _ = sub_sock.receive() + self.assertTrue(recv_called.peek()) + recv_called.clear() + + carState_handle.enabled = False + recv_ready.set() + pub_sock.send(b"test") + _ = sub_sock.receive() + self.assertFalse(recv_called.peek()) + except RuntimeError: + self.fail("event.wait() timed out") + + def test_synced_pub_sub(self): + def daemon_repub_process_run(): + pub_sock = msgq.pub_sock("ubloxGnss") + sub_sock = msgq.sub_sock("carState") + + frame = -1 + while True: + frame += 1 + msg = sub_sock.receive(non_blocking=True) + if msg is None: + print("none received") + continue + + bts = frame.to_bytes(8, 'little') + pub_sock.send(bts) + + carState_handle = msgq.fake_event_handle("carState", enable=True) + recv_called = carState_handle.recv_called_event + recv_ready = carState_handle.recv_ready_event + + p = multiprocessing.Process(target=daemon_repub_process_run) + p.start() + + pub_sock = msgq.pub_sock("carState") + sub_sock = msgq.sub_sock("ubloxGnss") + + try: + for i in range(10): + recv_called.wait(WAIT_TIMEOUT) + recv_called.clear() + + if i == 0: + sub_sock.receive(non_blocking=True) + + bts = i.to_bytes(8, 'little') + pub_sock.send(bts) + + recv_ready.set() + recv_called.wait(WAIT_TIMEOUT) + + msg = sub_sock.receive(non_blocking=True) + self.assertIsNotNone(msg) + self.assertEqual(len(msg), 8) + + frame = int.from_bytes(msg, 'little') + self.assertEqual(frame, i) + except RuntimeError: + self.fail("event.wait() timed out") + finally: + p.kill() + + +if __name__ == "__main__": + unittest.main() diff --git a/msgq_repo/msgq/tests/test_messaging.py b/msgq_repo/msgq/tests/test_messaging.py new file mode 100755 index 0000000000..bbeeb3d848 --- /dev/null +++ b/msgq_repo/msgq/tests/test_messaging.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 +import os +import random +import threading +import time +import string +import unittest +import msgq + + +def random_sock(): + return ''.join(random.choices(string.ascii_uppercase + string.digits, k=10)) + +def random_bytes(length=1000): + return bytes([random.randrange(0xFF) for _ in range(length)]) + +def zmq_sleep(t=1): + if "ZMQ" in os.environ: + time.sleep(t) + +def zmq_expected_failure(func): + if "ZMQ" in os.environ: + return unittest.expectedFailure(func) + else: + return func + +def delayed_send(delay, sock, dat): + def send_func(): + sock.send(dat) + threading.Timer(delay, send_func).start() + +class TestPubSubSockets(unittest.TestCase): + + def setUp(self): + # ZMQ pub socket takes too long to die + # sleep to prevent multiple publishers error between tests + zmq_sleep() + + def test_pub_sub(self): + sock = random_sock() + pub_sock = msgq.pub_sock(sock) + sub_sock = msgq.sub_sock(sock, conflate=False, timeout=None) + zmq_sleep(3) + + for _ in range(1000): + msg = random_bytes() + pub_sock.send(msg) + recvd = sub_sock.receive() + self.assertEqual(msg, recvd) + + def test_conflate(self): + sock = random_sock() + pub_sock = msgq.pub_sock(sock) + for conflate in [True, False]: + for _ in range(10): + num_msgs = random.randint(3, 10) + sub_sock = msgq.sub_sock(sock, conflate=conflate, timeout=None) + zmq_sleep() + + sent_msgs = [] + for __ in range(num_msgs): + msg = random_bytes() + pub_sock.send(msg) + sent_msgs.append(msg) + time.sleep(0.1) + recvd_msgs = msgq.drain_sock_raw(sub_sock) + if conflate: + self.assertEqual(len(recvd_msgs), 1) + else: + # TODO: compare actual data + self.assertEqual(len(recvd_msgs), len(sent_msgs)) + + def test_receive_timeout(self): + sock = random_sock() + for _ in range(10): + timeout = random.randrange(200) + sub_sock = msgq.sub_sock(sock, timeout=timeout) + zmq_sleep() + + start_time = time.monotonic() + recvd = sub_sock.receive() + self.assertLess(time.monotonic() - start_time, 0.2) + assert recvd is None + + +if __name__ == "__main__": + unittest.main() diff --git a/msgq_repo/msgq/tests/test_poller.py b/msgq_repo/msgq/tests/test_poller.py new file mode 100644 index 0000000000..a68ff4fe7d --- /dev/null +++ b/msgq_repo/msgq/tests/test_poller.py @@ -0,0 +1,142 @@ +import unittest +import time +import msgq +import concurrent.futures + +SERVICE_NAME = 'myService' + +def poller(): + context = msgq.Context() + + p = msgq.Poller() + + sub = msgq.SubSocket() + sub.connect(context, SERVICE_NAME) + p.registerSocket(sub) + + socks = p.poll(10000) + r = [s.receive(non_blocking=True) for s in socks] + + return r + + +class TestPoller(unittest.TestCase): + def test_poll_once(self): + context = msgq.Context() + + pub = msgq.PubSocket() + pub.connect(context, SERVICE_NAME) + + with concurrent.futures.ThreadPoolExecutor() as e: + poll = e.submit(poller) + + time.sleep(0.1) # Slow joiner syndrome + + # Send message + pub.send(b"a") + + # Wait for poll result + result = poll.result() + + del pub + context.term() + + self.assertEqual(result, [b"a"]) + + def test_poll_and_create_many_subscribers(self): + context = msgq.Context() + + pub = msgq.PubSocket() + pub.connect(context, SERVICE_NAME) + + with concurrent.futures.ThreadPoolExecutor() as e: + poll = e.submit(poller) + + time.sleep(0.1) # Slow joiner syndrome + c = msgq.Context() + for _ in range(10): + msgq.SubSocket().connect(c, SERVICE_NAME) + + time.sleep(0.1) + + # Send message + pub.send(b"a") + + # Wait for poll result + result = poll.result() + + del pub + context.term() + + self.assertEqual(result, [b"a"]) + + def test_multiple_publishers_exception(self): + context = msgq.Context() + + with self.assertRaises(msgq.MultiplePublishersError): + pub1 = msgq.PubSocket() + pub1.connect(context, SERVICE_NAME) + + pub2 = msgq.PubSocket() + pub2.connect(context, SERVICE_NAME) + + pub1.send(b"a") + + del pub1 + del pub2 + context.term() + + def test_multiple_messages(self): + context = msgq.Context() + + pub = msgq.PubSocket() + pub.connect(context, SERVICE_NAME) + + sub = msgq.SubSocket() + sub.connect(context, SERVICE_NAME) + + time.sleep(0.1) # Slow joiner + + for i in range(1, 100): + pub.send(b'a'*i) + + msg_seen = False + i = 1 + while True: + r = sub.receive(non_blocking=True) + + if r is not None: + self.assertEqual(b'a'*i, r) + + msg_seen = True + i += 1 + + if r is None and msg_seen: # ZMQ sometimes receives nothing on the first receive + break + + del pub + del sub + context.term() + + def test_conflate(self): + context = msgq.Context() + + pub = msgq.PubSocket() + pub.connect(context, SERVICE_NAME) + + sub = msgq.SubSocket() + sub.connect(context, SERVICE_NAME, conflate=True) + + time.sleep(0.1) # Slow joiner + pub.send(b'a') + pub.send(b'b') + + self.assertEqual(b'b', sub.receive()) + + del pub + del sub + context.term() + + +if __name__ == "__main__": + unittest.main() diff --git a/cereal/visionipc/.gitignore b/msgq_repo/msgq/visionipc/.gitignore similarity index 100% rename from cereal/visionipc/.gitignore rename to msgq_repo/msgq/visionipc/.gitignore diff --git a/msgq_repo/msgq/visionipc/__init__.py b/msgq_repo/msgq/visionipc/__init__.py new file mode 100644 index 0000000000..57011537e4 --- /dev/null +++ b/msgq_repo/msgq/visionipc/__init__.py @@ -0,0 +1,6 @@ +from msgq.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name +assert VisionBuf +assert VisionIpcClient +assert VisionIpcServer +assert VisionStreamType +assert get_endpoint_name diff --git a/msgq_repo/msgq/visionipc/test_runner.cc b/msgq_repo/msgq/visionipc/test_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/msgq_repo/msgq/visionipc/test_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/selfdrive/boardd/__init__.py b/msgq_repo/msgq/visionipc/tests/__init__.py similarity index 100% rename from selfdrive/boardd/__init__.py rename to msgq_repo/msgq/visionipc/tests/__init__.py diff --git a/msgq_repo/msgq/visionipc/tests/test_visionipc.py b/msgq_repo/msgq/visionipc/tests/test_visionipc.py new file mode 100755 index 0000000000..1c34613dde --- /dev/null +++ b/msgq_repo/msgq/visionipc/tests/test_visionipc.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +import os +import time +import random +import unittest +import numpy as np +from msgq.visionipc import VisionIpcServer, VisionIpcClient, VisionStreamType + +def zmq_sleep(t=1): + if "ZMQ" in os.environ: + time.sleep(t) + + +class TestVisionIpc(unittest.TestCase): + + def setup_vipc(self, name, *stream_types, num_buffers=1, rgb=False, width=100, height=100, conflate=False): + self.server = VisionIpcServer(name) + for stream_type in stream_types: + self.server.create_buffers(stream_type, num_buffers, rgb, width, height) + self.server.start_listener() + + if len(stream_types): + self.client = VisionIpcClient(name, stream_types[0], conflate) + self.assertTrue(self.client.connect(True)) + else: + self.client = None + + zmq_sleep() + return self.server, self.client + + def test_connect(self): + self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD) + self.assertTrue(self.client.is_connected) + + def test_available_streams(self): + for k in range(4): + stream_types = set(random.choices([x.value for x in VisionStreamType], k=k)) + self.setup_vipc("camerad", *stream_types) + available_streams = VisionIpcClient.available_streams("camerad", True) + self.assertEqual(available_streams, stream_types) + + def test_buffers(self): + width, height, num_buffers = 100, 200, 5 + self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, num_buffers=num_buffers, width=width, height=height) + self.assertEqual(self.client.width, width) + self.assertEqual(self.client.height, height) + self.assertGreater(self.client.buffer_len, 0) + self.assertEqual(self.client.num_buffers, num_buffers) + + def test_yuv_rgb(self): + _, client_yuv = self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, rgb=False) + _, client_rgb = self.setup_vipc("navd", VisionStreamType.VISION_STREAM_MAP, rgb=True) + self.assertTrue(client_rgb.rgb) + self.assertFalse(client_yuv.rgb) + + def test_send_single_buffer(self): + self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD) + + buf = np.zeros(self.client.buffer_len, dtype=np.uint8) + buf.view(' #include diff --git a/cereal/visionipc/visionbuf_ion.cc b/msgq_repo/msgq/visionipc/visionbuf_ion.cc similarity index 99% rename from cereal/visionipc/visionbuf_ion.cc rename to msgq_repo/msgq/visionipc/visionbuf_ion.cc index f72e76cf74..13be154914 100644 --- a/cereal/visionipc/visionbuf_ion.cc +++ b/msgq_repo/msgq/visionipc/visionbuf_ion.cc @@ -14,7 +14,7 @@ #include -#include "cereal/visionipc/visionbuf.h" +#include "msgq/visionipc/visionbuf.h" // keep trying if x gets interrupted by a signal #define HANDLE_EINTR(x) \ diff --git a/cereal/visionipc/ipc.cc b/msgq_repo/msgq/visionipc/visionipc.cc similarity index 98% rename from cereal/visionipc/ipc.cc rename to msgq_repo/msgq/visionipc/visionipc.cc index c4ab9a4753..48e13c27d9 100644 --- a/cereal/visionipc/ipc.cc +++ b/msgq_repo/msgq/visionipc/visionipc.cc @@ -15,7 +15,7 @@ #define getsocket() socket(AF_UNIX, SOCK_SEQPACKET, 0) #endif -#include "cereal/visionipc/ipc.h" +#include "msgq/visionipc/visionipc.h" int ipc_connect(const char* socket_path) { int err; diff --git a/cereal/visionipc/visionipc.h b/msgq_repo/msgq/visionipc/visionipc.h similarity index 57% rename from cereal/visionipc/visionipc.h rename to msgq_repo/msgq/visionipc/visionipc.h index 7489bc9585..224f129c91 100644 --- a/cereal/visionipc/visionipc.h +++ b/msgq_repo/msgq/visionipc/visionipc.h @@ -3,6 +3,12 @@ #include #include + +int ipc_connect(const char* socket_path); +int ipc_bind(const char* socket_path); +int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds, + int *out_num_fds); + constexpr int VISIONIPC_MAX_FDS = 128; struct VisionIpcBufExtra { diff --git a/cereal/visionipc/visionipc.pxd b/msgq_repo/msgq/visionipc/visionipc.pxd similarity index 87% rename from cereal/visionipc/visionipc.pxd rename to msgq_repo/msgq/visionipc/visionipc.pxd index 3151dfc7d7..645ab2208f 100644 --- a/cereal/visionipc/visionipc.pxd +++ b/msgq_repo/msgq/visionipc/visionipc.pxd @@ -7,7 +7,7 @@ from libcpp.set cimport set from libc.stdint cimport uint32_t, uint64_t from libcpp cimport bool, int -cdef extern from "cereal/visionipc/visionbuf.h": +cdef extern from "msgq/visionipc/visionbuf.h": struct _cl_device_id struct _cl_context struct _cl_mem @@ -30,14 +30,14 @@ cdef extern from "cereal/visionipc/visionbuf.h": cl_mem buf_cl void set_frame_id(uint64_t id) -cdef extern from "cereal/visionipc/visionipc.h": +cdef extern from "msgq/visionipc/visionipc.h": struct VisionIpcBufExtra: uint32_t frame_id uint64_t timestamp_sof uint64_t timestamp_eof bool valid -cdef extern from "cereal/visionipc/visionipc_server.h": +cdef extern from "msgq/visionipc/visionipc_server.h": string get_endpoint_name(string, VisionStreamType) cdef cppclass VisionIpcServer: @@ -48,7 +48,7 @@ cdef extern from "cereal/visionipc/visionipc_server.h": void send(VisionBuf *, VisionIpcBufExtra *, bool) void start_listener() -cdef extern from "cereal/visionipc/visionipc_client.h": +cdef extern from "msgq/visionipc/visionipc_client.h": cdef cppclass VisionIpcClient: int num_buffers VisionBuf buffers[1] diff --git a/cereal/visionipc/visionipc_client.cc b/msgq_repo/msgq/visionipc/visionipc_client.cc similarity index 95% rename from cereal/visionipc/visionipc_client.cc rename to msgq_repo/msgq/visionipc/visionipc_client.cc index e3c6d0d1ca..9b24da296d 100644 --- a/cereal/visionipc/visionipc_client.cc +++ b/msgq_repo/msgq/visionipc/visionipc_client.cc @@ -4,10 +4,11 @@ #include #include -#include "cereal/visionipc/ipc.h" -#include "cereal/visionipc/visionipc_client.h" -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/logger/logger.h" +#include "msgq/visionipc/visionipc.h" +#include "msgq/visionipc/visionipc_client.h" +#include "msgq/visionipc/visionipc_server.h" +#include "logger/logger.h" +#include "logger/logger.h" static int connect_to_vipc_server(const std::string &name, bool blocking) { const std::string ipc_path = get_ipc_path(name); diff --git a/cereal/visionipc/visionipc_client.h b/msgq_repo/msgq/visionipc/visionipc_client.h similarity index 90% rename from cereal/visionipc/visionipc_client.h rename to msgq_repo/msgq/visionipc/visionipc_client.h index 970bac3a71..e4abdc5de3 100644 --- a/cereal/visionipc/visionipc_client.h +++ b/msgq_repo/msgq/visionipc/visionipc_client.h @@ -3,8 +3,9 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/visionipc/visionbuf.h" +#include "msgq/ipc.h" +#include "msgq/visionipc/visionbuf.h" + class VisionIpcClient { private: diff --git a/cereal/visionipc/visionipc_pyx.pxd b/msgq_repo/msgq/visionipc/visionipc_pyx.pxd similarity index 100% rename from cereal/visionipc/visionipc_pyx.pxd rename to msgq_repo/msgq/visionipc/visionipc_pyx.pxd diff --git a/cereal/visionipc/visionipc_pyx.pyx b/msgq_repo/msgq/visionipc/visionipc_pyx.pyx similarity index 100% rename from cereal/visionipc/visionipc_pyx.pyx rename to msgq_repo/msgq/visionipc/visionipc_pyx.pyx diff --git a/cereal/visionipc/visionipc_server.cc b/msgq_repo/msgq/visionipc/visionipc_server.cc similarity index 97% rename from cereal/visionipc/visionipc_server.cc rename to msgq_repo/msgq/visionipc/visionipc_server.cc index da9d11f91a..611d10b206 100644 --- a/cereal/visionipc/visionipc_server.cc +++ b/msgq_repo/msgq/visionipc/visionipc_server.cc @@ -8,10 +8,10 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/visionipc/ipc.h" -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/logger/logger.h" +#include "msgq/ipc.h" +#include "msgq/visionipc/visionipc.h" +#include "msgq/visionipc/visionipc_server.h" +#include "logger/logger.h" std::string get_endpoint_name(std::string name, VisionStreamType type){ if (messaging_use_zmq()){ diff --git a/cereal/visionipc/visionipc_server.h b/msgq_repo/msgq/visionipc/visionipc_server.h similarity index 93% rename from cereal/visionipc/visionipc_server.h rename to msgq_repo/msgq/visionipc/visionipc_server.h index c494b1fcf4..feacc4d10d 100644 --- a/cereal/visionipc/visionipc_server.h +++ b/msgq_repo/msgq/visionipc/visionipc_server.h @@ -5,8 +5,8 @@ #include #include -#include "cereal/messaging/messaging.h" -#include "cereal/visionipc/visionbuf.h" +#include "msgq/ipc.h" +#include "msgq/visionipc/visionbuf.h" std::string get_endpoint_name(std::string name, VisionStreamType type); std::string get_ipc_path(const std::string &name); diff --git a/cereal/visionipc/visionipc_tests.cc b/msgq_repo/msgq/visionipc/visionipc_tests.cc similarity index 97% rename from cereal/visionipc/visionipc_tests.cc rename to msgq_repo/msgq/visionipc/visionipc_tests.cc index 4a081df11e..8c4e7fa3fc 100644 --- a/cereal/visionipc/visionipc_tests.cc +++ b/msgq_repo/msgq/visionipc/visionipc_tests.cc @@ -2,8 +2,10 @@ #include #include "catch2/catch.hpp" -#include "cereal/visionipc/visionipc_server.h" -#include "cereal/visionipc/visionipc_client.h" + +#include "msgq/visionipc/visionipc_server.h" +#include "msgq/visionipc/visionipc_client.h" + static void zmq_sleep(int milliseconds=1000){ if (messaging_use_zmq()){ diff --git a/msgq_repo/pyproject.toml b/msgq_repo/pyproject.toml new file mode 100644 index 0000000000..73928614eb --- /dev/null +++ b/msgq_repo/pyproject.toml @@ -0,0 +1,21 @@ +# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml +[tool.ruff] +lint.select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"] +lint.ignore = ["W292", "E741", "E402", "C408", "ISC003"] +lint.flake8-implicit-str-concat.allow-multiline=false + +line-length = 160 +target-version="py311" + +[mypy.tool] +# third-party packages +ignore_missing_imports=true + +# helpful warnings +warn_redundant_casts=true +warn_unreachable=true +warn_unused_ignores=true + +# restrict dynamic typing +warn_return_any=true +check_untyped_defs=true diff --git a/msgq_repo/site_scons/site_tools/cython.py b/msgq_repo/site_scons/site_tools/cython.py new file mode 100644 index 0000000000..c291475533 --- /dev/null +++ b/msgq_repo/site_scons/site_tools/cython.py @@ -0,0 +1,72 @@ +import re +import SCons +from SCons.Action import Action +from SCons.Scanner import Scanner + +pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M) +pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M) +cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M) + + +def pyx_scan(node, env, path, arg=None): + contents = node.get_text_contents() + + # from cimport ... + matches = pyx_from_import_re.findall(contents) + # cimport + matches += pyx_import_re.findall(contents) + + # Modules can be either .pxd or .pyx files + files = [m.replace('.', '/') + '.pxd' for m in matches] + files += [m.replace('.', '/') + '.pyx' for m in matches] + + # cdef extern from + files += cdef_import_re.findall(contents) + + # Handle relative imports + cur_dir = str(node.get_dir()) + files = [cur_dir + f if f.startswith('/') else f for f in files] + + # Filter out non-existing files (probably system imports) + files = [f for f in files if env.File(f).exists()] + return env.File(files) + + +pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True) +cythonAction = Action("$CYTHONCOM") + + +def create_builder(env): + try: + cython = env['BUILDERS']['Cython'] + except KeyError: + cython = SCons.Builder.Builder( + action=cythonAction, + emitter={}, + suffix=cython_suffix_emitter, + single_source=1 + ) + env.Append(SCANNERS=pyxscanner) + env['BUILDERS']['Cython'] = cython + return cython + +def cython_suffix_emitter(env, source): + return "$CYTHONCFILESUFFIX" + +def generate(env): + env["CYTHON"] = "cythonize" + env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + env["CYTHONCFILESUFFIX"] = ".cpp" + + c_file, _ = SCons.Tool.createCFileBuilders(env) + + c_file.suffix['.pyx'] = cython_suffix_emitter + c_file.add_action('.pyx', cythonAction) + + c_file.suffix['.py'] = cython_suffix_emitter + c_file.add_action('.py', cythonAction) + + create_builder(env) + +def exists(env): + return True diff --git a/opendbc/README.md b/opendbc/README.md new file mode 100644 index 0000000000..20e31a9034 --- /dev/null +++ b/opendbc/README.md @@ -0,0 +1,49 @@ +## DBC file basics + +A DBC file encodes, in a humanly readable way, the information needed to understand a vehicle's CAN bus traffic. A vehicle might have multiple CAN buses and every CAN bus is represented by its own dbc file. +Wondering what's the DBC file format? [Here](http://www.socialledge.com/sjsu/index.php?title=DBC_Format) and [Here](https://github.com/stefanhoelzl/CANpy/blob/master/docs/DBC_Specification.md) a couple of good overviews. + +## How to start reverse engineering cars + +[opendbc](https://github.com/commaai/opendbc) is integrated with [cabana](https://github.com/commaai/openpilot/tree/master/tools/cabana). + +Use [panda](https://github.com/commaai/panda) to connect your car to a computer. + +## How to use reverse engineered DBC +To create custom CAN simulations or send reverse engineered signals back to the car you can use [CANdevStudio](https://github.com/GENIVI/CANdevStudio) project. + +## DBC file preprocessor + +DBC files for different models of the same brand have a lot of overlap. Therefore, we wrote a preprocessor to create DBC files from a brand DBC file and a model specific DBC file. The source DBC files can be found in the generator folder. After changing one of the files run the generator.py script to regenerate the output files. These output files will be placed in the root of the opendbc repository and are suffixed by _generated. + +## Good practices for contributing to opendbc + +- Comments: the best way to store comments is to add them directly to the DBC files. For example: + ``` + CM_ SG_ 490 LONG_ACCEL "wheel speed derivative, noisy and zero snapping"; + ``` + is a comment that refers to signal `LONG_ACCEL` in message `490`. Using comments is highly recommended, especially for doubts and uncertainties. [cabana](https://community.comma.ai/cabana/) can easily display/add/edit comments to signals and messages. + +- Units: when applicable, it's recommended to convert signals into physical units, by using a proper signal factor. Using a SI unit is preferred, unless a non-SI unit rounds the signal factor much better. +For example: + ``` + SG_ VEHICLE_SPEED : 7|15@0+ (0.00278,0) [0|70] "m/s" PCM + ``` + is better than: + ``` + SG_ VEHICLE_SPEED : 7|15@0+ (0.00620,0) [0|115] "mph" PCM + ``` + However, the cleanest option is really: + ``` + SG_ VEHICLE_SPEED : 7|15@0+ (0.01,0) [0|250] "kph" PCM + ``` + +- Signal size: always use the smallest amount of bits possible. For example, let's say I'm reverse engineering the gas pedal position and I've determined that it's in a 3 bytes message. For 0% pedal position I read a message value of `0x00 0x00 0x00`, while for 100% of pedal position I read `0x64 0x00 0x00`: clearly, the gas pedal position is within the first byte of the message and I might be tempted to define the signal `GAS_POS` as: + ``` + SG_ GAS_POS : 7|8@0+ (1,0) [0|100] "%" PCM + ``` + However, I can't be sure that the very first bit of the message is referred to the pedal position: I haven't seen it changing! Therefore, a safer way of defining the signal is: + ``` + SG_ GAS_POS : 6|7@0+ (1,0) [0|100] "%" PCM + ``` + which leaves the first bit unallocated. This prevents from very erroneous reading of the gas pedal position, in case the first bit is indeed used for something else. diff --git a/opendbc/SConstruct b/opendbc/SConstruct new file mode 100644 index 0000000000..34e3061e78 --- /dev/null +++ b/opendbc/SConstruct @@ -0,0 +1,85 @@ +import os +import subprocess +import sysconfig +import numpy as np + +zmq = 'zmq' +arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() + +cereal_dir = Dir('.') + +python_path = sysconfig.get_paths()['include'] +cpppath = [ + '#', + '/usr/lib/include', + python_path +] + +AddOption('--minimal', + action='store_false', + dest='extras', + default=True, + help='the minimum build. no tests, tools, etc.') + +AddOption('--asan', + action='store_true', + help='turn on ASAN') + +ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] if GetOption('asan') else [] +ldflags_asan = ["-fsanitize=address"] if GetOption('asan') else [] + +env = Environment( + ENV=os.environ, + CC='clang', + CXX='clang++', + CCFLAGS=[ + "-g", + "-fPIC", + "-O2", + "-Wunused", + "-Werror", + "-Wshadow", + "-Wno-vla-cxx-extension", + ] + ccflags_asan, + LDFLAGS=ldflags_asan, + LINKFLAGS=ldflags_asan, + LIBPATH=[ + "#opendbc/can/", + ], + CFLAGS="-std=gnu11", + CXXFLAGS=["-std=c++1z"], + CPPPATH=cpppath, + CYTHONCFILESUFFIX=".cpp", + tools=["default", "cython"] +) + +common = '' +Export('env', 'zmq', 'arch', 'common') + +cereal = [File('#cereal/libcereal.a')] +messaging = [File('#cereal/libmessaging.a')] +Export('cereal', 'messaging') + + +envCython = env.Clone() +envCython["CPPPATH"] += [np.get_include()] +envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"] +envCython["CCFLAGS"].remove("-Werror") + +python_libs = [] +if arch == "Darwin": + envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] +elif arch == "aarch64": + envCython["LINKFLAGS"] = ["-shared"] + + python_libs.append(os.path.basename(python_path)) +else: + envCython["LINKFLAGS"] = ["-pthread", "-shared"] + +envCython["LIBS"] = python_libs + +Export('envCython') + + +SConscript(['cereal/SConscript']) +SConscript(['opendbc/can/SConscript']) diff --git a/opendbc/acura_ilx_2016_can_generated.dbc b/opendbc/acura_ilx_2016_can_generated.dbc index 1afb5bade6..24c8f67892 100644 --- a/opendbc/acura_ilx_2016_can_generated.dbc +++ b/opendbc/acura_ilx_2016_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/acura_rdx_2018_can_generated.dbc b/opendbc/acura_rdx_2018_can_generated.dbc index 6596b44514..4c23725176 100644 --- a/opendbc/acura_rdx_2018_can_generated.dbc +++ b/opendbc/acura_rdx_2018_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/can/SConscript b/opendbc/can/SConscript index f07234c111..7e8fd270f6 100644 --- a/opendbc/can/SConscript +++ b/opendbc/can/SConscript @@ -22,3 +22,7 @@ packer = lenv.Program('packer_pyx.so', 'packer_pyx.pyx') lenv.Depends(parser, libdbc) lenv.Depends(packer, libdbc) + +opendbc_python = Alias("opendbc_python", [parser, packer]) + +Export('opendbc_python') \ No newline at end of file diff --git a/opendbc/can/common.cc b/opendbc/can/common.cc index 37b48fa6aa..bb7b2ee56d 100644 --- a/opendbc/can/common.cc +++ b/opendbc/can/common.cc @@ -102,11 +102,15 @@ void gen_crc_lookup_table_16(uint16_t poly, uint16_t crc_lut[]) { } } -void init_crc_lookup_tables() { - // At init time, set up static lookup tables for fast CRC computation. - gen_crc_lookup_table_8(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen - gen_crc_lookup_table_16(0x1021, crc16_lut_xmodem); // CRC-16 XMODEM for HKG CAN FD -} +// Initializes CRC lookup tables at module initialization +struct CrcInitializer { + CrcInitializer() { + gen_crc_lookup_table_8(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen + gen_crc_lookup_table_16(0x1021, crc16_lut_xmodem); // CRC-16 XMODEM for HKG CAN FD + } +}; + +static CrcInitializer crcInitializer; unsigned int volkswagen_mqb_checksum(uint32_t address, const Signal &sig, const std::vector &d) { // Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with diff --git a/opendbc/can/common.h b/opendbc/can/common.h index cfc63eb105..46b3159b9f 100644 --- a/opendbc/can/common.h +++ b/opendbc/can/common.h @@ -13,6 +13,7 @@ #include "cereal/gen/cpp/log.capnp.h" #endif +#include "opendbc/can/logger.h" #include "opendbc/can/common_dbc.h" #define INFO printf @@ -23,8 +24,6 @@ #define MAX_BAD_COUNTER 5 #define CAN_INVALID_CNT 5 -void init_crc_lookup_tables(); - // Car specific functions unsigned int honda_checksum(uint32_t address, const Signal &sig, const std::vector &d); unsigned int toyota_checksum(uint32_t address, const Signal &sig, const std::vector &d); @@ -92,11 +91,10 @@ class CANPacker { private: const DBC *dbc = NULL; std::map, Signal> signal_lookup; - std::map message_lookup; std::map counters; public: CANPacker(const std::string& dbc_name); std::vector pack(uint32_t address, const std::vector &values); - Msg* lookup_message(uint32_t address); + const Msg* lookup_message(uint32_t address); }; diff --git a/opendbc/can/common.pxd b/opendbc/can/common.pxd index 57053b781e..4dab92cd5f 100644 --- a/opendbc/can/common.pxd +++ b/opendbc/can/common.pxd @@ -6,6 +6,7 @@ from libcpp cimport bool from libcpp.pair cimport pair from libcpp.string cimport string from libcpp.vector cimport vector +from libcpp.unordered_map cimport unordered_map ctypedef unsigned int (*calc_checksum_type)(uint32_t, const Signal&, const vector[uint8_t] &) @@ -48,6 +49,8 @@ cdef extern from "common_dbc.h": string name vector[Msg] msgs vector[Val] vals + unordered_map[uint32_t, const Msg*] addr_to_msg + unordered_map[string, const Msg*] name_to_msg cdef struct SignalValue: uint32_t address diff --git a/opendbc/can/common_dbc.h b/opendbc/can/common_dbc.h index ef4c98c803..19507ecd4e 100644 --- a/opendbc/can/common_dbc.h +++ b/opendbc/can/common_dbc.h @@ -1,8 +1,8 @@ #pragma once -#include #include #include +#include #include struct SignalPackValue { @@ -59,6 +59,8 @@ struct DBC { std::string name; std::vector msgs; std::vector vals; + std::unordered_map addr_to_msg; + std::unordered_map name_to_msg; }; typedef struct ChecksumState { diff --git a/opendbc/can/dbc.cc b/opendbc/can/dbc.cc index 7abe6b679c..44454b15f8 100644 --- a/opendbc/can/dbc.cc +++ b/opendbc/can/dbc.cc @@ -200,6 +200,8 @@ DBC* dbc_parse_from_stream(const std::string &dbc_name, std::istream &stream, Ch for (auto& m : dbc->msgs) { m.sigs = signals[m.address]; + dbc->addr_to_msg[m.address] = &m; + dbc->name_to_msg[m.name] = &m; } for (auto& v : dbc->vals) { v.sigs = signals[v.address]; diff --git a/opendbc/can/logger.h b/opendbc/can/logger.h new file mode 100644 index 0000000000..bab70c4894 --- /dev/null +++ b/opendbc/can/logger.h @@ -0,0 +1,27 @@ +#pragma once + +#ifdef SWAGLOG +// cppcheck-suppress preprocessorErrorDirective +#include SWAGLOG +#else + +#define CLOUDLOG_DEBUG 10 +#define CLOUDLOG_INFO 20 +#define CLOUDLOG_WARNING 30 +#define CLOUDLOG_ERROR 40 +#define CLOUDLOG_CRITICAL 50 + +#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__) +#define cloudlog_rl(burst, millis, lvl, fmt, ...) printf(fmt "\n", ##__VA_ARGS__) + +#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__) +#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__) +#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) +#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__) + +#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__) +#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__) +#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__) +#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__) + +#endif diff --git a/opendbc/can/packer.cc b/opendbc/can/packer.cc index 87fa0a8201..15f49cf6b6 100644 --- a/opendbc/can/packer.cc +++ b/opendbc/can/packer.cc @@ -33,16 +33,20 @@ CANPacker::CANPacker(const std::string& dbc_name) { assert(dbc); for (const auto& msg : dbc->msgs) { - message_lookup[msg.address] = msg; for (const auto& sig : msg.sigs) { signal_lookup[std::make_pair(msg.address, sig.name)] = sig; } } - init_crc_lookup_tables(); } std::vector CANPacker::pack(uint32_t address, const std::vector &signals) { - std::vector ret(message_lookup[address].size, 0); + auto msg_it = dbc->addr_to_msg.find(address); + if (msg_it == dbc->addr_to_msg.end()) { + LOGE("undefined address %d", address); + return {}; + } + + std::vector ret(msg_it->second->size, 0); // set all values for all given signal/value pairs bool counter_set = false; @@ -50,7 +54,7 @@ std::vector CANPacker::pack(uint32_t address, const std::vectorsecond; @@ -61,9 +65,9 @@ std::vector CANPacker::pack(uint32_t address, const std::vector CANPacker::pack(uint32_t address, const std::vectoraddr_to_msg.at(address); } diff --git a/opendbc/can/packer_pyx.pyx b/opendbc/can/packer_pyx.pyx index 1b9fc8fab6..2e9a6f9292 100644 --- a/opendbc/can/packer_pyx.pyx +++ b/opendbc/can/packer_pyx.pyx @@ -1,20 +1,17 @@ # distutils: language = c++ # cython: c_string_encoding=ascii, language_level=3 -from libc.stdint cimport uint8_t +from libc.stdint cimport uint8_t, uint32_t from libcpp.vector cimport vector -from libcpp.map cimport map -from libcpp.string cimport string from .common cimport CANPacker as cpp_CANPacker -from .common cimport dbc_lookup, SignalPackValue, DBC +from .common cimport dbc_lookup, SignalPackValue, DBC, Msg cdef class CANPacker: cdef: cpp_CANPacker *packer const DBC *dbc - map[string, int] name_to_address def __init__(self, dbc_name): self.dbc = dbc_lookup(dbc_name) @@ -22,9 +19,6 @@ cdef class CANPacker: raise RuntimeError(f"Can't lookup {dbc_name}") self.packer = new cpp_CANPacker(dbc_name) - for i in range(self.dbc[0].msgs.size()): - msg = self.dbc[0].msgs[i] - self.name_to_address[string(msg.name)] = msg.address def __dealloc__(self): if self.packer: @@ -43,11 +37,17 @@ cdef class CANPacker: return self.packer.pack(addr, values_thing) cpdef make_can_msg(self, name_or_addr, bus, values): - cdef int addr + cdef uint32_t addr = 0 + cdef const Msg* m if isinstance(name_or_addr, int): addr = name_or_addr else: - addr = self.name_to_address[name_or_addr.encode("utf8")] + try: + m = self.dbc.name_to_msg.at(name_or_addr.encode("utf8")) + addr = m.address + except IndexError: + # The C++ pack function will log an error message for invalid addresses + pass cdef vector[uint8_t] val = self.pack(addr, values) return [addr, 0, (&val[0])[:val.size()], bus] diff --git a/opendbc/can/parser.cc b/opendbc/can/parser.cc index 3ed9f02880..d36ec75bad 100644 --- a/opendbc/can/parser.cc +++ b/opendbc/can/parser.cc @@ -10,7 +10,6 @@ #include #include -#include "cereal/logger/logger.h" #include "opendbc/can/common.h" int64_t get_raw_value(const std::vector &msg, const Signal &sig) { @@ -65,7 +64,7 @@ bool MessageState::parse(uint64_t nanos, const std::vector &dat) { // only update values if both checksum and counter are valid if (checksum_failed || counter_failed) { - LOGE("0x%X message checks failed, checksum failed %d, counter failed %d", address, checksum_failed, counter_failed); + LOGE_100("0x%X message checks failed, checksum failed %d, counter failed %d", address, checksum_failed, counter_failed); return false; } @@ -97,7 +96,6 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vector(1024)) { dbc = dbc_lookup(dbc_name); assert(dbc); - init_crc_lookup_tables(); bus_timeout_threshold = std::numeric_limits::max(); @@ -121,18 +119,7 @@ CANParser::CANParser(int abus, const std::string& dbc_name, const std::vectormsgs) { - if (m.address == address) { - msg = &m; - break; - } - } - if (!msg) { - fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", address, dbc_name.c_str()); - assert(false); - } - + const Msg *msg = dbc->addr_to_msg.at(address); state.name = msg->name; state.size = msg->size; assert(state.size <= 64); // max signal size is 64 bytes @@ -150,7 +137,6 @@ CANParser::CANParser(int abus, const std::string& dbc_name, bool ignore_checksum dbc = dbc_lookup(dbc_name); assert(dbc); - init_crc_lookup_tables(); for (const auto& msg : dbc->msgs) { MessageState state = { @@ -238,7 +224,10 @@ void CANParser::UpdateCans(uint64_t nanos, const capnp::List::R // continue; //} - std::vector data(dat.size(), 0); + // TODO: can remove when we ignore unexpected can msg lengths + // make sure the data_size is not less than state_it->second.size + size_t data_size = std::max(dat.size(), state_it->second.size); + std::vector data(data_size, 0); memcpy(data.data(), dat.begin(), dat.size()); state_it->second.parse(nanos, data); } @@ -290,9 +279,9 @@ void CANParser::UpdateValid(uint64_t nanos) { if (state.check_threshold > 0 && (missing || timed_out)) { if (show_missing && !bus_timeout) { if (missing) { - LOGE("0x%X '%s' NOT SEEN", state.address, state.name.c_str()); + LOGE_100("0x%X '%s' NOT SEEN", state.address, state.name.c_str()); } else if (timed_out) { - LOGE("0x%X '%s' TIMED OUT", state.address, state.name.c_str()); + LOGE_100("0x%X '%s' TIMED OUT", state.address, state.name.c_str()); } } _valid = false; diff --git a/opendbc/can/parser_pyx.pyx b/opendbc/can/parser_pyx.pyx index 8ce67740b0..a5b802dfce 100644 --- a/opendbc/can/parser_pyx.pyx +++ b/opendbc/can/parser_pyx.pyx @@ -5,7 +5,6 @@ from cython.operator cimport dereference as deref, preincrement as preinc from libcpp.pair cimport pair from libcpp.string cimport string from libcpp.vector cimport vector -from libcpp.unordered_set cimport unordered_set from libc.stdint cimport uint32_t from .common cimport CANParser as cpp_CANParser @@ -20,6 +19,7 @@ cdef class CANParser: cpp_CANParser *can const DBC *dbc vector[SignalValue] can_values + vector[uint32_t] addresses cdef readonly: dict vl @@ -36,29 +36,24 @@ cdef class CANParser: self.vl = {} self.vl_all = {} self.ts_nanos = {} - msg_name_to_address = {} - address_to_msg_name = {} - - for i in range(self.dbc[0].msgs.size()): - msg = self.dbc[0].msgs[i] - name = msg.name.decode("utf8") - - msg_name_to_address[name] = msg.address - address_to_msg_name[msg.address] = name # Convert message names into addresses and check existence in DBC cdef vector[pair[uint32_t, int]] message_v for i in range(len(messages)): c = messages[i] - address = c[0] if isinstance(c[0], numbers.Number) else msg_name_to_address.get(c[0]) - if address not in address_to_msg_name: + try: + m = self.dbc.addr_to_msg.at(c[0]) if isinstance(c[0], numbers.Number) else self.dbc.name_to_msg.at(c[0]) + except IndexError: raise RuntimeError(f"could not find message {repr(c[0])} in DBC {self.dbc_name}") + + address = m.address message_v.push_back((address, c[1])) + self.addresses.push_back(address) - name = address_to_msg_name[address] + name = m.name.decode("utf8") self.vl[address] = {} self.vl[name] = self.vl[address] - self.vl_all[address] = {} + self.vl_all[address] = defaultdict(list) self.vl_all[name] = self.vl_all[address] self.ts_nanos[address] = {} self.ts_nanos[name] = self.ts_nanos[address] @@ -71,24 +66,35 @@ cdef class CANParser: del self.can def update_strings(self, strings, sendcan=False): - for v in self.vl_all.values(): - for l in v.values(): # no-cython-lint - l.clear() + for address in self.addresses: + self.vl_all[address].clear() cdef vector[SignalValue] new_vals - cdef unordered_set[uint32_t] updated_addrs + cur_address = -1 + vl = {} + vl_all = {} + ts_nanos = {} + updated_addrs = set() self.can.update_strings(strings, new_vals, sendcan) cdef vector[SignalValue].iterator it = new_vals.begin() cdef SignalValue* cv while it != new_vals.end(): cv = &deref(it) + + # Check if the address has changed + if cv.address != cur_address: + cur_address = cv.address + vl = self.vl[cur_address] + vl_all = self.vl_all[cur_address] + ts_nanos = self.ts_nanos[cur_address] + updated_addrs.add(cur_address) + # Cast char * directly to unicode cv_name = cv.name - self.vl[cv.address][cv_name] = cv.value - self.vl_all[cv.address][cv_name] = cv.all_values - self.ts_nanos[cv.address][cv_name] = cv.ts_nanos - updated_addrs.insert(cv.address) + vl[cv_name] = cv.value + vl_all[cv_name] = cv.all_values + ts_nanos[cv_name] = cv.ts_nanos preinc(it) return updated_addrs @@ -116,14 +122,6 @@ cdef class CANDefine(): if not self.dbc: raise RuntimeError(f"Can't find DBC: '{dbc_name}'") - address_to_msg_name = {} - - for i in range(self.dbc[0].msgs.size()): - msg = self.dbc[0].msgs[i] - name = msg.name.decode("utf8") - address = msg.address - address_to_msg_name[address] = name - dv = defaultdict(dict) for i in range(self.dbc[0].vals.size()): @@ -132,7 +130,11 @@ cdef class CANDefine(): sgname = val.name.decode("utf8") def_val = val.def_val.decode("utf8") address = val.address - msgname = address_to_msg_name[address] + try: + m = self.dbc.addr_to_msg.at(address) + except IndexError: + raise KeyError(address) + msgname = m.name.decode("utf-8") # separate definition/value pairs def_val = def_val.split() diff --git a/opendbc/can/tests/.gitignore b/opendbc/can/tests/.gitignore new file mode 100644 index 0000000000..192fb0945e --- /dev/null +++ b/opendbc/can/tests/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/opendbc/can/tests/__init__.py b/opendbc/can/tests/__init__.py new file mode 100644 index 0000000000..3bf02fdae0 --- /dev/null +++ b/opendbc/can/tests/__init__.py @@ -0,0 +1,8 @@ +import glob +import os + +from opendbc import DBC_PATH + +ALL_DBCS = [os.path.basename(dbc).split('.')[0] for dbc in + glob.glob(f"{DBC_PATH}/*.dbc")] +TEST_DBC = os.path.abspath(os.path.join(os.path.dirname(__file__), "test.dbc")) diff --git a/opendbc/can/tests/test_checksums.py b/opendbc/can/tests/test_checksums.py new file mode 100755 index 0000000000..93ddbe0074 --- /dev/null +++ b/opendbc/can/tests/test_checksums.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +import unittest + +from opendbc.can.parser import CANParser +from opendbc.can.packer import CANPacker +from opendbc.can.tests.test_packer_parser import can_list_to_can_capnp + + +class TestCanChecksums(unittest.TestCase): + + def test_honda_checksum(self): + """Test checksums for Honda standard and extended CAN ids""" + dbc_file = "honda_accord_2018_can_generated" + msgs = [("LKAS_HUD", 0), ("LKAS_HUD_A", 0)] + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + values = { + 'SET_ME_X41': 0x41, + 'STEERING_REQUIRED': 1, + 'SOLID_LANES': 1, + 'BEEP': 0, + } + + # known correct checksums according to the above values + checksum_std = [11, 10, 9, 8] + checksum_ext = [4, 3, 2, 1] + + for std, ext in zip(checksum_std, checksum_ext): + msgs = [ + packer.make_can_msg("LKAS_HUD", 0, values), + packer.make_can_msg("LKAS_HUD_A", 0, values), + ] + can_strings = [can_list_to_can_capnp(msgs), ] + parser.update_strings(can_strings) + + self.assertEqual(parser.vl['LKAS_HUD']['CHECKSUM'], std) + self.assertEqual(parser.vl['LKAS_HUD_A']['CHECKSUM'], ext) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/can/tests/test_dbc_exceptions.py b/opendbc/can/tests/test_dbc_exceptions.py new file mode 100755 index 0000000000..1f13f53927 --- /dev/null +++ b/opendbc/can/tests/test_dbc_exceptions.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 + +import unittest + +from opendbc.can.parser import CANParser, CANDefine +from opendbc.can.packer import CANPacker +from opendbc.can.tests import TEST_DBC + + +class TestCanParserPackerExceptions(unittest.TestCase): + def test_civic_exceptions(self): + dbc_file = "honda_civic_touring_2016_can_generated" + dbc_invalid = dbc_file + "abcdef" + msgs = [("STEERING_CONTROL", 50)] + with self.assertRaises(RuntimeError): + CANParser(dbc_invalid, msgs, 0) + with self.assertRaises(RuntimeError): + CANPacker(dbc_invalid) + with self.assertRaises(RuntimeError): + CANDefine(dbc_invalid) + with self.assertRaises(KeyError): + CANDefine(TEST_DBC) + + parser = CANParser(dbc_file, msgs, 0) + with self.assertRaises(RuntimeError): + parser.update_strings([b'']) + + # Everything is supposed to work below + CANParser(dbc_file, msgs, 0) + CANParser(dbc_file, [], 0) + CANPacker(dbc_file) + CANDefine(dbc_file) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/can/tests/test_dbc_parser.py b/opendbc/can/tests/test_dbc_parser.py new file mode 100755 index 0000000000..5da41d49b7 --- /dev/null +++ b/opendbc/can/tests/test_dbc_parser.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +import unittest + +from opendbc.can.parser import CANParser +from opendbc.can.tests import ALL_DBCS + + +class TestDBCParser(unittest.TestCase): + def test_enough_dbcs(self): + # sanity check that we're running on the real DBCs + self.assertGreater(len(ALL_DBCS), 20) + + def test_parse_all_dbcs(self): + """ + Dynamic DBC parser checks: + - Checksum and counter length, start bit, endianness + - Duplicate message addresses and names + - Signal out of bounds + - All BO_, SG_, VAL_ lines for syntax errors + """ + + for dbc in ALL_DBCS: + with self.subTest(dbc=dbc): + CANParser(dbc, [], 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/can/tests/test_define.py b/opendbc/can/tests/test_define.py new file mode 100755 index 0000000000..6387ef9cb0 --- /dev/null +++ b/opendbc/can/tests/test_define.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +import unittest + +from opendbc.can.can_define import CANDefine +from opendbc.can.tests import ALL_DBCS + + +class TestCADNDefine(unittest.TestCase): + def test_civic(self): + + dbc_file = "honda_civic_touring_2016_can_generated" + defs = CANDefine(dbc_file) + + self.assertDictEqual(defs.dv[399], defs.dv['STEER_STATUS']) + self.assertDictEqual(defs.dv[399], + {'STEER_STATUS': + {7: 'PERMANENT_FAULT', + 6: 'TMP_FAULT', + 5: 'FAULT_1', + 4: 'NO_TORQUE_ALERT_2', + 3: 'LOW_SPEED_LOCKOUT', + 2: 'NO_TORQUE_ALERT_1', + 0: 'NORMAL'} + } + ) + + def test_all_dbcs(self): + # Asserts no exceptions on all DBCs + for dbc in ALL_DBCS: + with self.subTest(dbc=dbc): + CANDefine(dbc) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/can/tests/test_packer_parser.py b/opendbc/can/tests/test_packer_parser.py new file mode 100755 index 0000000000..c313805c47 --- /dev/null +++ b/opendbc/can/tests/test_packer_parser.py @@ -0,0 +1,399 @@ +#!/usr/bin/env python3 +import unittest +import random + +import cereal.messaging as messaging +from opendbc.can.parser import CANParser +from opendbc.can.packer import CANPacker +from opendbc.can.tests import TEST_DBC + +MAX_BAD_COUNTER = 5 + + +# Python implementation so we don't have to depend on boardd +def can_list_to_can_capnp(can_msgs, msgtype='can', logMonoTime=None): + dat = messaging.new_message(msgtype, len(can_msgs)) + + if logMonoTime is not None: + dat.logMonoTime = logMonoTime + + for i, can_msg in enumerate(can_msgs): + if msgtype == 'sendcan': + cc = dat.sendcan[i] + else: + cc = dat.can[i] + + cc.address = can_msg[0] + cc.busTime = can_msg[1] + cc.dat = bytes(can_msg[2]) + cc.src = can_msg[3] + + return dat.to_bytes() + + +class TestCanParserPacker(unittest.TestCase): + def test_packer(self): + packer = CANPacker(TEST_DBC) + + for b in range(6): + for i in range(256): + values = {"COUNTER": i} + addr, _, dat, bus = packer.make_can_msg("CAN_FD_MESSAGE", b, values) + self.assertEqual(addr, 245) + self.assertEqual(bus, b) + self.assertEqual(dat[0], i) + + def test_packer_counter(self): + msgs = [("CAN_FD_MESSAGE", 0), ] + packer = CANPacker(TEST_DBC) + parser = CANParser(TEST_DBC, msgs, 0) + + # packer should increment the counter + for i in range(1000): + msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {}) + dat = can_list_to_can_capnp([msg, ]) + parser.update_strings([dat]) + self.assertEqual(parser.vl["CAN_FD_MESSAGE"]["COUNTER"], i % 256) + + # setting COUNTER should override + for _ in range(100): + cnt = random.randint(0, 255) + msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, { + "COUNTER": cnt, + "SIGNED": 0 + }) + dat = can_list_to_can_capnp([msg, ]) + parser.update_strings([dat]) + self.assertEqual(parser.vl["CAN_FD_MESSAGE"]["COUNTER"], cnt) + + # then, should resume counting from the override value + cnt = parser.vl["CAN_FD_MESSAGE"]["COUNTER"] + for i in range(100): + msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {}) + dat = can_list_to_can_capnp([msg, ]) + parser.update_strings([dat]) + self.assertEqual(parser.vl["CAN_FD_MESSAGE"]["COUNTER"], (cnt + i) % 256) + + def test_parser_can_valid(self): + msgs = [("CAN_FD_MESSAGE", 10), ] + packer = CANPacker(TEST_DBC) + parser = CANParser(TEST_DBC, msgs, 0) + + # shouldn't be valid initially + self.assertFalse(parser.can_valid) + + # not valid until the message is seen + for _ in range(100): + dat = can_list_to_can_capnp([]) + parser.update_strings([dat]) + self.assertFalse(parser.can_valid) + + # valid once seen + for i in range(1, 100): + t = int(0.01 * i * 1e9) + msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {}) + dat = can_list_to_can_capnp([msg, ], logMonoTime=t) + parser.update_strings([dat]) + self.assertTrue(parser.can_valid) + + def test_parser_counter_can_valid(self): + """ + Tests number of allowed bad counters + ensures CAN stays invalid + while receiving invalid messages + that we can recover + """ + msgs = [ + ("STEERING_CONTROL", 0), + ] + packer = CANPacker("honda_civic_touring_2016_can_generated") + parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0) + + msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 0}) + bts = can_list_to_can_capnp([msg]) + + # bad static counter, invalid once it's seen MAX_BAD_COUNTER messages + for idx in range(0x1000): + parser.update_strings([bts]) + self.assertEqual((idx + 1) < MAX_BAD_COUNTER, parser.can_valid) + + # one to recover + msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 1}) + bts = can_list_to_can_capnp([msg]) + parser.update_strings([bts]) + self.assertTrue(parser.can_valid) + + def test_parser_no_partial_update(self): + """ + Ensure that the CANParser doesn't partially update messages with invalid signals (COUNTER/CHECKSUM). + Previously, the signal update loop would only break once it got to one of these invalid signals, + after already updating most/all of the signals. + """ + msgs = [ + ("STEERING_CONTROL", 0), + ] + packer = CANPacker("honda_civic_touring_2016_can_generated") + parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0) + + def rx_steering_msg(values, bad_checksum=False): + msg = packer.make_can_msg("STEERING_CONTROL", 0, values) + if bad_checksum: + # add 1 to checksum + msg[2] = bytearray(msg[2]) + msg[2][4] = (msg[2][4] & 0xF0) | ((msg[2][4] & 0x0F) + 1) + + bts = can_list_to_can_capnp([msg]) + parser.update_strings([bts]) + + rx_steering_msg({"STEER_TORQUE": 100}, bad_checksum=False) + self.assertEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], 100) + self.assertEqual(parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"], [100]) + + for _ in range(5): + rx_steering_msg({"STEER_TORQUE": 200}, bad_checksum=True) + self.assertEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], 100) + self.assertEqual(parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"], []) + + # Even if CANParser doesn't update instantaneous vl, make sure it didn't add invalid values to vl_all + rx_steering_msg({"STEER_TORQUE": 300}, bad_checksum=False) + self.assertEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], 300) + self.assertEqual(parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"], [300]) + + def test_packer_parser(self): + msgs = [ + ("Brake_Status", 0), + ("CAN_FD_MESSAGE", 0), + ("STEERING_CONTROL", 0), + ] + packer = CANPacker(TEST_DBC) + parser = CANParser(TEST_DBC, msgs, 0) + + for steer in range(-256, 255): + for active in (1, 0): + values = { + "STEERING_CONTROL": { + "STEER_TORQUE": steer, + "STEER_TORQUE_REQUEST": active, + }, + "Brake_Status": { + "Signal1": 61042322657536.0, + }, + "CAN_FD_MESSAGE": { + "SIGNED": steer, + "64_BIT_LE": random.randint(0, 100), + "64_BIT_BE": random.randint(0, 100), + }, + } + + msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()] + bts = can_list_to_can_capnp(msgs) + parser.update_strings([bts]) + + for k, v in values.items(): + for key, val in v.items(): + self.assertAlmostEqual(parser.vl[k][key], val) + + # also check address + for sig in ("STEER_TORQUE", "STEER_TORQUE_REQUEST", "COUNTER", "CHECKSUM"): + self.assertEqual(parser.vl["STEERING_CONTROL"][sig], parser.vl[228][sig]) + + def test_scale_offset(self): + """Test that both scale and offset are correctly preserved""" + dbc_file = "honda_civic_touring_2016_can_generated" + msgs = [("VSA_STATUS", 50)] + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + for brake in range(100): + values = {"USER_BRAKE": brake} + msgs = packer.make_can_msg("VSA_STATUS", 0, values) + bts = can_list_to_can_capnp([msgs]) + + parser.update_strings([bts]) + + self.assertAlmostEqual(parser.vl["VSA_STATUS"]["USER_BRAKE"], brake) + + def test_subaru(self): + # Subaru is little endian + + dbc_file = "subaru_global_2017_generated" + + msgs = [("ES_LKAS", 50)] + + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + idx = 0 + for steer in range(-256, 255): + for active in [1, 0]: + values = { + "LKAS_Output": steer, + "LKAS_Request": active, + "SET_1": 1 + } + + msgs = packer.make_can_msg("ES_LKAS", 0, values) + bts = can_list_to_can_capnp([msgs]) + parser.update_strings([bts]) + + self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Output"], steer) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Request"], active) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["SET_1"], 1) + self.assertAlmostEqual(parser.vl["ES_LKAS"]["COUNTER"], idx % 16) + idx += 1 + + def test_bus_timeout(self): + """Test CAN bus timeout detection""" + dbc_file = "honda_civic_touring_2016_can_generated" + + freq = 100 + msgs = [("VSA_STATUS", freq), ("STEER_MOTOR_TORQUE", freq/2)] + + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + i = 0 + def send_msg(blank=False): + nonlocal i + i += 1 + t = i*((1 / freq) * 1e9) + + if blank: + msgs = [] + else: + msgs = [packer.make_can_msg("VSA_STATUS", 0, {}), ] + + can = can_list_to_can_capnp(msgs, logMonoTime=t) + parser.update_strings([can, ]) + + # all good, no timeout + for _ in range(1000): + send_msg() + self.assertFalse(parser.bus_timeout, str(_)) + + # timeout after 10 blank msgs + for n in range(200): + send_msg(blank=True) + self.assertEqual(n >= 10, parser.bus_timeout) + + # no timeout immediately after seen again + send_msg() + self.assertFalse(parser.bus_timeout) + + def test_updated(self): + """Test updated value dict""" + dbc_file = "honda_civic_touring_2016_can_generated" + msgs = [("VSA_STATUS", 50)] + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + # Make sure nothing is updated + self.assertEqual(len(parser.vl_all["VSA_STATUS"]["USER_BRAKE"]), 0) + + idx = 0 + for _ in range(10): + # Ensure CANParser holds the values of any duplicate messages over multiple frames + user_brake_vals = [random.randrange(100) for _ in range(random.randrange(5, 10))] + half_idx = len(user_brake_vals) // 2 + can_msgs = [[], []] + for frame, brake_vals in enumerate((user_brake_vals[:half_idx], user_brake_vals[half_idx:])): + for user_brake in brake_vals: + values = {"USER_BRAKE": user_brake} + can_msgs[frame].append(packer.make_can_msg("VSA_STATUS", 0, values)) + idx += 1 + + can_strings = [can_list_to_can_capnp(msgs) for msgs in can_msgs] + parser.update_strings(can_strings) + vl_all = parser.vl_all["VSA_STATUS"]["USER_BRAKE"] + + self.assertEqual(vl_all, user_brake_vals) + if len(user_brake_vals): + self.assertEqual(vl_all[-1], parser.vl["VSA_STATUS"]["USER_BRAKE"]) + + def test_timestamp_nanos(self): + """Test message timestamp dict""" + dbc_file = "honda_civic_touring_2016_can_generated" + + msgs = [ + ("VSA_STATUS", 50), + ("POWERTRAIN_DATA", 100), + ] + + parser = CANParser(dbc_file, msgs, 0) + packer = CANPacker(dbc_file) + + # Check the default timestamp is zero + for msg in ("VSA_STATUS", "POWERTRAIN_DATA"): + ts_nanos = parser.ts_nanos[msg].values() + self.assertEqual(set(ts_nanos), {0}) + + # Check: + # - timestamp is only updated for correct messages + # - timestamp is correct for multiple runs + # - timestamp is from the latest message if updating multiple strings + for _ in range(10): + can_strings = [] + log_mono_time = 0 + for i in range(10): + log_mono_time = int(0.01 * i * 1e+9) + can_msg = packer.make_can_msg("VSA_STATUS", 0, {}) + can_strings.append(can_list_to_can_capnp([can_msg], logMonoTime=log_mono_time)) + parser.update_strings(can_strings) + + ts_nanos = parser.ts_nanos["VSA_STATUS"].values() + self.assertEqual(set(ts_nanos), {log_mono_time}) + ts_nanos = parser.ts_nanos["POWERTRAIN_DATA"].values() + self.assertEqual(set(ts_nanos), {0}) + + def test_nonexistent_messages(self): + # Ensure we don't allow messages not in the DBC + existing_messages = ("STEERING_CONTROL", 228, "CAN_FD_MESSAGE", 245) + + for msg in existing_messages: + CANParser(TEST_DBC, [(msg, 0)]) + with self.assertRaises(RuntimeError): + new_msg = msg + "1" if isinstance(msg, str) else msg + 1 + CANParser(TEST_DBC, [(new_msg, 0)]) + + def test_track_all_signals(self): + parser = CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 0)]) + self.assertEqual(parser.vl["ACC_CONTROL"], { + "ACCEL_CMD": 0, + "ALLOW_LONG_PRESS": 0, + "ACC_MALFUNCTION": 0, + "RADAR_DIRTY": 0, + "DISTANCE": 0, + "MINI_CAR": 0, + "ACC_TYPE": 0, + "CANCEL_REQ": 0, + "ACC_CUT_IN": 0, + "LEAD_VEHICLE_STOPPED": 0, + "PERMIT_BRAKING": 0, + "RELEASE_STANDSTILL": 0, + "ITS_CONNECT_LEAD": 0, + "ACCEL_CMD_ALT": 0, + "CHECKSUM": 0, + }) + + def test_disallow_duplicate_messages(self): + CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5)]) + + with self.assertRaises(RuntimeError): + CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5), ("ACC_CONTROL", 10)]) + + with self.assertRaises(RuntimeError): + CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 10), ("ACC_CONTROL", 10)]) + + def test_allow_undefined_msgs(self): + # TODO: we should throw an exception for these, but we need good + # discovery tests in openpilot first + packer = CANPacker("toyota_nodsu_pt_generated") + + self.assertEqual(packer.make_can_msg("ACC_CONTROL", 0, {"UNKNOWN_SIGNAL": 0}), + [835, 0, b'\x00\x00\x00\x00\x00\x00\x00N', 0]) + self.assertEqual(packer.make_can_msg("UNKNOWN_MESSAGE", 0, {"UNKNOWN_SIGNAL": 0}), + [0, 0, b'', 0]) + self.assertEqual(packer.make_can_msg(0, 0, {"UNKNOWN_SIGNAL": 0}), + [0, 0, b'', 0]) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/can/tests/test_parser_performance.py b/opendbc/can/tests/test_parser_performance.py new file mode 100755 index 0000000000..2010fa4bf7 --- /dev/null +++ b/opendbc/can/tests/test_parser_performance.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +import time +import unittest + +from opendbc.can.parser import CANParser +from opendbc.can.packer import CANPacker +from opendbc.can.tests.test_packer_parser import can_list_to_can_capnp + + +@unittest.skip("TODO: varies too much between machines") +class TestParser(unittest.TestCase): + def _benchmark(self, checks, thresholds, n): + parser = CANParser('toyota_new_mc_pt_generated', checks, 0) + packer = CANPacker('toyota_new_mc_pt_generated') + + can_msgs = [] + for i in range(50000): + values = {"ACC_CONTROL": {"ACC_TYPE": 1, "ALLOW_LONG_PRESS": 3}} + msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()] + bts = can_list_to_can_capnp(msgs, logMonoTime=int(0.01 * i * 1e9)) + can_msgs.append(bts) + + ets = [] + for _ in range(25): + if n > 1: + strings = [] + for i in range(0, len(can_msgs), n): + strings.append(can_msgs[i:i + n]) + t1 = time.process_time_ns() + for m in strings: + parser.update_strings(m) + t2 = time.process_time_ns() + else: + t1 = time.process_time_ns() + for m in can_msgs: + parser.update_strings([m]) + t2 = time.process_time_ns() + + ets.append(t2 - t1) + + et = sum(ets) / len(ets) + avg_nanos = et / len(can_msgs) + print('%s: [%d] %.1fms to parse %s, avg: %dns' % (self._testMethodName, n, et/1e6, len(can_msgs), avg_nanos)) + + minn, maxx = thresholds + self.assertLess(avg_nanos, maxx) + self.assertGreater(avg_nanos, minn, "Performance seems to have improved, update test thresholds.") + + def test_performance_all_signals(self): + self._benchmark([('ACC_CONTROL', 10)], (10000, 19000), 1) + self._benchmark([('ACC_CONTROL', 10)], (1300, 5000), 10) + + +if __name__ == "__main__": + unittest.main() diff --git a/opendbc/ford_lincoln_base_pt.dbc b/opendbc/ford_lincoln_base_pt.dbc index ebe7a43213..828e4118ee 100644 --- a/opendbc/ford_lincoln_base_pt.dbc +++ b/opendbc/ford_lincoln_base_pt.dbc @@ -1756,6 +1756,14 @@ BO_ 126 SteeringPinion_Data: 8 PSCM SG_ StePinAn_No_Cs : 39|8@0+ (1,0) [0|255] "Unitless" VDM,ABS_ESC,TCCM,GWM SG_ StePinAn_No_Cnt : 47|4@0+ (1,0) [0|15] "unitless" VDM,TCCM,GWM,ABS_ESC +BO_ 133 SteeringPinion_Data_Alt: 8 PSCM + SG_ StePw_B_Rq : 23|1@0+ (1,0) [0|1] "SED" GWM,ECM_Diesel,PCM + SG_ StePinRelInit_An_Sns : 7|16@0+ (0.1,-3200) [-3200|3353.3] "degrees" VDM,IPMA_ADAS,GWM,ECM_Diesel,PCM,PCM_HEV,ABS_ESC,TCCM + SG_ StePinCompAnEst_D_Qf : 43|2@0+ (1,0) [0|3] "SED" VDM,CMR_DSMC,IPMA_ADAS,TCM_DSL,ECM_Diesel,PCM,PCM_HEV,ABS_ESC,TCCM,GWM + SG_ StePinComp_An_Est : 22|15@0+ (0.1,-1600) [-1600|1676.7] "degrees" VDM,CMR_DSMC,IPMA_ADAS,TCM_DSL,ECM_Diesel,PCM,PCM_HEV,ABS_ESC,TCCM,GWM + SG_ StePinAn_No_Cs : 39|8@0+ (1,0) [0|255] "Unitless" VDM,ABS_ESC,TCCM,GWM + SG_ StePinAn_No_Cnt : 47|4@0+ (1,0) [0|15] "unitless" VDM,TCCM,GWM,ABS_ESC + BO_ 1430 ABS_AutoSar_NetworkMgt: 8 ABS_ESC SG_ ABS_GWOnBoardTester : 39|8@0+ (1,0) [0|255] "unitless" GWM SG_ ABS_GWNMProxy : 47|8@0+ (1,0) [0|255] "unitless" GWM @@ -3608,6 +3616,9 @@ BO_ 922 DCACA_Data1_FD1: 8 GWM SG_ DcacClntFlw_D_Rq : 5|2@0+ (1,0) [0|3] "SED" SOBDMC_HPCM_FD1 SG_ CoolFanDcac_D_Rq : 7|2@0+ (1,0) [0|3] "SED" SOBDMC_HPCM_FD1 +BO_ 1082 INSTRUMENT_PANEL: 8 GWM + SG_ METRIC_UNITS : 54|1@0+ (1,0) [0|1] "SED" IPMA,PCM,PCM_HEV + BO_TX_BU_ 2612224016 : ECM_Diesel,PCM,PCM_HEV; BO_TX_BU_ 878 : PCM,PCM_HEV; BO_TX_BU_ 1085 : ECM_Diesel,PCM; @@ -3694,6 +3705,9 @@ CM_ SG_ 130 SteMdule_U_Meas "DCR 1745 to update Tx from GWM to EP100ms. SteMdul CM_ SG_ 130 SteMdule_I_Est "DCR 1745 to update Tx from GWM to EP100ms. SteMdule_I_Est & SteMdule_U_Meas are used by BMS system & filtered. The filter uses standard periodic rate & changing to EP would adversely affect the filter.(jweinfur)"; CM_ SG_ 126 StePinRelInit_An_Sns "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; CM_ SG_ 126 StePinAn_No_Cs "Signal not transmitted on gas variants."; +CM_ BO_ 133 "Seen on Ford Edge MK2"; +CM_ SG_ 133 StePinRelInit_An_Sns "SASM will transmit these signals on vehicles with SASM and without PSCM. SCCM will not Tx this signal when SASM is present."; +CM_ SG_ 133 StePinAn_No_Cs "Signal not transmitted on gas variants."; CM_ SG_ 1200 BrkTot_Tq_RqDrv "DCR 1836 to update Tx from GWM to EP100ms rejected due MPS6 TCM requires BrkTot_Tq_RqDrv at 20ms (bshu1)."; CM_ SG_ 1046 TCMode "Signal data set to 0x0 for ABS only vehicle option content"; CM_ SG_ 1046 DrvAntiLckLamp_D_Rq "update value table v8.34, not align w/GSDB, need etracker. ABS & IPC implemented as updated."; @@ -4938,6 +4952,11 @@ BA_ "GenMsgSendType" BO_ 126 0; BA_ "GenMsgCycleTime" BO_ 126 10; BA_ "GenMsgDelayTime" BO_ 126 0; BA_ "VFrameFormat" BO_ 126 14; +BA_ "FrameRouting" BO_ 133 ""; +BA_ "GenMsgSendType" BO_ 133 0; +BA_ "GenMsgCycleTime" BO_ 133 10; +BA_ "GenMsgDelayTime" BO_ 133 0; +BA_ "VFrameFormat" BO_ 133 14; BA_ "FrameRouting" BO_ 1430 ""; BA_ "GenMsgILSupport" BO_ 1430 0; BA_ "NmAsrMessage" BO_ 1430 1; @@ -11578,6 +11597,9 @@ VAL_ 130 SAPPAngleControlStat1 3 "Fault" 2 "Active" 1 "Open" 0 "Closed"; VAL_ 126 StePw_B_Rq 1 "Yes" 0 "No"; VAL_ 126 StePinRelInit_An_Sns 65535 "Faulty" 65534 "NoDataExists"; VAL_ 126 StePinCompAnEst_D_Qf 3 "OK" 2 "Degraded" 1 "No_Data_Exists" 0 "Faulty"; +VAL_ 133 StePw_B_Rq 1 "Yes" 0 "No"; +VAL_ 133 StePinRelInit_An_Sns 65535 "Faulty" 65534 "NoDataExists"; +VAL_ 133 StePinCompAnEst_D_Qf 3 "OK" 2 "Degraded" 1 "No_Data_Exists" 0 "Faulty"; VAL_ 1200 BrkHold_D_Stat 7 "NotUsed_3" 6 "NotUsed_2" 5 "NotUsed_1" 4 "HeldSecondary" 3 "HeldSecure" 2 "Held" 1 "Inactive" 0 "Off"; VAL_ 1200 HsaTrnAout_Tq_Rq 65535 "Fault" 65534 "Unknown"; VAL_ 1200 BrkBstrVac_P_Actl 127 "Invalid"; diff --git a/opendbc/gm_global_a_powertrain_generated.dbc b/opendbc/gm_global_a_powertrain_generated.dbc index 37ad918cb2..1c78dd7b07 100644 --- a/opendbc/gm_global_a_powertrain_generated.dbc +++ b/opendbc/gm_global_a_powertrain_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 NEO SG_ GAS_COMMAND : 7|16@0+ (0.125677,-75.909) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.251976,-76.601) [0|1] "" INTERCEPTOR @@ -188,6 +188,9 @@ BO_ 497 BCMGeneralPlatformStatus: 8 K9_BCM SG_ SystemBackUpPowerMode : 5|2@0+ (1,0) [0|3] "" XXX SG_ ParkBrakeSwActive : 36|1@0+ (1,0) [0|3] "" XXX +BO_ 500 SportMode: 6 XXX + SG_ SportMode : 15|1@0+ (1,0) [0|1] "" XXX + BO_ 501 ECMPRDNL2: 8 K20_ECM SG_ TransmissionState : 48|4@1+ (1,0) [0|7] "" NEO SG_ PRNDL2 : 27|4@0+ (1,0) [0|255] "" NEO diff --git a/opendbc/honda_civic_ex_2022_can_generated.dbc b/opendbc/honda_civic_ex_2022_can_generated.dbc index 581e7c66e4..8433001dcf 100644 --- a/opendbc/honda_civic_ex_2022_can_generated.dbc +++ b/opendbc/honda_civic_ex_2022_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_civic_touring_2016_can_generated.dbc b/opendbc/honda_civic_touring_2016_can_generated.dbc index 82271c417e..db6b21423f 100644 --- a/opendbc/honda_civic_touring_2016_can_generated.dbc +++ b/opendbc/honda_civic_touring_2016_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_crv_executive_2016_can_generated.dbc b/opendbc/honda_crv_executive_2016_can_generated.dbc index 272cf5dd36..c218b19ed5 100644 --- a/opendbc/honda_crv_executive_2016_can_generated.dbc +++ b/opendbc/honda_crv_executive_2016_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_crv_touring_2016_can_generated.dbc b/opendbc/honda_crv_touring_2016_can_generated.dbc index 1648442155..8b3f1b208b 100644 --- a/opendbc/honda_crv_touring_2016_can_generated.dbc +++ b/opendbc/honda_crv_touring_2016_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_fit_ex_2018_can_generated.dbc b/opendbc/honda_fit_ex_2018_can_generated.dbc index 1a80a9d4e2..05cd391c78 100644 --- a/opendbc/honda_fit_ex_2018_can_generated.dbc +++ b/opendbc/honda_fit_ex_2018_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_odyssey_exl_2018_generated.dbc b/opendbc/honda_odyssey_exl_2018_generated.dbc index bb33513b28..1dd222ed97 100644 --- a/opendbc/honda_odyssey_exl_2018_generated.dbc +++ b/opendbc/honda_odyssey_exl_2018_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc index b5664d9e74..b109a70b62 100644 --- a/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc +++ b/opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 512 GAS_COMMAND: 6 EON SG_ GAS_COMMAND : 7|16@0+ (0.253984064,-83.3) [0|1] "" INTERCEPTOR SG_ GAS_COMMAND2 : 23|16@0+ (0.126992032,-83.3) [0|1] "" INTERCEPTOR diff --git a/opendbc/hyundai_canfd.dbc b/opendbc/hyundai_canfd.dbc index 8d27594528..1ef67c044a 100644 --- a/opendbc/hyundai_canfd.dbc +++ b/opendbc/hyundai_canfd.dbc @@ -263,7 +263,7 @@ BO_ 416 SCC_CONTROL: 32 ADRV SG_ aReqValue : 128|11@1+ (0.01,-10.23) [-10.23|10.24] "m/s^2" XXX SG_ ZEROS_7 : 63|8@0+ (1,0) [0|255] "" XXX SG_ ACCMode : 68|3@1+ (1,0) [0|7] "" XXX - SG_ NEW_SIGNAL_12 : 35|9@1+ (0.1,0) [0|255] "" XXX + SG_ ACC_ObjRelSpd : 35|9@1+ (0.1,-16.4) [-16.4|34.7] "m/s" XXX SG_ JerkLowerLimit : 166|7@0+ (0.1,0) [0|12.7] "m/s^3" XXX SG_ StopReq : 184|1@0+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_15 : 192|11@1+ (0.1,0) [0|204.7] "m" XXX diff --git a/opendbc/pyproject.toml b/opendbc/pyproject.toml new file mode 100644 index 0000000000..035775acc4 --- /dev/null +++ b/opendbc/pyproject.toml @@ -0,0 +1,20 @@ +[tool.poetry] +name = "opendbc" +version = "1.0.0" +description = "CAN bus databases and tools" +license = "MIT" +authors = ["Vehicle Researcher "] +readme = "README.md" +repository = "https://github.com/commaai/opendbc" + +[tool.cython-lint] +max-line-length = 120 +ignore = ["E111", "E114"] + +# 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"] +line-length = 160 +target-version="py311" +flake8-implicit-str-concat.allow-multiline=false diff --git a/opendbc/requirements.txt b/opendbc/requirements.txt new file mode 100644 index 0000000000..10f8414dda --- /dev/null +++ b/opendbc/requirements.txt @@ -0,0 +1,8 @@ +ruff +Cython +Jinja2 +numpy +pycapnp +pyyaml +scons +pytest diff --git a/opendbc/site_scons/site_tools/cython.py b/opendbc/site_scons/site_tools/cython.py new file mode 100644 index 0000000000..c291475533 --- /dev/null +++ b/opendbc/site_scons/site_tools/cython.py @@ -0,0 +1,72 @@ +import re +import SCons +from SCons.Action import Action +from SCons.Scanner import Scanner + +pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M) +pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M) +cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M) + + +def pyx_scan(node, env, path, arg=None): + contents = node.get_text_contents() + + # from cimport ... + matches = pyx_from_import_re.findall(contents) + # cimport + matches += pyx_import_re.findall(contents) + + # Modules can be either .pxd or .pyx files + files = [m.replace('.', '/') + '.pxd' for m in matches] + files += [m.replace('.', '/') + '.pyx' for m in matches] + + # cdef extern from + files += cdef_import_re.findall(contents) + + # Handle relative imports + cur_dir = str(node.get_dir()) + files = [cur_dir + f if f.startswith('/') else f for f in files] + + # Filter out non-existing files (probably system imports) + files = [f for f in files if env.File(f).exists()] + return env.File(files) + + +pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True) +cythonAction = Action("$CYTHONCOM") + + +def create_builder(env): + try: + cython = env['BUILDERS']['Cython'] + except KeyError: + cython = SCons.Builder.Builder( + action=cythonAction, + emitter={}, + suffix=cython_suffix_emitter, + single_source=1 + ) + env.Append(SCANNERS=pyxscanner) + env['BUILDERS']['Cython'] = cython + return cython + +def cython_suffix_emitter(env, source): + return "$CYTHONCFILESUFFIX" + +def generate(env): + env["CYTHON"] = "cythonize" + env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + env["CYTHONCFILESUFFIX"] = ".cpp" + + c_file, _ = SCons.Tool.createCFileBuilders(env) + + c_file.suffix['.pyx'] = cython_suffix_emitter + c_file.add_action('.pyx', cythonAction) + + c_file.suffix['.py'] = cython_suffix_emitter + c_file.add_action('.py', cythonAction) + + create_builder(env) + +def exists(env): + return True diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc index db8d67e0bc..56624c3e54 100644 --- a/opendbc/tesla_can.dbc +++ b/opendbc/tesla_can.dbc @@ -131,6 +131,19 @@ BO_ 880 EPAS_sysStatus: 8 EPAS SG_ EPAS_sysStatusCounter : 51|4@0+ (1,0) [0|15] "" NEO SG_ EPAS_torsionBarTorque : 19|12@0+ (0.01,-20.5) [0|0] "Nm" NEO +BO_ 305 EPAS3P_sysStatus: 8 NEO + SG_ EPAS_currentTuneMode : 7|4@0+ (1,0) [0|0] "" NEO + SG_ EPAS_eacErrorCode : 23|4@0+ (1,0) [0|15] "" NEO + SG_ EPAS_eacStatus : 55|3@0+ (1,0) [0|7] "" NEO + SG_ EPAS_handsOnLevel : 39|2@0+ (1,0) [0|3] "" NEO + SG_ EPAS_internalSAS : 37|14@0+ (0.1,-819.2) [-819.2|819] "deg" NEO + SG_ EPAS_steeringFault : 2|1@0+ (1,0) [0|1] "" NEO + SG_ EPAS_steeringRackForce : 1|10@0+ (50,-25575) [-25575|25575] "N" NEO + SG_ EPAS_steeringReduced : 3|1@0+ (1,0) [0|1] "" NEO + SG_ EPAS_sysStatusChecksum : 63|8@0+ (1,0) [0|255] "" NEO + SG_ EPAS_sysStatusCounter : 51|4@0+ (1,0) [0|15] "" NEO + SG_ EPAS_torsionBarTorque : 19|12@0+ (0.01,-20.5) [-20.5|20.45] "Nm" NEO + BO_ 3 STW_ANGL_STAT: 8 STW SG_ StW_Angl : 5|14@0+ (0.5,-2048) [0|0] "deg" NEO SG_ StW_AnglSpd : 21|14@0+ (0.5,-2048) [0|0] "/s" NEO @@ -708,6 +721,10 @@ BO_ 1001 DAS_bodyControls: 8 XXX SG_ DAS_bodyControlsCounter : 52|4@1+ (1,0) [0|15] "" XXX SG_ DAS_bodyControlsChecksum : 56|8@1+ (1,0) [0|255] "" XXX +BO_ 780 DriverSeat: 8 XXX + SG_ occupancyStatus : 16|3@1+ (1,0) [0|7] "" XXX + SG_ buckleStatus : 19|2@1+ (1,0) [0|3] "" XXX + VAL_ 3 StW_Angl 16383 "SNA" ; VAL_ 3 StW_AnglSens_Id 2 "MUST" 0 "PSBL" 1 "SELF" ; VAL_ 3 StW_AnglSens_Stat 2 "ERR" 3 "ERR_INI" 1 "INI" 0 "OK" ; @@ -802,6 +819,8 @@ VAL_ 697 DAS_jerkMin 511 "SNA" ; VAL_ 697 DAS_jerkMax 255 "SNA" ; VAL_ 697 DAS_accelMin 511 "SNA" ; VAL_ 697 DAS_accelMax 511 "SNA" ; +VAL_ 780 occupancyStatus 1 "OCCUPIED" 0 "UNOCCUPIED" ; +VAL_ 780 buckleStatus 1 "LATCHED" 0 "UNLATCHED" ; VAL_ 792 BOOT_STATE 2 "Init" 3 "SNA" 0 "closed" 1 "open" ; VAL_ 792 CERRD 1 "CAN error detect" 0 "no Can error detected" ; VAL_ 792 DAY 1 "Init" 0 "SNA" ; diff --git a/opendbc/tesla_radar.dbc b/opendbc/tesla_radar.dbc deleted file mode 100644 index e7b9cbc055..0000000000 --- a/opendbc/tesla_radar.dbc +++ /dev/null @@ -1,1371 +0,0 @@ -VERSION "" - - -NS_ : - NS_DESC_ - CM_ - BA_DEF_ - BA_ - VAL_ - CAT_DEF_ - CAT_ - FILTER - BA_DEF_DEF_ - EV_DATA_ - ENVVAR_DATA_ - SGTYPE_ - SGTYPE_VAL_ - BA_DEF_SGTYPE_ - BA_SGTYPE_ - SIG_TYPE_REF_ - VAL_TABLE_ - SIG_GROUP_ - SIG_VALTYPE_ - SIGTYPE_VALTYPE_ - BO_TX_BU_ - BA_DEF_REL_ - BA_REL_ - BA_DEF_DEF_REL_ - BU_SG_REL_ - BU_EV_REL_ - BU_BO_REL_ - SG_MUL_VAL_ - -BS_: - -BU_: FrontCamera Radar - - -BO_ 769 TeslaRadarSguInfo: 8 Radar - SG_ RADC_VerticalMisalignment : 0|8@1+ (1,0) [0|255] "" FrontCamera - SG_ RADC_SCUTemperature : 8|8@1+ (1,-128) [-128|127] "" FrontCamera - SG_ RADC_VMA_Plaus : 16|8@1+ (1,0) [0|255] "" FrontCamera - SG_ RADC_SGU_ITC : 24|8@1+ (1,0) [0|255] "" FrontCamera - SG_ RADC_HorizontMisalignment : 32|12@1+ (1,0) [0|4096] "" FrontCamera - SG_ RADC_SensorDirty : 44|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_HWFail : 45|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_SGUFail : 46|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_SGUInfoConsistBit : 47|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 770 TeslaRadarTguInfo: 8 Radar - SG_ RADC_ACCTargObj1_sguIndex : 0|6@1+ (1,0) [0|63] "" FrontCamera - SG_ RADC_ACCTargObj2_sguIndex : 6|6@1+ (1,0) [0|63] "" FrontCamera - SG_ RADC_ACCTargObj3_sguIndex : 12|6@1+ (1,0) [0|63] "" FrontCamera - SG_ RADC_ACCTargObj4_sguIndex : 18|6@1+ (1,0) [0|63] "" FrontCamera - SG_ RADC_ACCTargObj5_sguIndex : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ unused30 : 30|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_TGUInfoConsistBit : 31|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_ACCTargObj1_dBPower : 32|16@1+ (1,0) [0|65535] "" FrontCamera - SG_ RADC_ACCTargObj5_dBPower : 48|16@1+ (1,0) [0|65535] "" FrontCamera - -BO_ 1281 TeslaRadarAlertMatrix: 8 Radar - SG_ RADC_a001_ecuInternalPerf : 0|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a002_flashPerformance : 1|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a003_vBatHigh : 2|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a004_adjustmentNotDone : 3|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a005_adjustmentReq : 4|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a006_adjustmentNotOk : 5|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a007_sensorBlinded : 6|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a008_plantModeActive : 7|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a009_configMismatch : 8|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a010_canBusOff : 9|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a011_bdyMIA : 10|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a012_espMIA : 11|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a013_gtwMIA : 12|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a014_sccmMIA : 13|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a015_adasMIA : 14|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a016_bdyInvalidCount : 15|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a017_adasInvalidCount : 16|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a018_espInvalidCount : 17|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a019_sccmInvalidCount : 18|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a020_bdyInvalidChkSm : 19|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a021_espInvalidChkSm : 20|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a022_sccmInvalidChkSm : 21|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a023_sccmInvalidChkSm : 22|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a024_absValidity : 23|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a025_ambTValidity : 24|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a026_brakeValidity : 25|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a027_CntryCdValidity : 26|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a028_espValidity : 27|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a029_longAccOffValidity : 28|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a030_longAccValidity : 29|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a031_odoValidity : 30|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a032_gearValidity : 31|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a033_steerAngValidity : 32|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a034_steerAngSpdValidity : 33|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a035_indctrValidity : 34|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a036_vehStandStillValidity : 35|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a037_vinValidity : 36|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a038_whlRotValidity : 37|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a039_whlSpdValidity : 38|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a040_whlStandStillValidity : 39|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a041_wiperValidity : 40|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a042_xwdValidity : 41|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a043_yawOffValidity : 42|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a044_yawValidity : 43|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a045_bsdSanity : 44|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a046_rctaSanity : 45|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a047_lcwSanity : 46|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a048_steerAngOffSanity : 47|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a049_tireSizeSanity : 48|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a050_velocitySanity : 49|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a051_yawSanity : 50|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a052_radomeHtrInop : 51|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a053_espmodValidity : 52|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a054_gtwmodValidity : 53|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a055_stwmodValidity : 54|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a056_bcmodValidity : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a057_dimodValidity : 56|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a058_opmodValidity : 57|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a059_drmiInvalidChkSm : 58|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a060_drmiInvalidCount : 59|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a061_radPositionMismatch : 60|1@1+ (1,0) [0|1] "" FrontCamera - SG_ RADC_a062_strRackMismatch : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ unused62 : 62|2@1+ (1,0) [0|3] "" FrontCamera - -BO_ 784 M_310hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 785 M_310hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 787 M_313hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 788 M_313hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 790 M_316hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 791 M_316hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 793 M_319hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 794 M_319hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 796 M_31Chex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 797 M_31Chex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 799 M_31Fhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 800 M_31Fhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 802 M_322hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 803 M_322hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 805 M_325hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 806 M_325hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 808 M_328hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 809 M_328hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 811 M_32Bhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 812 M_32Bhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 814 M_32Ehex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 815 M_32Ehex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 817 M_331hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 818 M_331hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 820 M_334hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 821 M_334hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 823 M_337hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 824 M_337hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 826 M_33Ahex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 827 M_33Ahex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 829 M_33Dhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 830 M_33Dhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 832 M_340hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 833 M_340hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 835 M_343hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 836 M_343hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 838 M_346hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 839 M_346hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 841 M_349hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 842 M_349hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 844 M_34Chex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 845 M_34Chex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 847 M_34Fhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 848 M_34Fhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 850 M_352hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 851 M_352hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 853 M_355hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 854 M_355hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 856 M_358hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 857 M_358hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 859 M_35Bhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 860 M_35Bhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 862 M_35Ehex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 863 M_35Ehex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 865 M_361hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 866 M_361hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 868 M_364hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 869 M_364hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 871 M_367hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 872 M_367hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 874 M_36Ahex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 875 M_36Ahex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 877 M_36Dhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 878 M_36Dhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 881 L_1_371hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 882 L_1_371hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 884 L_2_374hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 885 L_2_375hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 887 L_3_377hex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 888 L_3_378hex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 890 L_4_37ahex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 891 L_4_37ahex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 893 L_5_37dhex: 8 Radar - SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" FrontCamera - SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" FrontCamera - SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" FrontCamera - SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" FrontCamera - SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Valid : 55|1@1+ (1,0) [0|1] "" FrontCamera - SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Meas : 61|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Tracked : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 894 L_5_37dhex2: 8 Radar - SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" FrontCamera - SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" FrontCamera - SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" FrontCamera - SG_ MovingState : 22|2@1+ (1,0) [0|3] "" FrontCamera - SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" FrontCamera - SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" FrontCamera - SG_ axSigma : 36|6@1+ (1,0) [0|63] "" FrontCamera - SG_ dySigma : 42|6@1+ (1,0) [0|63] "" FrontCamera - SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" FrontCamera - SG_ Class : 53|3@1+ (1,0) [0|7] "" FrontCamera - SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" FrontCamera - SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" FrontCamera - SG_ Index2 : 63|1@1+ (1,0) [0|1] "" FrontCamera - -BO_ 697 VIN_VIP_405HS: 8 FrontCamera - SG_ VIN_MuxID M : 0|8@1+ (1,0) [0|0] "" Radar - SG_ VIN_Part1 m16 : 47|24@0+ (1,0) [0|16777215] "" Radar - SG_ VIN_Part2 m17 : 15|56@0+ (1,0) [0|7.2057594038E+16] "" Radar - SG_ VIN_Part3 m18 : 15|56@0+ (1,0) [0|7.2057594038E+16] "" Radar - -BO_ 681 Msg2A9_GTW_carConfig: 8 FrontCamera - SG_ Msg2A9_Always0x02 : 48|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Always0x10 : 56|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Always0x16 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Always0x41 : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Value1_0x02 : 0|3@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_FourWheelDrive : 3|2@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Value2_0x02 : 5|3@1+ (1,0) [0|0] "" Radar - SG_ Msg2A9_Always0x43 : 16|8@1+ (1,0) [0|0] "" Radar - -BO_ 409 Msg199_STW_ANGLHP_STAT: 8 FrontCamera - SG_ Msg199Always0x04 : 32|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Always0x20 : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Always0x2F : 0|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Always0x67 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Always0xFF : 40|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Checksum : 56|8@1+ (1,0) [0|0] "" Radar - SG_ Msg199Counter : 52|4@1+ (1,0) [0|0] "" Radar - -BO_ 361 Msg169_ESP_wheelSpeeds: 8 FrontCamera - SG_ ESP_wheelSpeedFrL_HS : 0|13@1+ (0.04,0) [0|327.64] "km/h" Radar - SG_ ESP_wheelSpeedFrR_HS : 13|13@1+ (0.04,0) [0|327.64] "km/h" Radar - SG_ ESP_wheelSpeedReL_HS : 26|13@1+ (0.04,0) [0|327.64] "km/h" Radar - SG_ ESP_wheelSpeedReR_HS : 39|13@1+ (0.04,0) [0|327.64] "km/h" Radar - SG_ Msg169Checksum : 56|8@1+ (1,0) [0|0] "" Radar - SG_ Msg169Counter : 52|4@1+ (1,0) [0|0] "" Radar - -BO_ 345 Msg159_ESP_C: 8 FrontCamera - SG_ Msg159Always0x3A : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg159Always0xA5 : 0|8@1+ (1,0) [0|0] "" Radar - SG_ Msg159Always0xCF : 32|8@1+ (1,0) [0|0] "" Radar - SG_ Msg159Always0xF4 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg159Counter : 44|4@1+ (1,0) [0|0] "" Radar - SG_ Msg159Checksum : 24|8@1+ (1,0) [0|0] "" Radar - -BO_ 329 Msg149_ESP_145h: 8 FrontCamera - SG_ Msg149Always0x02 : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Always0x04 : 40|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Always0x26 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Always0x6A : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Always0xAA : 32|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Always0xF : 48|4@1+ (1,0) [0|0] "" Radar - SG_ Msg149Checksum : 56|8@1+ (1,0) [0|0] "" Radar - SG_ Msg149Counter : 52|4@1+ (1,0) [0|0] "" Radar - -BO_ 297 Msg129_ESP_115h: 6 FrontCamera - SG_ Msg129Always0x20 : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg129Checksum : 40|8@1+ (1,0) [0|0] "" Radar - SG_ Msg129Counter : 36|4@1+ (1,0) [0|0] "" Radar - -BO_ 281 Msg119_DI_torque2: 6 FrontCamera - SG_ Msg119Always0x11 : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg119Always0x1F : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg119Always0x8 : 36|4@1+ (1,0) [0|0] "" Radar - SG_ Msg119Always0xF4 : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg119Always0xFF : 0|8@1+ (1,0) [0|0] "" Radar - SG_ Msg119Checksum : 40|8@1+ (1,0) [0|0] "" Radar - SG_ Msg119Counter : 32|4@1+ (1,0) [0|0] "" Radar - -BO_ 265 Msg109_DI_torque1: 8 FrontCamera - SG_ Msg109Always0x80 : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg109Checksum : 56|8@1+ (1,0) [0|0] "" Radar - SG_ Msg109Counter : 13|3@1+ (1,0) [0|0] "" Radar - -BO_ 521 Msg209_GTW_odo: 8 FrontCamera - SG_ Msg209Always0x61 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg209Always0x94 : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg209Always0x52 : 24|8@1+ (1,0) [0|0] "" Radar - SG_ Msg209Always0x13 : 32|8@1+ (1,0) [0|0] "" Radar - SG_ Msg209Always0x03 : 40|8@1+ (1,0) [0|0] "" Radar - SG_ Msg209Always0x80 : 48|8@1+ (1,0) [0|0] "" Radar - -BO_ 537 Msg219_STW_ACTN_RQ: 8 FrontCamera - SG_ Msg219Counter : 52|4@1+ (1,0) [0|15] "" Radar - SG_ Msg219CRC : 56|8@1+ (1,0) [0|0] "" Radar - -BO_ 425 Msg1A9_DI_espControl: 5 FrontCamera - SG_ Msg1A9Always0x0C : 16|8@1+ (1,0) [0|0] "" Radar - SG_ Msg1A9Counter : 28|4@1+ (1,0) [0|0] "" Radar - SG_ Msg1A9Checksum : 32|8@1+ (1,0) [0|0] "" Radar - -BO_ 729 Msg2D9_BC_status : 8 FrontCamera - SG_ Msg2D9Always0x80 : 0|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2D9Always0x40 : 8|8@1+ (1,0) [0|0] "" Radar - SG_ Msg2D9Always0x83 : 16|8@1+ (1,0) [0|0] "" Radar - -BO_ 1601 UDS_radarRequest: 8 FrontCamera - SG_ UDS_radarRequestData : 7|64@0+ (1,0) [0|0] "" Radar - -BO_ 1617 Radar_udsResponse: 8 Radar - SG_ Radar_udsResponseData : 7|64@0+ (1,0) [0|0] "" FrontCamera - -CM_ BO_ 697 "Start with MuxID 0x12, then 0x11 and finally 0x10 (VIN is then transmitted in the reverse order)"; -CM_ BO_ 681 "Message sent every 1000 ms. All fixed bytes, no checksum, the byte for RWD or AWD needs to match VIN config"; -CM_ BO_ 409 "Message sent every 10ms. Checksum : use all first 7 bytes with the SAE J1850 CRC algo"; -CM_ BO_ 361 "Message sent every 10ms. Checksum : Sum of all first 7 bytes + 0x76"; -CM_ BO_ 345 "Message sent every 20ms. Checksum : Sum of all first bytes + 0xc; place checksum in 4th octet"; -CM_ BO_ 329 "Message sent every 20ms. Checksum : Sum of all first 7 bytes + 0x46"; -CM_ BO_ 297 "Message sent every 20ms. Checksum : Sum of all first 5 bytes + 0x16"; -CM_ BO_ 281 "Message sent every 10ms. Checksum : Sum of all first 5 bytes + 0x17"; -CM_ BO_ 265 "Message sent every 10ms. Checksum : Sum of all first 7 bytes + 0x7"; -CM_ BO_ 521 "Message sent every 100ms. All fixed bytes, no checksum."; -CM_ BO_ 537 "Message sent every 100ms. Checksum : use all first 7 bytes with the SAE J1850 CRC algo"; -CM_ BO_ 425 "Message sent every 20ms. Checksum : Sum of all first 4 bytes + 0x38"; -CM_ BO_ 729 "Message sent every 1000ms. All fixed bytes, no checksum."; - -BA_DEF_ "BusType" STRING ; -BA_DEF_ BO_ "GenMsgCycleTime" INT 0 0; -BA_DEF_ SG_ "FieldType" STRING ; - -BA_DEF_DEF_ "BusType" "CAN"; -BA_DEF_DEF_ "FieldType" ""; -BA_DEF_DEF_ "GenMsgCycleTime" 0; - -BA_ "GenMsgCycleTime" BO_ 697 250; -BA_ "GenMsgCycleTime" BO_ 681 1000; -BA_ "GenMsgCycleTime" BO_ 409 10; -BA_ "GenMsgCycleTime" BO_ 361 10; -BA_ "GenMsgCycleTime" BO_ 345 20; -BA_ "GenMsgCycleTime" BO_ 329 20; -BA_ "GenMsgCycleTime" BO_ 297 20; -BA_ "GenMsgCycleTime" BO_ 281 10; -BA_ "GenMsgCycleTime" BO_ 265 10; -BA_ "GenMsgCycleTime" BO_ 521 100; -BA_ "GenMsgCycleTime" BO_ 537 100; -BA_ "GenMsgCycleTime" BO_ 425 20; -BA_ "GenMsgCycleTime" BO_ 729 1000; - -VAL_ 681 Msg2A9_FourWheelDrive 3 "SNA" 2 "UNUSED" 1 "4WD" 0 "2WD" ; -VAL_ 785 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 785 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 788 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 788 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 791 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 791 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 794 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 794 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 797 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 797 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 800 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 800 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 803 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 803 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 806 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 806 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 809 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 809 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 812 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 812 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 815 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 815 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 818 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 818 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 821 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 821 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 824 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 824 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 827 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 827 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 830 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 830 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 833 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 833 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 836 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 836 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 839 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 839 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 842 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 842 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 845 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 845 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 848 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 848 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 851 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 851 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 854 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 854 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 857 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 857 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 860 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 860 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 863 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 863 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 866 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 866 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 869 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 869 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 872 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 872 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 875 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 875 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 878 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 878 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 882 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 882 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 885 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 885 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 888 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 888 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 891 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 891 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; -VAL_ 894 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; -VAL_ 894 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; - diff --git a/opendbc/tesla_radar_bosch_generated.dbc b/opendbc/tesla_radar_bosch_generated.dbc new file mode 100644 index 0000000000..cdd03908a5 --- /dev/null +++ b/opendbc/tesla_radar_bosch_generated.dbc @@ -0,0 +1,1373 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; + +CM_ "tesla_radar_bosch.dbc starts here"; + +VERSION "" + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: Autopilot Radar Diag + + +BO_ 769 TeslaRadarSguInfo: 8 Radar + SG_ RADC_VerticalMisalignment : 0|8@1+ (1,0) [0|255] "" Autopilot + SG_ RADC_SCUTemperature : 8|8@1+ (1,-128) [-128|127] "" Autopilot + SG_ RADC_VMA_Plaus : 16|8@1+ (1,0) [0|255] "" Autopilot + SG_ RADC_SGU_ITC : 24|8@1+ (1,0) [0|255] "" Autopilot + SG_ RADC_HorizontMisalignment : 32|12@1+ (1,0) [0|4096] "" Autopilot + SG_ RADC_SensorDirty : 44|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_HWFail : 45|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_SGUFail : 46|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_SGUInfoConsistBit : 47|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 770 TeslaRadarTguInfo: 8 Radar + SG_ RADC_ACCTargObj1_sguIndex : 0|6@1+ (1,0) [0|63] "" Autopilot + SG_ RADC_ACCTargObj2_sguIndex : 6|6@1+ (1,0) [0|63] "" Autopilot + SG_ RADC_ACCTargObj3_sguIndex : 12|6@1+ (1,0) [0|63] "" Autopilot + SG_ RADC_ACCTargObj4_sguIndex : 18|6@1+ (1,0) [0|63] "" Autopilot + SG_ RADC_ACCTargObj5_sguIndex : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ unused30 : 30|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_TGUInfoConsistBit : 31|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_ACCTargObj1_dBPower : 32|16@1+ (1,0) [0|65535] "" Autopilot + SG_ RADC_ACCTargObj5_dBPower : 48|16@1+ (1,0) [0|65535] "" Autopilot + +BO_ 1281 TeslaRadarAlertMatrix: 8 Radar + SG_ RADC_a001_ecuInternalPerf : 0|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a002_flashPerformance : 1|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a003_vBatHigh : 2|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a004_adjustmentNotDone : 3|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a005_adjustmentReq : 4|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a006_adjustmentNotOk : 5|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a007_sensorBlinded : 6|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a008_plantModeActive : 7|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a009_configMismatch : 8|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a010_canBusOff : 9|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a011_bdyMIA : 10|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a012_espMIA : 11|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a013_gtwMIA : 12|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a014_sccmMIA : 13|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a015_adasMIA : 14|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a016_bdyInvalidCount : 15|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a017_adasInvalidCount : 16|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a018_espInvalidCount : 17|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a019_sccmInvalidCount : 18|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a020_bdyInvalidChkSm : 19|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a021_espInvalidChkSm : 20|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a022_sccmInvalidChkSm : 21|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a023_sccmInvalidChkSm : 22|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a024_absValidity : 23|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a025_ambTValidity : 24|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a026_brakeValidity : 25|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a027_CntryCdValidity : 26|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a028_espValidity : 27|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a029_longAccOffValidity : 28|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a030_longAccValidity : 29|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a031_odoValidity : 30|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a032_gearValidity : 31|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a033_steerAngValidity : 32|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a034_steerAngSpdValidity : 33|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a035_indctrValidity : 34|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a036_vehStandStillValidity : 35|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a037_vinValidity : 36|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a038_whlRotValidity : 37|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a039_whlSpdValidity : 38|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a040_whlStandStillValidity : 39|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a041_wiperValidity : 40|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a042_xwdValidity : 41|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a043_yawOffValidity : 42|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a044_yawValidity : 43|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a045_bsdSanity : 44|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a046_rctaSanity : 45|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a047_lcwSanity : 46|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a048_steerAngOffSanity : 47|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a049_tireSizeSanity : 48|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a050_velocitySanity : 49|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a051_yawSanity : 50|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a052_radomeHtrInop : 51|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a053_espmodValidity : 52|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a054_gtwmodValidity : 53|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a055_stwmodValidity : 54|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a056_bcmodValidity : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a057_dimodValidity : 56|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a058_opmodValidity : 57|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a059_drmiInvalidChkSm : 58|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a060_drmiInvalidCount : 59|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a061_radPositionMismatch : 60|1@1+ (1,0) [0|1] "" Autopilot + SG_ RADC_a062_strRackMismatch : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ unused62 : 62|2@1+ (1,0) [0|3] "" Autopilot + +BO_ 784 RadarPoint0_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 785 RadarPoint0_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 787 RadarPoint1_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 788 RadarPoint1_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 790 RadarPoint2_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 791 RadarPoint2_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 793 RadarPoint3_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 794 RadarPoint3_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 796 RadarPoint4_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 797 RadarPoint4_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 799 RadarPoint5_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 800 RadarPoint5_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 802 RadarPoint6_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 803 RadarPoint6_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 805 RadarPoint7_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 806 RadarPoint7_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 808 RadarPoint8_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 809 RadarPoint8_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 811 RadarPoint9_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 812 RadarPoint9_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 814 RadarPoint10_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 815 RadarPoint10_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 817 RadarPoint11_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 818 RadarPoint11_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 820 RadarPoint12_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 821 RadarPoint12_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 823 RadarPoint13_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 824 RadarPoint13_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 826 RadarPoint14_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 827 RadarPoint14_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 829 RadarPoint15_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 830 RadarPoint15_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 832 RadarPoint16_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 833 RadarPoint16_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 835 RadarPoint17_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 836 RadarPoint17_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 838 RadarPoint18_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 839 RadarPoint18_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 841 RadarPoint19_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 842 RadarPoint19_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 844 RadarPoint20_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 845 RadarPoint20_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 847 RadarPoint21_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 848 RadarPoint21_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 850 RadarPoint22_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 851 RadarPoint22_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 853 RadarPoint23_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 854 RadarPoint23_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 856 RadarPoint24_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 857 RadarPoint24_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 859 RadarPoint25_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 860 RadarPoint25_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 862 RadarPoint26_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 863 RadarPoint26_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 865 RadarPoint27_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 866 RadarPoint27_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 868 RadarPoint28_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 869 RadarPoint28_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 871 RadarPoint29_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 872 RadarPoint29_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 874 RadarPoint30_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 875 RadarPoint30_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 877 RadarPoint31_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 878 RadarPoint31_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 881 ProcessedRadarPoint1_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 882 ProcessedRadarPoint1_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 884 ProcessedRadarPoint2_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 885 ProcessedRadarPoint2_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 887 ProcessedRadarPoint3_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 888 ProcessedRadarPoint3_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 890 ProcessedRadarPoint4_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 891 ProcessedRadarPoint4_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 893 ProcessedRadarPoint5_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 894 ProcessedRadarPoint5_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 697 VIN_VIP_405HS: 8 Autopilot + SG_ VIN_MuxID M : 0|8@1+ (1,0) [0|0] "" Radar + SG_ VIN_Part1 m16 : 47|24@0+ (1,0) [0|16777215] "" Radar + SG_ VIN_Part2 m17 : 15|56@0+ (1,0) [0|7.2057594038E+16] "" Radar + SG_ VIN_Part3 m18 : 15|56@0+ (1,0) [0|7.2057594038E+16] "" Radar + +BO_ 681 Msg2A9_GTW_carConfig: 8 Autopilot + SG_ Msg2A9_Always0x02 : 48|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Always0x10 : 56|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Always0x16 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Always0x41 : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Value1_0x02 : 0|3@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_FourWheelDrive : 3|2@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Value2_0x02 : 5|3@1+ (1,0) [0|0] "" Radar + SG_ Msg2A9_Always0x43 : 16|8@1+ (1,0) [0|0] "" Radar + +BO_ 409 Msg199_STW_ANGLHP_STAT: 8 Autopilot + SG_ Msg199Always0x04 : 32|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Always0x20 : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Always0x2F : 0|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Always0x67 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Always0xFF : 40|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Checksum : 56|8@1+ (1,0) [0|0] "" Radar + SG_ Msg199Counter : 52|4@1+ (1,0) [0|0] "" Radar + +BO_ 361 Msg169_ESP_wheelSpeeds: 8 Autopilot + SG_ ESP_wheelSpeedFrL_HS : 0|13@1+ (0.04,0) [0|327.64] "km/h" Radar + SG_ ESP_wheelSpeedFrR_HS : 13|13@1+ (0.04,0) [0|327.64] "km/h" Radar + SG_ ESP_wheelSpeedReL_HS : 26|13@1+ (0.04,0) [0|327.64] "km/h" Radar + SG_ ESP_wheelSpeedReR_HS : 39|13@1+ (0.04,0) [0|327.64] "km/h" Radar + SG_ Msg169Checksum : 56|8@1+ (1,0) [0|0] "" Radar + SG_ Msg169Counter : 52|4@1+ (1,0) [0|0] "" Radar + +BO_ 345 Msg159_ESP_C: 8 Autopilot + SG_ Msg159Always0x3A : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg159Always0xA5 : 0|8@1+ (1,0) [0|0] "" Radar + SG_ Msg159Always0xCF : 32|8@1+ (1,0) [0|0] "" Radar + SG_ Msg159Always0xF4 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg159Counter : 44|4@1+ (1,0) [0|0] "" Radar + SG_ Msg159Checksum : 24|8@1+ (1,0) [0|0] "" Radar + +BO_ 329 Msg149_ESP_145h: 8 Autopilot + SG_ Msg149Always0x02 : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Always0x04 : 40|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Always0x26 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Always0x6A : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Always0xAA : 32|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Always0xF : 48|4@1+ (1,0) [0|0] "" Radar + SG_ Msg149Checksum : 56|8@1+ (1,0) [0|0] "" Radar + SG_ Msg149Counter : 52|4@1+ (1,0) [0|0] "" Radar + +BO_ 297 Msg129_ESP_115h: 6 Autopilot + SG_ Msg129Always0x20 : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg129Checksum : 40|8@1+ (1,0) [0|0] "" Radar + SG_ Msg129Counter : 36|4@1+ (1,0) [0|0] "" Radar + +BO_ 281 Msg119_DI_torque2: 6 Autopilot + SG_ Msg119Always0x11 : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg119Always0x1F : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg119Always0x8 : 36|4@1+ (1,0) [0|0] "" Radar + SG_ Msg119Always0xF4 : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg119Always0xFF : 0|8@1+ (1,0) [0|0] "" Radar + SG_ Msg119Checksum : 40|8@1+ (1,0) [0|0] "" Radar + SG_ Msg119Counter : 32|4@1+ (1,0) [0|0] "" Radar + +BO_ 265 Msg109_DI_torque1: 8 Autopilot + SG_ Msg109Always0x80 : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg109Checksum : 56|8@1+ (1,0) [0|0] "" Radar + SG_ Msg109Counter : 13|3@1+ (1,0) [0|0] "" Radar + +BO_ 521 Msg209_GTW_odo: 8 Autopilot + SG_ Msg209Always0x61 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg209Always0x94 : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg209Always0x52 : 24|8@1+ (1,0) [0|0] "" Radar + SG_ Msg209Always0x13 : 32|8@1+ (1,0) [0|0] "" Radar + SG_ Msg209Always0x03 : 40|8@1+ (1,0) [0|0] "" Radar + SG_ Msg209Always0x80 : 48|8@1+ (1,0) [0|0] "" Radar + +BO_ 537 Msg219_STW_ACTN_RQ: 8 Autopilot + SG_ Msg219Counter : 52|4@1+ (1,0) [0|15] "" Radar + SG_ Msg219CRC : 56|8@1+ (1,0) [0|0] "" Radar + +BO_ 425 Msg1A9_DI_espControl: 5 Autopilot + SG_ Msg1A9Always0x0C : 16|8@1+ (1,0) [0|0] "" Radar + SG_ Msg1A9Counter : 28|4@1+ (1,0) [0|0] "" Radar + SG_ Msg1A9Checksum : 32|8@1+ (1,0) [0|0] "" Radar + +BO_ 729 Msg2D9_BC_status : 8 Autopilot + SG_ Msg2D9Always0x80 : 0|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2D9Always0x40 : 8|8@1+ (1,0) [0|0] "" Radar + SG_ Msg2D9Always0x83 : 16|8@1+ (1,0) [0|0] "" Radar + +BO_ 1601 UDS_radarRequest: 8 Diag + SG_ UDS_radarRequestData : 7|64@0+ (1,0) [0|0] "" Radar + +BO_ 1617 Radar_udsResponse: 8 Radar + SG_ Radar_udsResponseData : 7|64@0+ (1,0) [0|0] "" Diag + +CM_ BO_ 697 "Start with MuxID 0x12, then 0x11 and finally 0x10 (VIN is then transmitted in the reverse order)"; +CM_ BO_ 681 "Message sent every 1000 ms. All fixed bytes, no checksum, the byte for RWD or AWD needs to match VIN config"; +CM_ BO_ 409 "Message sent every 10ms. Checksum : use all first 7 bytes with the SAE J1850 CRC algo"; +CM_ BO_ 361 "Message sent every 10ms. Checksum : Sum of all first 7 bytes + 0x76"; +CM_ BO_ 345 "Message sent every 20ms. Checksum : Sum of all first bytes + 0xc; place checksum in 4th octet"; +CM_ BO_ 329 "Message sent every 20ms. Checksum : Sum of all first 7 bytes + 0x46"; +CM_ BO_ 297 "Message sent every 20ms. Checksum : Sum of all first 5 bytes + 0x16"; +CM_ BO_ 281 "Message sent every 10ms. Checksum : Sum of all first 5 bytes + 0x17"; +CM_ BO_ 265 "Message sent every 10ms. Checksum : Sum of all first 7 bytes + 0x7"; +CM_ BO_ 521 "Message sent every 100ms. All fixed bytes, no checksum."; +CM_ BO_ 537 "Message sent every 100ms. Checksum : use all first 7 bytes with the SAE J1850 CRC algo"; +CM_ BO_ 425 "Message sent every 20ms. Checksum : Sum of all first 4 bytes + 0x38"; +CM_ BO_ 729 "Message sent every 1000ms. All fixed bytes, no checksum."; + +BA_DEF_ "BusType" STRING ; +BA_DEF_ BO_ "GenMsgCycleTime" INT 0 0; +BA_DEF_ SG_ "FieldType" STRING ; + +BA_DEF_DEF_ "BusType" "CAN"; +BA_DEF_DEF_ "FieldType" ""; +BA_DEF_DEF_ "GenMsgCycleTime" 0; + +BA_ "GenMsgCycleTime" BO_ 697 250; +BA_ "GenMsgCycleTime" BO_ 681 1000; +BA_ "GenMsgCycleTime" BO_ 409 10; +BA_ "GenMsgCycleTime" BO_ 361 10; +BA_ "GenMsgCycleTime" BO_ 345 20; +BA_ "GenMsgCycleTime" BO_ 329 20; +BA_ "GenMsgCycleTime" BO_ 297 20; +BA_ "GenMsgCycleTime" BO_ 281 10; +BA_ "GenMsgCycleTime" BO_ 265 10; +BA_ "GenMsgCycleTime" BO_ 521 100; +BA_ "GenMsgCycleTime" BO_ 537 100; +BA_ "GenMsgCycleTime" BO_ 425 20; +BA_ "GenMsgCycleTime" BO_ 729 1000; + +VAL_ 681 Msg2A9_FourWheelDrive 3 "SNA" 2 "UNUSED" 1 "4WD" 0 "2WD" ; +VAL_ 785 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 785 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 788 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 788 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 791 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 791 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 794 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 794 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 797 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 797 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 800 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 800 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 803 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 803 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 806 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 806 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 809 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 809 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 812 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 812 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 815 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 815 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 818 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 818 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 821 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 821 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 824 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 824 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 827 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 827 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 830 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 830 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 833 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 833 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 836 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 836 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 839 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 839 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 842 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 842 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 845 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 845 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 848 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 848 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 851 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 851 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 854 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 854 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 857 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 857 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 860 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 860 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 863 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 863 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 866 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 866 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 869 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 869 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 872 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 872 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 875 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 875 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 878 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 878 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 882 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 882 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 885 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 885 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 888 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 888 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 891 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 891 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 894 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 894 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; \ No newline at end of file diff --git a/opendbc/tesla_radar_continental_generated.dbc b/opendbc/tesla_radar_continental_generated.dbc new file mode 100644 index 0000000000..9a543b3459 --- /dev/null +++ b/opendbc/tesla_radar_continental_generated.dbc @@ -0,0 +1,1262 @@ +CM_ "AUTOGENERATED FILE, DO NOT EDIT"; + +CM_ "tesla_radar_continental.dbc starts here"; + +VERSION "" + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: + +BU_: Autopilot Radar Diag + +BO_ 1025 RadarStatus: 8 Radar + SG_ carparkDetected : 29|1@1+ (1,0) [0|1] "" Autopilot + SG_ decreaseBlockage : 25|1@1+ (1,0) [0|1] "" Autopilot + SG_ horizontMisalignment : 8|12@1+ (0.00012207,-0.25) [-0.25|0.249878] "rad" Autopilot + SG_ increaseBlockage : 24|1@1+ (1,0) [0|1] "" Autopilot + SG_ lowPowerMode : 20|2@1+ (1,0) [0|3] "" Autopilot + SG_ powerOnSelfTest : 22|1@1+ (1,0) [0|1] "" Autopilot + SG_ sensorBlocked : 26|1@1+ (1,0) [0|1] "" Autopilot + SG_ sensorInfoConsistBit : 30|1@1+ (1,0) [0|1] "" Autopilot + SG_ sensorReplace : 31|1@1+ (1,0) [0|1] "" Autopilot + SG_ shortTermUnavailable : 23|1@1+ (1,0) [0|1] "" Autopilot + SG_ tunnelDetected : 28|1@1+ (1,0) [0|1] "" Autopilot + SG_ vehDynamicsError : 27|1@1+ (1,0) [0|1] "" Autopilot + SG_ verticalMisalignment : 0|8@1+ (0.00195313,-0.25) [-0.25|0.248047] "rad" Autopilot + +BO_ 1617 Radar_udsResponse: 8 Radar + SG_ Radar_udsResponseData : 7|64@0+ (1,0) [0|1.84467e+19] "" Diag + +BO_ 1601 UDS_radcRequest: 8 Diag + SG_ UDS_radcRequestData : 7|64@0+ (1,0) [0|1.84467e+19] "" Radar + +BO_ 1040 RadarPoint0_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1041 RadarPoint0_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1042 RadarPoint1_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1043 RadarPoint1_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1044 RadarPoint2_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1045 RadarPoint2_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1046 RadarPoint3_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1047 RadarPoint3_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1048 RadarPoint4_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1049 RadarPoint4_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1050 RadarPoint5_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1051 RadarPoint5_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1052 RadarPoint6_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1053 RadarPoint6_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1054 RadarPoint7_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1055 RadarPoint7_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1056 RadarPoint8_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1057 RadarPoint8_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1058 RadarPoint9_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1059 RadarPoint9_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1060 RadarPoint10_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1061 RadarPoint10_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1062 RadarPoint11_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1063 RadarPoint11_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1064 RadarPoint12_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1065 RadarPoint12_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1066 RadarPoint13_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1067 RadarPoint13_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1068 RadarPoint14_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1069 RadarPoint14_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1070 RadarPoint15_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1071 RadarPoint15_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1072 RadarPoint16_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1073 RadarPoint16_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1074 RadarPoint17_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1075 RadarPoint17_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1076 RadarPoint18_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1077 RadarPoint18_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1078 RadarPoint19_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1079 RadarPoint19_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1080 RadarPoint20_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1081 RadarPoint20_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1082 RadarPoint21_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1083 RadarPoint21_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1084 RadarPoint22_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1085 RadarPoint22_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1086 RadarPoint23_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1087 RadarPoint23_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1088 RadarPoint24_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1089 RadarPoint24_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1090 RadarPoint25_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1091 RadarPoint25_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1092 RadarPoint26_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1093 RadarPoint26_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1094 RadarPoint27_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1095 RadarPoint27_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1096 RadarPoint28_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1097 RadarPoint28_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1098 RadarPoint29_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1099 RadarPoint29_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1100 RadarPoint30_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1101 RadarPoint30_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1102 RadarPoint31_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1103 RadarPoint31_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1104 RadarPoint32_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1105 RadarPoint32_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1106 RadarPoint33_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1107 RadarPoint33_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1108 RadarPoint34_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1109 RadarPoint34_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1110 RadarPoint35_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1111 RadarPoint35_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1112 RadarPoint36_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1113 RadarPoint36_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1114 RadarPoint37_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1115 RadarPoint37_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1116 RadarPoint38_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1117 RadarPoint38_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1118 RadarPoint39_A: 8 Radar + SG_ LongDist : 0|12@1+ (0.0625,0) [0|255.9] "meters" Autopilot + SG_ LongSpeed : 12|12@1+ (0.0625,-128) [-128|128] "meters/sec" Autopilot + SG_ LatDist : 24|11@1+ (0.125,-128) [-128|128] "meters" Autopilot + SG_ ProbExist : 35|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ LongAccel : 40|10@1+ (0.03125,-16) [-16|16] "meters/sec/sec" Autopilot + SG_ ProbObstacle : 50|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Valid : 55|1@1+ (1,0) [0|1] "" Autopilot + SG_ ProbNonObstacle : 56|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Meas : 61|1@1+ (1,0) [0|1] "" Autopilot + SG_ Tracked : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index : 63|1@1+ (1,0) [0|1] "" Autopilot + +BO_ 1119 RadarPoint39_B: 8 Radar + SG_ LatSpeed : 0|10@1+ (0.125,-64) [-64|64] "meters/sec" Autopilot + SG_ Length : 10|6@1+ (0.125,0) [0|7.875] "m" Autopilot + SG_ dZ : 16|6@1+ (0.25,-5) [-5|10.75] "m" Autopilot + SG_ MovingState : 22|2@1+ (1,0) [0|3] "" Autopilot + SG_ dxSigma : 24|6@1+ (1,0) [0|63] "" Autopilot + SG_ vxSigma : 30|6@1+ (1,0) [0|63] "" Autopilot + SG_ axSigma : 36|6@1+ (1,0) [0|63] "" Autopilot + SG_ dySigma : 42|6@1+ (1,0) [0|63] "" Autopilot + SG_ ProbClass : 48|5@1+ (3.125,0) [0|96.875] "%" Autopilot + SG_ Class : 53|3@1+ (1,0) [0|7] "" Autopilot + SG_ dxRearEndLoss : 56|6@1+ (1,0) [0|63] "" Autopilot + SG_ NotUsed : 62|1@1+ (1,0) [0|1] "" Autopilot + SG_ Index2 : 63|1@1+ (1,0) [0|1] "" Autopilot + +VAL_ 1025 lowPowerMode 1 "COMMANDED_LOW_POWER" 0 "DEFAULT_LOW_POWER" 2 "NORMAL_POWER" 3 "SNA"; +VAL_ 1041 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1041 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1043 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1043 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1045 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1045 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1047 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1047 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1049 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1049 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1051 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1051 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1053 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1053 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1055 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1055 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1057 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1057 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1059 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1059 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1061 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1061 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1063 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1063 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1065 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1065 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1067 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1067 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1069 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1069 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1071 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1071 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1073 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1073 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1075 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1075 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1077 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1077 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1079 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1079 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1081 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1081 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1083 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1083 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1085 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1085 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1087 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1087 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1089 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1089 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1091 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1091 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1093 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1093 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1095 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1095 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1097 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1097 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1099 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1099 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1101 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1101 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1103 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1103 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1105 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1105 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1107 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1107 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1109 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1109 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1111 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1111 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1113 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1113 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1115 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1115 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1117 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1117 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; +VAL_ 1119 MovingState 3 "RADAR_MOVESTATE_STANDING" 2 "RADAR_MOVESTATE_STOPPED" 1 "RADAR_MOVESTATE_MOVING" 0 "RADAR_MOVESTATE_INDETERMINATE" ; +VAL_ 1119 Class 4 "RADAR_CLASS_CONSTRUCTION_ELEMENT" 3 "RADAR_CLASS_MOVING_PEDESTRIAN" 2 "RADAR_CLASS_MOVING_TWO_WHEEL_VEHICLE" 1 "RADAR_CLASS_MOVING_FOUR_WHEEL_VEHICLE" 0 "RADAR_CLASS_UNKNOWN" ; \ No newline at end of file diff --git a/opendbc/toyota_new_mc_pt_generated.dbc b/opendbc/toyota_new_mc_pt_generated.dbc index 04910dcfc2..dfb69df492 100644 --- a/opendbc/toyota_new_mc_pt_generated.dbc +++ b/opendbc/toyota_new_mc_pt_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -37,10 +37,13 @@ CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor th BO_ 767 SDSU: 8 XXX SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STATE : 23|4@0+ (1,0) [0|15] "" XXX -CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu"; +CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS-P Toyotas. Learn more: https://github.com/RetroPilot/ocelot/tree/main/firmware/smart_dsu"; CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu"; +VAL_ 767 STATE 7 "STATE_AEB_CTRL" 6 "FAULT_INVALID" 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -202,6 +205,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +454,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "Set to 1 when lead is stopped, likely only used in older TSS-P vehicles"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; @@ -520,6 +525,7 @@ VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear"; VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press"; VAL_ 865 CLUTCH_RELEASED 0 "clutch pressed any amount" 1 "clutch released" VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; +VAL_ 921 DISTANCE_LINES 0 "not displayed" 1 "close" 2 "medium" 3 "far"; VAL_ 956 SPORT_ON 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc index 7538b46a72..efbcccc860 100644 --- a/opendbc/toyota_nodsu_pt_generated.dbc +++ b/opendbc/toyota_nodsu_pt_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -37,10 +37,13 @@ CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor th BO_ 767 SDSU: 8 XXX SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STATE : 23|4@0+ (1,0) [0|15] "" XXX -CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu"; +CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS-P Toyotas. Learn more: https://github.com/RetroPilot/ocelot/tree/main/firmware/smart_dsu"; CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu"; +VAL_ 767 STATE 7 "STATE_AEB_CTRL" 6 "FAULT_INVALID" 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -202,6 +205,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +454,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "Set to 1 when lead is stopped, likely only used in older TSS-P vehicles"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; @@ -520,6 +525,7 @@ VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear"; VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press"; VAL_ 865 CLUTCH_RELEASED 0 "clutch pressed any amount" 1 "clutch released" VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; +VAL_ 921 DISTANCE_LINES 0 "not displayed" 1 "close" 2 "medium" 3 "far"; VAL_ 956 SPORT_ON 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; diff --git a/opendbc/toyota_tnga_k_pt_generated.dbc b/opendbc/toyota_tnga_k_pt_generated.dbc index f54f7df852..d95cfbe69a 100644 --- a/opendbc/toyota_tnga_k_pt_generated.dbc +++ b/opendbc/toyota_tnga_k_pt_generated.dbc @@ -1,7 +1,7 @@ CM_ "AUTOGENERATED FILE, DO NOT EDIT"; -CM_ "Imported file _comma.dbc starts here"; +CM_ "Imported file _community.dbc starts here"; BO_ 359 STEERING_IPAS_COMMA: 8 IPAS SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX SG_ ANGLE : 3|12@0- (1.5,0) [-510|510] "deg" XXX @@ -37,10 +37,13 @@ CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor th BO_ 767 SDSU: 8 XXX SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX + SG_ STATE : 23|4@0+ (1,0) [0|15] "" XXX -CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu"; +CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS-P Toyotas. Learn more: https://github.com/RetroPilot/ocelot/tree/main/firmware/smart_dsu"; CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu"; +VAL_ 767 STATE 7 "STATE_AEB_CTRL" 6 "FAULT_INVALID" 5 "FAULT_TIMEOUT" 4 "FAULT_STARTUP" 3 "FAULT_SCE" 2 "FAULT_SEND" 1 "FAULT_BAD_CHECKSUM" 0 "NO_FAULT" ; + CM_ "Imported file _toyota_2017.dbc starts here"; VERSION "" @@ -202,6 +205,7 @@ BO_ 835 ACC_CONTROL: 8 DSU SG_ ACC_TYPE : 23|2@0+ (1,0) [0|3] "" HCU SG_ CANCEL_REQ : 24|1@0+ (1,0) [0|1] "" HCU SG_ ACC_CUT_IN : 25|1@0+ (1,0) [0|1] "" XXX + SG_ LEAD_VEHICLE_STOPPED : 29|1@0+ (1,0) [0|0] "" DSU SG_ PERMIT_BRAKING : 30|1@0+ (1,0) [0|1] "" HCU SG_ RELEASE_STANDSTILL : 31|1@0+ (1,0) [0|1] "" HCU SG_ ITS_CONNECT_LEAD : 39|8@0+ (1,0) [0|1] "" Vector__XXX @@ -450,6 +454,7 @@ CM_ SG_ 835 ACC_MALFUNCTION "display ACC fault on dash if set to 1"; CM_ SG_ 835 ACC_CUT_IN "Display blinking yellow lead if set to 1"; CM_ SG_ 835 DISTANCE "Cycle through ACC following distance from long, mid, short when set to 1"; CM_ SG_ 835 ITS_CONNECT_LEAD "Displayed when lead car is capable of ITS Connect"; +CM_ SG_ 835 LEAD_VEHICLE_STOPPED "Set to 1 when lead is stopped, likely only used in older TSS-P vehicles"; CM_ SG_ 835 ALLOW_LONG_PRESS "Enable Toyota's factory set speed increment behaviour, available on both metrics cars and imperial unit cars"; CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front is detected. In openpilot and before the PERMIT_BRAKING name, this was 'SET_ME_1' and is hardcoded to be high. Unsure if only informational or has an effect though existing usage in openpilot is to always set it to 1. Originally 'PMTBRKG' in the leaked toyota_2017_ref_pt.dbc file and name expansion speculated to be PerMiT BRaKinG."; CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file."; @@ -520,6 +525,7 @@ VAL_ 835 ACC_CUT_IN 1 "CUT-IN Detected" 0 "clear"; VAL_ 835 ALLOW_LONG_PRESS 2 "set speed increase by 5 speed units regardless" 1 "set speed increase by 1 speed unit on short press, 5 speed units on long press"; VAL_ 865 CLUTCH_RELEASED 0 "clutch pressed any amount" 1 "clutch released" VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted"; +VAL_ 921 DISTANCE_LINES 0 "not displayed" 1 "close" 2 "medium" 3 "far"; VAL_ 956 SPORT_ON 0 "off" 1 "on"; VAL_ 956 GEAR 0 "D" 1 "S" 8 "N" 16 "R" 32 "P"; VAL_ 956 SPORT_GEAR_ON 0 "off" 1 "on"; diff --git a/opendbc/vw_mqb_2010.dbc b/opendbc/vw_mqb_2010.dbc index 8806e0011a..44ec0cd3e0 100644 --- a/opendbc/vw_mqb_2010.dbc +++ b/opendbc/vw_mqb_2010.dbc @@ -91,6 +91,7 @@ BO_ 679 ACC_13: 8 XXX SG_ ACC_Tachokranz : 58|1@1+ (1,0) [0|1] "" XXX SG_ ACC_Typ_Tachokranz_unten : 59|1@1+ (1,0) [0|1] "" XXX SG_ ACC_ENG_Texte : 60|2@1+ (1,0) [0|3] "" XXX + SG_ ACC_ADAPTIVE : 63|2@0+ (1,0) [0|3] "" XXX BO_ 681 ACC_15: 8 XXX SG_ AWV_Warnung : 16|3@1+ (1,0) [0|7] "" XXX @@ -1144,19 +1145,43 @@ BO_ 288 TSK_06: 8 Motor_Diesel_MQB SG_ TSK_Status : 24|3@1+ (1,0) [0|7] "" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB SG_ TSK_v_Begrenzung_aktiv : 27|1@1+ (1,0) [0|1] "" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB SG_ TSK_Standby_Anf_ESP : 28|1@1+ (1,0) [0|1] "" Gateway_MQB + SG_ TSK_Freig_WU : 29|1@1+ (1.0,0.0) [0.0|1] "" Gateway_MQB SG_ TSK_Freig_Verzoeg_Anf : 30|1@1+ (1,0) [0|1] "" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB SG_ TSK_Limiter_ausgewaehlt : 31|1@1+ (1,0) [0|1] "" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB + SG_ TSK_Wunsch_Uebersetz : 32|10@1+ (0.0245,0) [0.0245|25.0635] "" Gateway_MQB + SG_ TSK_Hauptschalter_GRA_ACC : 42|2@1+ (1.0,0.0) [0.0|3] "" Gateway_MQB + SG_ TSK_SRBM_Anf_ASIL : 44|3@1+ (1.0,0.0) [0.0|7] "" Gateway_MQB SG_ TSK_ax_Getriebe_02 : 48|9@1+ (0.024,-2.016) [-2.016|10.224] "Unit_MeterPerSeconSquar" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB SG_ TSK_Zwangszusch_ESP : 57|1@1+ (1,0) [0|1] "" Gateway_MQB SG_ TSK_zul_Regelabw : 58|6@1+ (0.024,0) [0|1.512] "Unit_MeterPerSeconSquar" Gateway_MQB,Getriebe_DQ_Hybrid_MQB,Getriebe_DQ_MQB BO_ 798 TSK_07: 8 Motor_Diesel_MQB - SG_ TSK_07_CRC : 0|8@1+ (1,0) [0|255] "" Gateway_MQB - SG_ TSK_07_BZ : 8|4@1+ (1,0) [0|15] "" Gateway_MQB - SG_ TSK_Wunschgeschw : 12|10@1+ (0.32,0) [0|326.72] "Unit_KiloMeterPerHour" Gateway_MQB - SG_ TSK_Texte_Primaeranz : 48|5@1+ (1,0) [0|31] "" Gateway_MQB - SG_ TSK_Limiter_Anzeige : 55|1@1+ (1,0) [0|1] "" Gateway_MQB - SG_ TSK_Status_Anzeige : 61|3@1+ (1,0) [0|7] "" Gateway_MQB + SG_ TSK_07_CRC : 0|8@1+ (1,0) [0|255] "" Gateway_MQB,Getriebe_AQ + SG_ TSK_07_BZ : 8|4@1+ (1,0) [0|15] "" Gateway_MQB,Getriebe_AQ + SG_ TSK_Wunschgeschw : 12|10@1+ (0.32,0) [0.00|326.72] "Unit_KiloMeterPerHour" Gateway_MQB,Getriebe_AQ + SG_ TSK_Texte : 40|5@1+ (1.0,0.0) [0.0|31] "" Gateway_MQB + SG_ TSK_Akustik : 45|3@1+ (1.0,0.0) [0.0|7] "" Gateway_MQB + SG_ TSK_Texte_Primaeranz : 48|5@1+ (1.0,0.0) [0.0|31] "" Gateway_MQB + SG_ TSK_Limiter_Fahrerinfo : 53|2@1+ (1.0,0.0) [0.0|3] "" Gateway_MQB + SG_ TSK_Limiter_Anzeige : 55|1@1+ (1.0,0.0) [0.0|1] "" Gateway_MQB + SG_ TSK_Fahrzeugstatus_GRA : 56|1@1+ (1.0,0.0) [0.0|1] "" Gateway_MQB + SG_ TSK_Fahrzeugstatus_Limiter : 57|1@1+ (1.0,0.0) [0.0|1] "" Gateway_MQB + SG_ MO_Motorlaufwarnung : 58|1@1+ (1.0,0.0) [0.0|1] "" Gateway_MQB + SG_ TSK_Status_Anzeige : 61|3@1+ (1.0,0.0) [0.0|7] "" Gateway_MQB + +BO_ 346 TSK_08: 8 Motor_Diesel_MQB + SG_ TSK_08_CRC : 0|8@1+ (1,0) [0|255] "" Frontradar + SG_ TSK_08_BZ : 8|4@1+ (1,0) [0|15] "" Frontradar + SG_ MO_Anforderung_HMS : 12|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ TSK_Status_EA : 32|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ TSK_vMax_Fahrerassistenz : 40|9@1+ (1,0) [0|510] "" Frontradar + SG_ TSK_Einheit_vMax_Fahrerassistenz : 49|1@1+ (1,0) [0|1] "" Frontradar + SG_ TSK_Status_PLA : 50|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ TSK_aktives_System : 53|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ TSK_erhoehter_Fahrwiderstand : 56|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ TSK_Anf_Antriebsmoment : 57|1@1+ (1,0) [0|1] "" Vector__XXX + SG_ TSK_Status_ARA : 58|3@1+ (1,0) [0|7] "" Vector__XXX + SG_ TSK_Status_IPA : 61|3@1+ (1,0) [0|7] "" Vector__XXX BO_ 1716 VIN_01: 8 Gateway_MQB SG_ VIN_01_MUX M : 0|2@1+ (1,0) [0|3] "" Airbag_MQB @@ -1459,6 +1484,7 @@ CM_ SG_ 780 SetAbstand "Set following distance"; CM_ SG_ 780 Abstand "Following distance"; CM_ SG_ 780 SetSpeed "ACC set speed"; CM_ SG_ 391 GearPosition "Traditional PRND plus B-mode aggressive regen, B-mode mapped to Drive"; +CM_ SG_ 679 ACC_ADAPTIVE "TSK_06.TSK_Limiter_ausgewaehlt seems to take precedence"; CM_ SG_ 960 ZAS_Kl_15 "Indicates ignition on"; CM_ SG_ 1720 KBI_BCmE_aktiv "Anzeige BCmE aktiv (BCmE-Screen oder Einsparhinweis in der Anzeige)"; CM_ SG_ 1720 KBI_Max_Tankinhalt "Mitteilung des maximalen Tankinhalts an das Reichweitenmodul"; @@ -1477,9 +1503,18 @@ CM_ SG_ 1720 KBI_Variante "Zeigt an ob es sich um ein konventionelles Zeiger-Kom CM_ SG_ 1720 KBI_Variante_USA "In diesem Signal wird die HW-Variante des Kombis ausgegeben, ACC plausibilisiert auf dieses Signal hin seine US-Codierung"; -VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" ; +VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" 8 "preempted" ; VAL_ 173 GE_Fahrstufe 5 "P" 6 "R" 7 "N" 8 "D" 9 "S" 10 "E" 13 "T" 14 "T" ; VAL_ 288 TSK_Status 0 "init" 1 "disabled" 2 "enabled" 3 "regulating" 4 "accel_pedal_override" 5 "brake_only" 6 "temp_fault" 7 "perm_fault" ; +VAL_ 288 TSK_v_Begrenzung_aktiv 0 "inaktiv" 1 "aktiv" ; +VAL_ 288 TSK_Standby_Anf_ESP 0 "keine_Standby_Anforderung" 1 "Standby_Anforderung" ; +VAL_ 288 TSK_Freig_WU 0 "TSK_Uebersetzungswunsch_nicht_freigegeben" 1 "TSK_Uebersetzungswunsch_freigegeben" ; +VAL_ 288 TSK_Freig_Verzoeg_Anf 0 "Verzoegerungsanforderung_nicht_freigegeben" 1 "Verzoegerungsanforderung_freigegeben" ; +VAL_ 288 TSK_Limiter_ausgewaehlt 0 "kein_Limiter_ausgewaehlt" 1 "Limiter_ausgewaehlt" ; +VAL_ 288 TSK_Wunsch_Uebersetz 0 "Init" ; +VAL_ 288 TSK_Hauptschalter_GRA_ACC 0 "Init" 1 "Aus" 2 "Ein" 3 "Fehler" ; +VAL_ 288 TSK_ax_Getriebe_02 511 "Neutralwert" ; +VAL_ 288 TSK_Zwangszusch_ESP 0 "keine_ESP_ASR_Beeinflussung" 1 "ESP_ASR_Beeinflussung" ; VAL_ 294 EA_ACC_Sollstatus 0 "Init" 1 "ACC_aktivieren" 2 "ACC_deaktivieren" ; VAL_ 294 EA_Ruckprofil 0 "Init" 1 "Profil_1" 2 "Profil_2" 3 "Profil_3" 4 "Profil_4" 5 "Profil_5" 6 "Profil_6" 7 "Profil_7" ; VAL_ 294 HCA_01_Sendestatus 0 "HCA_sendet_mit_1000ms" 1 "HCA_sendet_mit_20ms" ; @@ -1487,6 +1522,26 @@ VAL_ 294 HCA_01_LM_OffSign 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen" ; VAL_ 294 HCA_01_Status_HCA 0 "deaktiviert" 1 "reserviert" 2 "reserviert" 3 "funktionsbereit" 4 "reserviert" 5 "HCA_Momenteneingriff_1" 6 "MA_Aktiv" 7 "HCA_Momenteneingriff_2" 8 "reserviert" 9 "reserviert" 10 "reserviert" 11 "reserviert" 12 "reserviert" 13 "reserviert" 14 "reserviert" 15 "reserviert" ; VAL_ 294 EA_Ruckfreigabe 0 "keine_Freigabe" 1 "Freigabe" ; VAL_ 294 EA_ACC_Wunschgeschwindigkeit 1023 "Init" ; +VAL_ 346 MO_Anforderung_HMS 0 "keine_Anforderung" 1 "halten" 2 "parken" 3 "halten_Standby" 4 "anfahren" 5 "Loesen_ueber_Rampe" ; +VAL_ 346 TSK_Status_EA 0 "Aus" 1 "Init_oder_nicht_verbaut" 3 "Aktiv" 4 "Uebertreten" 5 "Abschaltung_laeuft" 6 "Reversibel_aus" 7 "Irreversibel_Aus" ; +VAL_ 346 TSK_vMax_Fahrerassistenz 511 "Init_ungueltig_keine_Beschraenkung" ; +VAL_ 346 TSK_Einheit_vMax_Fahrerassistenz 0 "kmh" 1 "mph" ; +VAL_ 346 TSK_Status_PLA 0 "Aus_Funktionsbereit" 1 "Init_oder_nicht_verbaut" 2 "aktivierbar" 3 "aktiv" 5 "Abschaltung_laeuft" 6 "reversibel_aus" 7 "Fehler" ; +VAL_ 346 TSK_aktives_System 0 "keine_Funktion_aktiv" 1 "GRA_ACC" 2 "ARA" 3 "Speedlimiter" 4 "IPA" 5 "PLA" 6 "PEA_Ausrollassistent" 7 "EA" ; +VAL_ 346 TSK_erhoehter_Fahrwiderstand 0 "kein_erhoehter_Fahrwiderstand" 1 "erhoehter_Fahrwiderstand" ; +VAL_ 346 TSK_Anf_Antriebsmoment 0 "keine_Anforderung" 1 "Anforderung_aktiv" ; +VAL_ 346 TSK_Status_ARA 0 "Aus" 1 "Init_oder_nicht_verbaut" 2 "aktivierbar" 3 "aktiv" 5 "abschaltung_laeuft" 6 "reversibel_aus" 7 "Fehler" ; +VAL_ 346 TSK_Status_IPA 0 "Aus_Funktionsbereit" 1 "Init_oder_nicht_verbaut" 2 "aktivierbar" 3 "aktiv" 5 "Abschaltung_laueft" 6 "reversibel_aus" 7 "Fehler" ; +VAL_ 798 TSK_Wunschgeschw 1022 "keine_Anzeige" 1023 "kein_Wert_im_Speicher" ; +VAL_ 798 TSK_Texte 0 "kein_Text" 1 "GRA_Modus_ausgewaehlt" 2 "ACC_Modus_ausgewaehlt" 3 "Lim_Modus_ausgewaehlt" 4 "Lim_nicht_verfuegbar_ESC_passiv" 5 "GRA_nicht_verfuegbar_ESC_passiv" 6 "Lim_nicht_verfuegbar_Charisma" 7 "GRA_nicht_verfuegbar_Charisma" 8 "Lim_nicht_verfuegbar_HDC" 9 "GRA_nicht_verfuegbar_HDC" ; +VAL_ 798 TSK_Akustik 0 "keine_Akustik" 1 "einzelner_Warnton" 2 "dauerhafter_Warnton" ; +VAL_ 798 TSK_Texte_Primaeranz 0 "keine_Anzeige" 1 "GRA_Symbol_passiv_xxx_kmh_mph" 2 "GRA_Symbol_aktiv_xxx_kmh_mph" 3 "Bremse_ueberhitzt" 4 "Limiter_Modus_aktiviert" 5 "GRA_Modus_aktiviert" 6 "ACC_Modus_aktiviert" 7 "Opt_Geschwindigkeitswarnung" 8 "Opt_und_akustische_GeschwWarnung" 9 "Opt_GeschwWarnung_dauerhaft_mit_einmal_Akustik" 10 "Limiter_passiv_mit_Akustik" 11 "Limiter_Fehler_mit_Akustik" 12 "Limiter_Symbol_passiv_xxx_kmh_mph" 13 "Limiter_Symbol_aktiv_xxx_kmh_mph" 14 "Popup_Geschw_zu_hoch__Resume_unzulaessig" ; +VAL_ 798 TSK_Limiter_Fahrerinfo 0 "keine_Info" 1 "Limit_erreicht" 2 "Ueberschritten" 3 "Vom_Fahrer_Ueberstimmt" ; +VAL_ 798 TSK_Limiter_Anzeige 0 "Display_Anzeige_GRA_ACC" 1 "Display_Anzeige_Limiter" ; +VAL_ 798 TSK_Fahrzeugstatus_GRA 0 "GRA_verfuegbar" 1 "GRA_nicht_verfuegbar" ; +VAL_ 798 TSK_Fahrzeugstatus_Limiter 0 "Limiter_verfuegbar" 1 "Limiter_nicht_verfuegbar" ; +VAL_ 798 MO_Motorlaufwarnung 0 "keine_Anzeige" 1 "Anforderung_Motorlaufwarnung" ; +VAL_ 798 TSK_Status_Anzeige 0 "Hauptschalter_aus" 1 "Init" 2 "passiv" 3 "aktiv" 4 "Uebertreten" 5 "Limitiierung_aktiv" 6 "reversibel_aus" 7 "irreversibel_aus" ; VAL_ 780 ACC_Wunschgeschw_02 1023 "keine_Anzeige" ; VAL_ 780 ACC_Status_Prim_Anz 0 "Symbol nicht beleuchtet" 1 "Farbe 1 (typisch 'gruen')" 2 "Farbe 2 (typisch 'rot')" 3 "Farbe 3 (typisch 'gelb')" ; VAL_ 780 ACC_Abstandsindex 0 "Sonderanzeige_graue_Fahrbahn" 1022 "Sonderanzeige_graue_Fahrbahn" 1023 "Sonderanzeige_Fahrbahn_mit_gruenem_roten_Bereich" ; @@ -1559,6 +1614,7 @@ VAL_ 679 ACC_RUV 0 "keine_Anzeige" 1 "RUV_aktiv_Rechtsverkehr" 2 "RUV_aktiv_Link VAL_ 679 ACC_Tachokranz 0 "Tachokranz_nicht_beleuchtet" 1 "Tachokranz_beleuchtet" ; VAL_ 679 ACC_Typ_Tachokranz_unten 0 "LEDs_an" 1 "LEDs_aus" ; VAL_ 679 ACC_ENG_Texte 0 "keine_Anzeige" 1 "keine_Laenderverfuegbarkeit" 2 "nicht_verfuegbar" 3 "Geschwindigkeitsgrenze" ; +VAL_ 679 ACC_ADAPTIVE 1 "adaptive" 2 "non-adaptive" ; VAL_ 681 AWV_Warnung 0 "keine_Anzeige" 1 "latente_Vorwarnung" 2 "Vorwarnung" 3 "Akutwarnung" 4 "Eingriff" 5 "Fahreruebernahmeaufforderung" 6 "Abbiegewarnung" ; VAL_ 681 AWV_Texte 0 "keine_Anzeige" 1 "Systemstoerung" 2 "keine_Sensorsicht" 3 "Demomodus" 4 "System_aus" 5 "nicht_definiert" 6 "ESC_aus" 7 "zurzeit_eingeschraenkt" ; VAL_ 681 AWV_Status_Anzeige 0 "Init" 1 "verfuegbar" 2 "nicht_verfuegbar" ; diff --git a/openpilot/__init__.py b/openpilot/__init__.py index b28b04f643..e69de29bb2 100644 --- a/openpilot/__init__.py +++ b/openpilot/__init__.py @@ -1,3 +0,0 @@ - - - diff --git a/panda/Jenkinsfile b/panda/Jenkinsfile new file mode 100644 index 0000000000..2d4c615dfe --- /dev/null +++ b/panda/Jenkinsfile @@ -0,0 +1,138 @@ +def docker_run(String step_label, int timeout_mins, String cmd) { + timeout(time: timeout_mins, unit: 'MINUTES') { + sh script: "docker run --rm --privileged \ + --env PYTHONWARNINGS=error \ + --volume /dev/bus/usb:/dev/bus/usb \ + --volume /var/run/dbus:/var/run/dbus \ + --workdir /tmp/openpilot/panda \ + --net host \ + ${env.DOCKER_IMAGE_TAG} \ + bash -c 'scons -j8 && ${cmd}'", \ + label: step_label + } +} + + +def phone(String ip, String step_label, String cmd) { + withCredentials([file(credentialsId: 'id_rsa', variable: 'key_file')]) { + def ssh_cmd = """ +ssh -tt -o StrictHostKeyChecking=no -i ${key_file} 'comma@${ip}' /usr/bin/bash <<'END' + +set -e + + +source ~/.bash_profile +if [ -f /etc/profile ]; then + source /etc/profile +fi + +export CI=1 +export TEST_DIR=${env.TEST_DIR} +export SOURCE_DIR=${env.SOURCE_DIR} +export GIT_BRANCH=${env.GIT_BRANCH} +export GIT_COMMIT=${env.GIT_COMMIT} +export PYTHONPATH=${env.TEST_DIR}/../ +export PYTHONWARNINGS=error + +cd ${env.TEST_DIR} || true +${cmd} +exit 0 + +END""" + + sh script: ssh_cmd, label: step_label + } +} + +def phone_steps(String device_type, steps) { + lock(resource: "", label: device_type, inversePrecedence: true, variable: 'device_ip', quantity: 1) { + timeout(time: 20, unit: 'MINUTES') { + phone(device_ip, "git checkout", readFile("tests/setup_device_ci.sh"),) + steps.each { item -> + phone(device_ip, item[0], item[1]) + } + } + } +} + + + +pipeline { + agent any + environment { + CI = "1" + PYTHONWARNINGS= "error" + DOCKER_IMAGE_TAG = "panda:build-${env.GIT_COMMIT}" + + TEST_DIR = "/data/panda" + SOURCE_DIR = "/data/panda_source/" + } + options { + timeout(time: 3, unit: 'HOURS') + disableConcurrentBuilds(abortPrevious: env.BRANCH_NAME != 'master') + } + + stages { + stage('panda tests') { + parallel { + stage('test dos') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("panda-dos", [ + ["build", "scons -j4"], + ["flash", "cd tests/ && ./reflash_internal_panda.py"], + ["flash jungle", "cd board/jungle && ./flash.py"], + ["test", "cd tests/hitl && HW_TYPES=6 pytest -n0 --durations=0 [2-9]*.py -k 'not test_send_recv'"], + ]) + } + } + + stage('test tres') { + agent { docker { image 'ghcr.io/commaai/alpine-ssh'; args '--user=root' } } + steps { + phone_steps("panda-tres", [ + ["build", "scons -j4"], + ["flash", "cd tests/ && ./reflash_internal_panda.py"], + ["flash jungle", "cd board/jungle && ./flash.py"], + ["test", "cd tests/hitl && HW_TYPES=9 pytest -n0 --durations=0 2*.py [5-9]*.py"], + ]) + } + } + + stage ('Acquire resource locks') { + options { + lock(resource: "pandas") + } + stages { + stage('Build Docker Image') { + steps { + timeout(time: 20, unit: 'MINUTES') { + script { + sh 'git archive -v -o panda.tar.gz --format=tar.gz HEAD' + dockerImage = docker.build("${env.DOCKER_IMAGE_TAG}") + } + } + } + } + stage('jungle tests') { + steps { + script { + retry (3) { + docker_run("reset hardware", 3, "python ./tests/hitl/reset_jungles.py") + } + } + } + } + stage('bootkick tests') { + steps { + script { + docker_run("test", 10, "pytest -n0 ./tests/som/test_bootkick.py") + } + } + } + } + } + } + } + } +} diff --git a/panda/README.md b/panda/README.md new file mode 100644 index 0000000000..b2830b21ef --- /dev/null +++ b/panda/README.md @@ -0,0 +1,105 @@ +# Welcome to panda + +![panda tests](https://github.com/commaai/panda/workflows/tests/badge.svg) +![panda drivers](https://github.com/commaai/panda/workflows/drivers/badge.svg) + +panda speaks CAN and CAN FD, and it runs on [STM32F413](https://www.st.com/resource/en/reference_manual/rm0430-stm32f413423-advanced-armbased-32bit-mcus-stmicroelectronics.pdf) and [STM32H725](https://www.st.com/resource/en/reference_manual/rm0468-stm32h723733-stm32h725735-and-stm32h730-value-line-advanced-armbased-32bit-mcus-stmicroelectronics.pdf). + +## Directory structure + +``` +. +├── board # Code that runs on the STM32 +├── drivers # Drivers (not needed for use with Python) +├── python # Python userspace library for interfacing with the panda +├── tests # Tests and helper programs for panda +``` + +## Safety Model + +When a panda powers up, by default it's in `SAFETY_SILENT` mode. While in `SAFETY_SILENT` mode, the CAN buses are forced to be silent. In order to send messages, you have to select a safety mode. Some of safety modes (for example `SAFETY_ALLOUTPUT`) are disabled in release firmwares. In order to use them, compile and flash your own build. + +Safety modes optionally support `controls_allowed`, which allows or blocks a subset of messages based on a customizable state in the board. + +## Code Rigor + +The panda firmware is written for its use in conjuction with [openpilot](https://github.com/commaai/openpilot). The panda firmware, through its safety model, provides and enforces the +[openpilot safety](https://github.com/commaai/openpilot/blob/master/docs/SAFETY.md). Due to its critical function, it's important that the application code rigor within the `board` folder is held to high standards. + +These are the [CI regression tests](https://github.com/commaai/panda/actions) we have in place: +* A generic static code analysis is performed by [cppcheck](https://github.com/danmar/cppcheck/). +* In addition, [cppcheck](https://github.com/danmar/cppcheck/) has a specific addon to check for [MISRA C:2012](https://misra.org.uk/) violations. See [current coverage](https://github.com/commaai/panda/blob/master/tests/misra/coverage_table). +* Compiler options are relatively strict: the flags `-Wall -Wextra -Wstrict-prototypes -Werror` are enforced. +* The [safety logic](https://github.com/commaai/panda/tree/master/board/safety) is tested and verified by [unit tests](https://github.com/commaai/panda/tree/master/tests/safety) for each supported car variant. +to ensure that the behavior remains unchanged. +* A hardware-in-the-loop test verifies panda's functionalities on all active panda variants, including: + * additional safety model checks + * compiling and flashing the bootstub and app code + * receiving, sending, and forwarding CAN messages on all buses + * CAN loopback and latency tests through USB and SPI + +The above tests are themselves tested by: +* a [mutation test](tests/misra/test_mutation.py) on the MISRA coverage +* 100% line coverage enforced on the safety unit tests + +In addition, we run the [ruff linter](https://github.com/astral-sh/ruff) and [mypy](https://mypy-lang.org/) on panda's Python library. + +## Usage + +Setup dependencies: +```bash +# Ubuntu +sudo apt-get install dfu-util gcc-arm-none-eabi python3-pip libffi-dev git + +# macOS +brew install --cask gcc-arm-embedded +brew install python3 dfu-util gcc@13 +``` + +Clone panda repository and install: +``` bash +git clone https://github.com/commaai/panda.git +cd panda + +# install dependencies +pip install -r requirements.txt + +# install panda +python setup.py install +``` + +See [the Panda class](https://github.com/commaai/panda/blob/master/python/__init__.py) for how to interact with the panda. + +For example, to receive CAN messages: +``` python +>>> from panda import Panda +>>> panda = Panda() +>>> panda.can_recv() +``` +And to send one on bus 0: +``` python +>>> panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) +>>> panda.can_send(0x1aa, b'message', 0) +``` +Note that you may have to setup [udev rules](https://github.com/commaai/panda/tree/master/drivers/linux) for Linux, such as +``` bash +sudo tee /etc/udev/rules.d/11-panda.rules <CR3), PWR_CR3_USBREGEN); register_set_bits(&(PWR->CR3), PWR_CR3_USB33DEN); - while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0); + while ((PWR->CR3 & PWR_CR3_USB33RDY) == 0U); red_chiplet_init(); @@ -70,12 +70,11 @@ void tres_init(void) { clock_source_init(); } -const board board_tres = { +board board_tres = { .harness_config = &red_chiplet_harness_config, .has_obd = true, .has_spi = true, .has_canfd = true, - .has_rtc_battery = true, .fan_max_rpm = 6600U, .avdd_mV = 1800U, .fan_stall_recovery = false, diff --git a/panda/board/boards/uno.h b/panda/board/boards/uno.h index f4e2016bd1..a2e1e98354 100644 --- a/panda/board/boards/uno.h +++ b/panda/board/boards/uno.h @@ -147,8 +147,6 @@ void uno_init(void) { // Initialize harness harness_init(); - // Initialize RTC - rtc_init(); // Enable CAN transceivers uno_enable_can_transceivers(true); @@ -161,11 +159,6 @@ void uno_init(void) { // Set normal CAN mode uno_set_can_mode(CAN_MODE_NORMAL); - // change CAN mapping when flipped - if (harness.status == HARNESS_STATUS_FLIPPED) { - can_flip_buses(0, 2); - } - // Switch to phone usb mode if harness connection is powered by less than 7V if(white_read_voltage_mV() < 7000U){ uno_set_usb_switch(true); @@ -184,7 +177,7 @@ void uno_init_bootloader(void) { set_gpio_output(GPIOC, 12, 0); } -const harness_configuration uno_harness_config = { +harness_configuration uno_harness_config = { .has_harness = true, .GPIO_SBU1 = GPIOC, .GPIO_SBU2 = GPIOC, @@ -198,12 +191,11 @@ const harness_configuration uno_harness_config = { .adc_channel_SBU2 = 13 }; -const board board_uno = { +board board_uno = { .harness_config = &uno_harness_config, .has_obd = true, .has_spi = false, .has_canfd = false, - .has_rtc_battery = true, .fan_max_rpm = 5100U, .avdd_mV = 3300U, .fan_stall_recovery = false, diff --git a/panda/board/boards/white.h b/panda/board/boards/white.h index 0de29a39be..8d22afbc9b 100644 --- a/panda/board/boards/white.h +++ b/panda/board/boards/white.h @@ -66,61 +66,22 @@ void white_set_usb_power_mode(uint8_t mode){ } void white_set_can_mode(uint8_t mode){ - switch (mode) { - case CAN_MODE_NORMAL: - // B12,B13: disable GMLAN mode - set_gpio_mode(GPIOB, 12, MODE_INPUT); - set_gpio_mode(GPIOB, 13, MODE_INPUT); - - // B3,B4: disable GMLAN mode - set_gpio_mode(GPIOB, 3, MODE_INPUT); - set_gpio_mode(GPIOB, 4, MODE_INPUT); - - // B5,B6: normal CAN2 mode - set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); - - // A8,A15: normal CAN3 mode - set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); - set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); - break; - case CAN_MODE_GMLAN_CAN2: - // B5,B6: disable CAN2 mode - set_gpio_mode(GPIOB, 5, MODE_INPUT); - set_gpio_mode(GPIOB, 6, MODE_INPUT); - - // B3,B4: disable GMLAN mode - set_gpio_mode(GPIOB, 3, MODE_INPUT); - set_gpio_mode(GPIOB, 4, MODE_INPUT); - - // B12,B13: GMLAN mode - set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2); - set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2); - - // A8,A15: normal CAN3 mode - set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); - set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); - break; - case CAN_MODE_GMLAN_CAN3: - // A8,A15: disable CAN3 mode - set_gpio_mode(GPIOA, 8, MODE_INPUT); - set_gpio_mode(GPIOA, 15, MODE_INPUT); - - // B12,B13: disable GMLAN mode - set_gpio_mode(GPIOB, 12, MODE_INPUT); - set_gpio_mode(GPIOB, 13, MODE_INPUT); - - // B3,B4: GMLAN mode - set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3); - set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3); - - // B5,B6: normal CAN2 mode - set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2); - set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); - break; - default: - print("Tried to set unsupported CAN mode: "); puth(mode); print("\n"); - break; + if (mode == CAN_MODE_NORMAL) { + // B12,B13: disable GMLAN mode + set_gpio_mode(GPIOB, 12, MODE_INPUT); + set_gpio_mode(GPIOB, 13, MODE_INPUT); + + // B3,B4: disable GMLAN mode + set_gpio_mode(GPIOB, 3, MODE_INPUT); + set_gpio_mode(GPIOB, 4, MODE_INPUT); + + // B5,B6: normal CAN2 mode + set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2); + set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); + + // A8,A15: normal CAN3 mode + set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); + set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); } } @@ -168,8 +129,8 @@ void white_grey_init(void) { 0 1 high voltage wakeup 1 1 33kbit (normal) */ - set_gpio_output(GPIOB, 14, 1); - set_gpio_output(GPIOB, 15, 1); + set_gpio_output(GPIOB, 14, 0); + set_gpio_output(GPIOB, 15, 0); // B7: K-line enable set_gpio_output(GPIOB, 7, 1); @@ -187,8 +148,6 @@ void white_grey_init(void) { set_gpio_alternate(GPIOC, 11, GPIO_AF7_USART3); set_gpio_pullup(GPIOC, 11, PULL_UP); - // Initialize RTC - rtc_init(); // Enable CAN transceivers white_enable_can_transceivers(true); @@ -221,17 +180,16 @@ void white_grey_init_bootloader(void) { set_gpio_output(GPIOC, 14, 0); } -const harness_configuration white_harness_config = { +harness_configuration white_harness_config = { .has_harness = false }; -const board board_white = { +board board_white = { .set_bootkick = unused_set_bootkick, .harness_config = &white_harness_config, .has_obd = false, .has_spi = false, .has_canfd = false, - .has_rtc_battery = false, .fan_max_rpm = 0U, .avdd_mV = 3300U, .fan_stall_recovery = false, diff --git a/panda/board/bootstub_declarations.h b/panda/board/bootstub_declarations.h index ae115d2eb3..5cdec508e7 100644 --- a/panda/board/bootstub_declarations.h +++ b/panda/board/bootstub_declarations.h @@ -6,8 +6,6 @@ void puth4(uint8_t i){ UNUSED(i); } void hexdump(const void *a, int l){ UNUSED(a); UNUSED(l); } typedef struct board board; typedef struct harness_configuration harness_configuration; -// No CAN support on bootloader -void can_flip_buses(uint8_t bus1, uint8_t bus2){UNUSED(bus1); UNUSED(bus2);} void pwm_init(TIM_TypeDef *TIM, uint8_t channel); void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); // No UART support in bootloader @@ -17,4 +15,4 @@ void uart_init(uart_ring *q, int baud) { UNUSED(q); UNUSED(baud); } // ********************* Globals ********************** uint8_t hw_type = 0; -const board *current_board; +board *current_board; diff --git a/panda/board/can_comms.h b/panda/board/can_comms.h index 56ca912f8e..c8f071abd5 100644 --- a/panda/board/can_comms.h +++ b/panda/board/can_comms.h @@ -63,7 +63,7 @@ void comms_can_write(const uint8_t *data, uint32_t len) { if (can_write_buffer.ptr != 0U) { if (can_write_buffer.tail_size <= (len - pos)) { // we have enough data to complete the buffer - CANPacket_t to_push; + CANPacket_t to_push = {0}; (void)memcpy(&can_write_buffer.data[can_write_buffer.ptr], &data[pos], can_write_buffer.tail_size); can_write_buffer.ptr += can_write_buffer.tail_size; pos += can_write_buffer.tail_size; @@ -89,7 +89,7 @@ void comms_can_write(const uint8_t *data, uint32_t len) { while (pos < len) { uint32_t pckt_len = CANPACKET_HEAD_SIZE + dlc_to_len[(data[pos] >> 4U)]; if ((pos + pckt_len) <= len) { - CANPacket_t to_push; + CANPacket_t to_push = {0}; (void)memcpy(&to_push, &data[pos], pckt_len); can_send(&to_push, to_push.bus, false); pos += pckt_len; diff --git a/panda/board/can_definitions.h b/panda/board/can_definitions.h index b3631d8071..f3ce437827 100644 --- a/panda/board/can_definitions.h +++ b/panda/board/can_definitions.h @@ -1,7 +1,7 @@ #pragma once const uint8_t PANDA_CAN_CNT = 3U; -const uint8_t PANDA_BUS_CNT = 4U; +const uint8_t PANDA_BUS_CNT = 3U; // bump this when changing the CAN packet #define CAN_PACKET_VERSION 4 diff --git a/panda/board/config.h b/panda/board/config.h index 0adcc3e09a..ef9ffcb8dc 100644 --- a/panda/board/config.h +++ b/panda/board/config.h @@ -11,7 +11,6 @@ //#define DEBUG_FAN #define CAN_INIT_TIMEOUT_MS 500U -#define DEEPSLEEP_WAKEUP_DELAY 3U #define USBPACKET_MAX_SIZE 0x40U #define MAX_CAN_MSGS_PER_USB_BULK_TRANSFER 51U #define MAX_CAN_MSGS_PER_SPI_BULK_TRANSFER 170U @@ -37,7 +36,7 @@ #ifdef STM32H7 #include "stm32h7/stm32h7_config.h" #elif defined(STM32F4) - #include "stm32fx/stm32fx_config.h" + #include "stm32f4/stm32f4_config.h" #else // TODO: uncomment this, cppcheck complains // building for tests diff --git a/panda/board/debug/README.md b/panda/board/debug/README.md index d89eab818a..d475ce90b6 100644 --- a/panda/board/debug/README.md +++ b/panda/board/debug/README.md @@ -11,7 +11,7 @@ Connect an ST-Link V2 programmer to the SWD pins on the board. The pins that nee Make sure you're using a genuine one for boards that do not have a 3.3V panda power rail. For example, the tres runs at 1.8V, which is not supported by the clones. ## Openocd -Install openocd. For Ubuntu 20.04, the one in the package manager works fine: `sudo apt install openocd`. +Install openocd. For Ubuntu 24.04, the one in the package manager works fine: `sudo apt install openocd`. To run, use `./debug_f4.sh (TODO)` or `./debug_h7.sh` depending on the panda. diff --git a/panda/board/drivers/bootkick.h b/panda/board/drivers/bootkick.h index acae3616af..4b75f09088 100644 --- a/panda/board/drivers/bootkick.h +++ b/panda/board/drivers/bootkick.h @@ -29,7 +29,7 @@ void bootkick_tick(bool ignition, bool recent_heartbeat) { * once BOOT_RESET is triggered, it stays until countdown is finished */ if (!bootkick_reset_triggered && (boot_state == BOOT_BOOTKICK) && (boot_state_prev == BOOT_STANDBY)) { - waiting_to_boot_countdown = 45U; + waiting_to_boot_countdown = 20U; } if (waiting_to_boot_countdown > 0U) { bool serial_activity = uart_ring_som_debug.w_ptr_tx != bootkick_last_serial_ptr; diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h index ea2705d9d0..27ffcee525 100644 --- a/panda/board/drivers/bxcan.h +++ b/panda/board/drivers/bxcan.h @@ -48,7 +48,7 @@ void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) { can_health[can_number].total_error_cnt += 1U; // RX message lost due to FIFO overrun - if ((CANx->RF0R & (CAN_RF0R_FOVR0)) != 0) { + if ((CANx->RF0R & (CAN_RF0R_FOVR0)) != 0U) { can_health[can_number].total_rx_lost_cnt += 1U; CANx->RF0R &= ~(CAN_RF0R_FOVR0); } @@ -74,7 +74,7 @@ void process_can(uint8_t can_number) { // check for empty mailbox CANPacket_t to_send; - if ((CANx->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0) { // last TX failed due to error arbitration lost + if ((CANx->TSR & (CAN_TSR_TERR0 | CAN_TSR_ALST0)) != 0U) { // last TX failed due to error arbitration lost can_health[can_number].total_tx_lost_cnt += 1U; CANx->TSR |= (CAN_TSR_TERR0 | CAN_TSR_ALST0); } @@ -129,7 +129,7 @@ void can_rx(uint8_t can_number) { CAN_TypeDef *CANx = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - while ((CANx->RF0R & CAN_RF0R_FMP0) != 0) { + while ((CANx->RF0R & CAN_RF0R_FMP0) != 0U) { can_health[can_number].total_rx_cnt += 1U; // can is live diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index bc9adde7ce..3a829c69ab 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -20,7 +20,6 @@ uint32_t safety_tx_blocked = 0; uint32_t safety_rx_invalid = 0; uint32_t tx_buffer_overflow = 0; uint32_t rx_buffer_overflow = 0; -uint32_t gmlan_send_errs = 0; can_health_t can_health[] = {{0}, {0}, {0}}; @@ -54,7 +53,6 @@ void process_can(uint8_t can_number); #define CAN_RX_BUFFER_SIZE 4096U #define CAN_TX_BUFFER_SIZE 416U -#define GMLAN_TX_BUFFER_SIZE 416U #ifdef STM32H7 // ITCM RAM and DTCM RAM are the fastest for Cortex-M7 core access @@ -67,10 +65,10 @@ can_buffer(tx1_q, CAN_TX_BUFFER_SIZE) can_buffer(tx2_q, CAN_TX_BUFFER_SIZE) #endif can_buffer(tx3_q, CAN_TX_BUFFER_SIZE) -can_buffer(txgmlan_q, GMLAN_TX_BUFFER_SIZE) + // FIXME: // cppcheck-suppress misra-c2012-9.3 -can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; +can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; // helpers #define WORD_TO_BYTE_ARRAY(dst8, src32) 0[dst8] = ((src32) & 0xFFU); 1[dst8] = (((src32) >> 8U) & 0xFFU); 2[dst8] = (((src32) >> 16U) & 0xFFU); 3[dst8] = (((src32) >> 24U) & 0xFFU) @@ -122,8 +120,6 @@ bool can_push(can_ring *q, const CANPacket_t *elem) { print("can_tx2_q"); } else if (q == &can_tx3_q) { print("can_tx3_q"); - } else if (q == &can_txgmlan_q) { - print("can_txgmlan_q"); } else { print("unknown"); } @@ -157,7 +153,7 @@ void can_clear(can_ring *q) { } // assign CAN numbering -// bus num: Can bus number on ODB connector. Sent to/from USB +// bus num: CAN Bus numbers in panda, sent to/from USB // Min: 0; Max: 127; Bit 7 marks message as receipt (bus 129 is receipt for but 1) // cans: Look up MCU can interface from bus number // can number: numeric lookup for MCU CAN interfaces (0 = CAN1, 1 = CAN2, etc); @@ -179,22 +175,20 @@ bus_config_t bus_config[] = { #define CAN_NUM_FROM_BUS_NUM(num) (bus_config[num].can_num_lookup) void can_init_all(void) { - bool ret = true; for (uint8_t i=0U; i < PANDA_CAN_CNT; i++) { if (!current_board->has_canfd) { bus_config[i].can_data_speed = 0U; } can_clear(can_queues[i]); - ret &= can_init(i); + (void)can_init(i); } - UNUSED(ret); } -void can_flip_buses(uint8_t bus1, uint8_t bus2){ - bus_config[bus1].bus_lookup = bus2; - bus_config[bus2].bus_lookup = bus1; - bus_config[bus1].can_num_lookup = bus2; - bus_config[bus2].can_num_lookup = bus1; +void can_set_orientation(bool flipped) { + bus_config[0].bus_lookup = flipped ? 2U : 0U; + bus_config[0].can_num_lookup = flipped ? 2U : 0U; + bus_config[2].bus_lookup = flipped ? 0U : 2U; + bus_config[2].can_num_lookup = flipped ? 0U : 2U; } void can_set_forwarding(uint8_t from, uint8_t to) { @@ -203,10 +197,10 @@ void can_set_forwarding(uint8_t from, uint8_t to) { void ignition_can_hook(CANPacket_t *to_push) { int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - int len = GET_LEN(to_push); - if (bus == 0) { + int addr = GET_ADDR(to_push); + int len = GET_LEN(to_push); + // GM exception if ((addr == 0x1F1) && (len == 8)) { // SystemPowerMode (2=Run, 3=Crank Request) @@ -234,8 +228,7 @@ bool can_tx_check_min_slots_free(uint32_t min) { return (can_slots_empty(&can_tx1_q) >= min) && (can_slots_empty(&can_tx2_q) >= min) && - (can_slots_empty(&can_tx3_q) >= min) && - (can_slots_empty(&can_txgmlan_q) >= min); + (can_slots_empty(&can_tx3_q) >= min); } uint8_t calculate_checksum(const uint8_t *dat, uint32_t len) { @@ -259,12 +252,8 @@ void can_send(CANPacket_t *to_push, uint8_t bus_number, bool skip_tx_hook) { if (skip_tx_hook || safety_tx_hook(to_push) != 0) { if (bus_number < PANDA_BUS_CNT) { // add CAN packet to send queue - if ((bus_number == 3U) && (bus_config[3].can_num_lookup == 0xFFU)) { - gmlan_send_errs += bitbang_gmlan(to_push) ? 0U : 1U; - } else { - tx_buffer_overflow += can_push(can_queues[bus_number], to_push) ? 0U : 1U; - process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); - } + tx_buffer_overflow += can_push(can_queues[bus_number], to_push) ? 0U : 1U; + process_can(CAN_NUM_FROM_BUS_NUM(bus_number)); } } else { safety_tx_blocked += 1U; diff --git a/panda/board/drivers/clock_source.h b/panda/board/drivers/clock_source.h index 11b2fa3247..5d6fdc8a77 100644 --- a/panda/board/drivers/clock_source.h +++ b/panda/board/drivers/clock_source.h @@ -26,8 +26,8 @@ void clock_source_init(void) { set_gpio_alternate(GPIOB, 15, GPIO_AF1_TIM1); // Set PWM mode - register_set(&(TIM1->CCMR1), (0b110 << TIM_CCMR1_OC2M_Pos), 0xFFFFU); - register_set(&(TIM1->CCMR2), (0b110 << TIM_CCMR2_OC3M_Pos), 0xFFFFU); + register_set(&(TIM1->CCMR1), (0b110UL << TIM_CCMR1_OC2M_Pos), 0xFFFFU); + register_set(&(TIM1->CCMR2), (0b110UL << TIM_CCMR2_OC3M_Pos), 0xFFFFU); // Enable output register_set(&(TIM1->BDTR), TIM_BDTR_MOE, 0xFFFFU); diff --git a/panda/board/drivers/fake_siren.h b/panda/board/drivers/fake_siren.h index 38c87deb0c..7c73be0b1c 100644 --- a/panda/board/drivers/fake_siren.h +++ b/panda/board/drivers/fake_siren.h @@ -59,13 +59,13 @@ void fake_siren_init(void) { register_set(&DMA1_Stream1->PAR, (uint32_t) &(DAC1->DHR8R1), 0xFFFFFFFFU); DMA1_Stream1->NDTR = sizeof(fake_siren_lut); register_set(&DMA1_Stream1->FCR, 0U, 0x00000083U); - DMA1_Stream1->CR = (0b11 << DMA_SxCR_PL_Pos); - DMA1_Stream1->CR |= DMA_SxCR_MINC | DMA_SxCR_CIRC | (1 << DMA_SxCR_DIR_Pos); + DMA1_Stream1->CR = (0b11UL << DMA_SxCR_PL_Pos); + DMA1_Stream1->CR |= DMA_SxCR_MINC | DMA_SxCR_CIRC | (1U << DMA_SxCR_DIR_Pos); // Init trigger timer (around 2.5kHz) register_set(&TIM7->PSC, 0U, 0xFFFFU); register_set(&TIM7->ARR, 133U, 0xFFFFU); - register_set(&TIM7->CR2, (0b10 << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); + register_set(&TIM7->CR2, (0b10U << TIM_CR2_MMS_Pos), TIM_CR2_MMS_Msk); register_set(&TIM7->CR1, TIM_CR1_ARPE | TIM_CR1_URS, 0x088EU); TIM7->SR = 0U; TIM7->CR1 |= TIM_CR1_CEN; diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h index bc4c2a532e..0ba228a9e6 100644 --- a/panda/board/drivers/fdcan.h +++ b/panda/board/drivers/fdcan.h @@ -67,14 +67,14 @@ void update_can_health_pkt(uint8_t can_number, uint32_t ir_reg) { FDCANx->IR |= (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L); can_health[can_number].total_error_cnt += 1U; // Check for RX FIFO overflow - if ((ir_reg & (FDCAN_IR_RF0L)) != 0) { + if ((ir_reg & (FDCAN_IR_RF0L)) != 0U) { can_health[can_number].total_rx_lost_cnt += 1U; } // Cases: // 1. while multiplexing between buses 1 and 3 we are getting ACK errors that overwhelm CAN core, by resetting it recovers faster // 2. H7 gets stuck in bus off recovery state indefinitely if ((((can_health[can_number].last_error == CAN_ACK_ERROR) || (can_health[can_number].last_data_error == CAN_ACK_ERROR)) && (can_health[can_number].transmit_error_cnt > 127U)) || - ((ir_reg & FDCAN_IR_BO) != 0)) { + ((ir_reg & FDCAN_IR_BO) != 0U)) { can_health[can_number].can_core_reset_cnt += 1U; can_health[can_number].total_tx_lost_cnt += (FDCAN_TX_FIFO_EL_CNT - (FDCANx->TXFQS & FDCAN_TXFQS_TFFL)); // TX FIFO msgs will be lost after reset llcan_clear_send(FDCANx); @@ -93,7 +93,7 @@ void process_can(uint8_t can_number) { FDCANx->IR |= FDCAN_IR_TFE; // Clear Tx FIFO Empty flag - if ((FDCANx->TXFQS & FDCAN_TXFQS_TFQF) == 0) { + if ((FDCANx->TXFQS & FDCAN_TXFQS_TFQF) == 0U) { CANPacket_t to_send; if (can_pop(can_queues[bus_number], &to_send)) { if (can_check_checksum(&to_send)) { @@ -101,7 +101,7 @@ void process_can(uint8_t can_number) { uint32_t TxFIFOSA = FDCAN_START_ADDRESS + (can_number * FDCAN_OFFSET) + (FDCAN_RX_FIFO_0_EL_CNT * FDCAN_RX_FIFO_0_EL_SIZE); // get the index of the next TX FIFO element (0 to FDCAN_TX_FIFO_EL_CNT - 1) - uint32_t tx_index = (FDCANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1F; + uint32_t tx_index = (FDCANx->TXFQS >> FDCAN_TXFQS_TFQPI_Pos) & 0x1FU; // only send if we have received a packet canfd_fifo *fifo; fifo = (canfd_fifo *)(TxFIFOSA + (tx_index * FDCAN_TX_FIFO_EL_SIZE)); @@ -126,7 +126,7 @@ void process_can(uint8_t can_number) { to_push.rejected = 0U; to_push.extended = to_send.extended; to_push.addr = to_send.addr; - to_push.bus = to_send.bus; + to_push.bus = bus_number; to_push.data_len_code = to_send.data_len_code; (void)memcpy(to_push.data, to_send.data, dlc_to_len[to_push.data_len_code]); can_set_checksum(&to_push); @@ -153,14 +153,14 @@ void can_rx(uint8_t can_number) { // Clear all new messages from Rx FIFO 0 FDCANx->IR |= FDCAN_IR_RF0N; - while((FDCANx->RXF0S & FDCAN_RXF0S_F0FL) != 0) { + while((FDCANx->RXF0S & FDCAN_RXF0S_F0FL) != 0U) { can_health[can_number].total_rx_cnt += 1U; // can is live pending_can_live = 1; // get the index of the next RX FIFO element (0 to FDCAN_RX_FIFO_0_EL_CNT - 1) - uint32_t rx_fifo_idx = (uint8_t)((FDCANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3F); + uint32_t rx_fifo_idx = (uint8_t)((FDCANx->RXF0S >> FDCAN_RXF0S_F0GI_Pos) & 0x3FU); // Recommended to offset get index by at least +1 if RX FIFO is in overwrite mode and full (datasheet) if((FDCANx->RXF0S & FDCAN_RXF0S_F0F) == FDCAN_RXF0S_F0F) { @@ -232,7 +232,7 @@ void can_rx(uint8_t can_number) { } // Error handling - if ((ir_reg & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L)) != 0) { + if ((ir_reg & (FDCAN_IR_PED | FDCAN_IR_PEA | FDCAN_IR_EP | FDCAN_IR_BO | FDCAN_IR_RF0L)) != 0U) { update_can_health_pkt(can_number, ir_reg); } } diff --git a/panda/board/drivers/gmlan_alt.h b/panda/board/drivers/gmlan_alt.h deleted file mode 100644 index 2dbc381bd2..0000000000 --- a/panda/board/drivers/gmlan_alt.h +++ /dev/null @@ -1,270 +0,0 @@ -#define GMLAN_TICKS_PER_SECOND 33300 //1sec @ 33.3kbps -#define GMLAN_TICKS_PER_TIMEOUT_TICKLE 500 //15ms @ 33.3kbps -#define GMLAN_HIGH 0 //0 is high on bus (dominant) -#define GMLAN_LOW 1 //1 is low on bus - -#define DISABLED -1 -#define BITBANG 0 -#define GPIO_SWITCH 1 - -#define MAX_BITS_CAN_PACKET (200) - -int gmlan_alt_mode = DISABLED; - -// returns out_len -int do_bitstuff(char *out, const char *in, int in_len) { - int last_bit = -1; - int bit_cnt = 0; - int j = 0; - for (int i = 0; i < in_len; i++) { - char bit = in[i]; - out[j] = bit; - j++; - - // do the stuffing - if (bit == (char)last_bit) { - bit_cnt++; - if (bit_cnt == 5) { - // 5 in a row the same, do stuff - last_bit = !bit ? 1 : 0; - out[j] = last_bit; - j++; - bit_cnt = 1; - } - } else { - // this is a new bit - last_bit = (int)bit; - bit_cnt = 1; - } - } - return j; -} - -int append_crc(char *in, int in_len) { - unsigned int crc = 0; - for (int i = 0; i < in_len; i++) { - crc <<= 1; - if (((unsigned int)(in[i]) ^ ((crc >> 15) & 1U)) != 0U) { - crc = crc ^ 0x4599U; - } - crc &= 0x7fffU; - } - int in_len_copy = in_len; - for (int i = 14; i >= 0; i--) { - in[in_len_copy] = (crc >> (unsigned int)(i)) & 1U; - in_len_copy++; - } - return in_len_copy; -} - -int append_bits(char *in, int in_len, const char *app, int app_len) { - int in_len_copy = in_len; - for (int i = 0; i < app_len; i++) { - in[in_len_copy] = app[i]; - in_len_copy++; - } - return in_len_copy; -} - -int append_int(char *in, int in_len, int val, int val_len) { - int in_len_copy = in_len; - for (int i = val_len - 1; i >= 0; i--) { - in[in_len_copy] = ((unsigned int)(val) & (1U << (unsigned int)(i))) != 0U; - in_len_copy++; - } - return in_len_copy; -} - -int get_bit_message(char *out, const CANPacket_t *to_bang) { - char pkt[MAX_BITS_CAN_PACKET]; - char footer[] = { - 1, // CRC delimiter - 1, // ACK - 1, // ACK delimiter - 1,1,1,1,1,1,1, // EOF - 1,1,1, // IFS - }; - - int len = 0; - - // test packet - int dlc_len = GET_LEN(to_bang); - len = append_int(pkt, len, 0, 1); // Start-of-frame - - if (to_bang->extended != 0U) { - // extended identifier - len = append_int(pkt, len, GET_ADDR(to_bang) >> 18, 11); // Identifier - len = append_int(pkt, len, 3, 2); // SRR+IDE - len = append_int(pkt, len, (GET_ADDR(to_bang)) & ((1UL << 18) - 1U), 18); // Identifier - len = append_int(pkt, len, 0, 3); // RTR+r1+r0 - } else { - // standard identifier - len = append_int(pkt, len, GET_ADDR(to_bang), 11); // Identifier - len = append_int(pkt, len, 0, 3); // RTR+IDE+reserved - } - - len = append_int(pkt, len, dlc_len, 4); // Data length code - - // append data - for (int i = 0; i < dlc_len; i++) { - len = append_int(pkt, len, to_bang->data[i], 8); - } - - // append crc - len = append_crc(pkt, len); - - // do bitstuffing - len = do_bitstuff(out, pkt, len); - - // append footer - len = append_bits(out, len, footer, sizeof(footer)); - return len; -} - -void TIM12_IRQ_Handler(void); - -void setup_timer(void) { - // register interrupt - REGISTER_INTERRUPT(TIM8_BRK_TIM12_IRQn, TIM12_IRQ_Handler, 40000U, FAULT_INTERRUPT_RATE_GMLAN) - - // setup - register_set(&(TIM12->PSC), (APB1_TIMER_FREQ-1U), 0xFFFFU); // Tick on 1 us - register_set(&(TIM12->CR1), TIM_CR1_CEN, 0x3FU); // Enable - register_set(&(TIM12->ARR), (30U-1U), 0xFFFFU); // 33.3 kbps - - // in case it's disabled - NVIC_EnableIRQ(TIM8_BRK_TIM12_IRQn); - - // run the interrupt - register_set(&(TIM12->DIER), TIM_DIER_UIE, 0x5F5FU); // Update interrupt - TIM12->SR = 0; -} - -int gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; //GMLAN transceiver times out every 17ms held high; tickle every 15ms -int can_timeout_counter = GMLAN_TICKS_PER_SECOND; //1 second - -int inverted_bit_to_send = GMLAN_HIGH; -int gmlan_switch_below_timeout = -1; -int gmlan_switch_timeout_enable = 0; - -void set_bitbanged_gmlan(int val) { - if (val != 0) { - register_set_bits(&(GPIOB->ODR), (1UL << 13)); - } else { - register_clear_bits(&(GPIOB->ODR), (1UL << 13)); - } -} - -char pkt_stuffed[MAX_BITS_CAN_PACKET]; -int gmlan_sending = -1; -int gmlan_sendmax = -1; -bool gmlan_send_ok = true; - -int gmlan_silent_count = 0; -int gmlan_fail_count = 0; -#define REQUIRED_SILENT_TIME 10 -#define MAX_FAIL_COUNT 10 - -void TIM12_IRQ_Handler(void) { - if (gmlan_alt_mode == BITBANG) { - if ((TIM12->SR & TIM_SR_UIF) && (gmlan_sendmax != -1)) { - int read = get_gpio_input(GPIOB, 12); - if (gmlan_silent_count < REQUIRED_SILENT_TIME) { - if (read == 0) { - gmlan_silent_count = 0; - } else { - gmlan_silent_count++; - } - } else { - bool retry = 0; - // in send loop - if ((gmlan_sending > 0) && // not first bit - ((read == 0) && (pkt_stuffed[gmlan_sending-1] == (char)1)) && // bus wrongly dominant - (gmlan_sending != (gmlan_sendmax - 11))) { //not ack bit - print("GMLAN ERR: bus driven at "); - puth(gmlan_sending); - print("\n"); - retry = 1; - } else if ((read == 1) && (gmlan_sending == (gmlan_sendmax - 11))) { // recessive during ACK - print("GMLAN ERR: didn't recv ACK\n"); - retry = 1; - } else { - // do not retry - } - if (retry) { - // reset sender (retry after 7 silent) - set_bitbanged_gmlan(1); // recessive - gmlan_silent_count = 0; - gmlan_sending = 0; - gmlan_fail_count++; - if (gmlan_fail_count == MAX_FAIL_COUNT) { - print("GMLAN ERR: giving up send\n"); - gmlan_send_ok = false; - } - } else { - set_bitbanged_gmlan(pkt_stuffed[gmlan_sending]); - gmlan_sending++; - } - } - if ((gmlan_sending == gmlan_sendmax) || (gmlan_fail_count == MAX_FAIL_COUNT)) { - set_bitbanged_gmlan(1); // recessive - set_gpio_mode(GPIOB, 13, MODE_INPUT); - register_clear_bits(&(TIM12->DIER), TIM_DIER_UIE); // No update interrupt - register_set(&(TIM12->CR1), 0U, 0x3FU); // Disable timer - gmlan_sendmax = -1; // exit - } - } - } else if (gmlan_alt_mode == GPIO_SWITCH) { - if ((TIM12->SR & TIM_SR_UIF) && (gmlan_switch_below_timeout != -1)) { - if ((can_timeout_counter == 0) && gmlan_switch_timeout_enable) { - //it has been more than 1 second since timeout was reset; disable timer and restore the GMLAN output - set_gpio_output(GPIOB, 13, GMLAN_LOW); - gmlan_switch_below_timeout = -1; - gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; - gmlan_alt_mode = DISABLED; - } - else { - can_timeout_counter--; - if (gmlan_timeout_counter == 0) { - //Send a 1 (bus low) every 15ms to reset the GMLAN transceivers timeout - gmlan_timeout_counter = GMLAN_TICKS_PER_TIMEOUT_TICKLE; - set_gpio_output(GPIOB, 13, GMLAN_LOW); - } - else { - set_gpio_output(GPIOB, 13, inverted_bit_to_send); - gmlan_timeout_counter--; - } - } - } - } else { - // Invalid GMLAN mode. Do not put a print statement here, way too fast to keep up with - } - TIM12->SR = 0; -} - -bool bitbang_gmlan(const CANPacket_t *to_bang) { - gmlan_send_ok = true; - gmlan_alt_mode = BITBANG; - -#ifdef HW_TYPE_DOS - if (hw_type == HW_TYPE_DOS) { - if (gmlan_sendmax == -1) { - int len = get_bit_message(pkt_stuffed, to_bang); - gmlan_fail_count = 0; - gmlan_silent_count = 0; - gmlan_sending = 0; - gmlan_sendmax = len; - // setup for bitbang loop - set_bitbanged_gmlan(1); // recessive - set_gpio_mode(GPIOB, 13, MODE_OUTPUT); - - // 33kbps - setup_timer(); - } - } -#else - UNUSED(to_bang); -#endif - - return gmlan_send_ok; -} diff --git a/panda/board/drivers/gpio.h b/panda/board/drivers/gpio.h index bf96a0326e..0b8fc091b1 100644 --- a/panda/board/drivers/gpio.h +++ b/panda/board/drivers/gpio.h @@ -11,7 +11,7 @@ #define OUTPUT_TYPE_OPEN_DRAIN 1U typedef struct { - GPIO_TypeDef *bank; + GPIO_TypeDef * const bank; uint8_t pin; } gpio_t; @@ -68,13 +68,13 @@ int get_gpio_input(const GPIO_TypeDef *GPIO, unsigned int pin) { return (GPIO->IDR & (1UL << pin)) == (1UL << pin); } -void gpio_set_all_output(const gpio_t *pins, uint8_t num_pins, bool enabled) { +void gpio_set_all_output(gpio_t *pins, uint8_t num_pins, bool enabled) { for (uint8_t i = 0; i < num_pins; i++) { set_gpio_output(pins[i].bank, pins[i].pin, enabled); } } -void gpio_set_bitmask(const gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { +void gpio_set_bitmask(gpio_t *pins, uint8_t num_pins, uint32_t bitmask) { for (uint8_t i = 0; i < num_pins; i++) { set_gpio_output(pins[i].bank, pins[i].pin, (bitmask >> i) & 1U); } diff --git a/panda/board/drivers/harness.h b/panda/board/drivers/harness.h index 60f99fc85c..f8f0ccb7d3 100644 --- a/panda/board/drivers/harness.h +++ b/panda/board/drivers/harness.h @@ -13,16 +13,16 @@ struct harness_t harness; struct harness_configuration { const bool has_harness; - GPIO_TypeDef *GPIO_SBU1; - GPIO_TypeDef *GPIO_SBU2; - GPIO_TypeDef *GPIO_relay_SBU1; - GPIO_TypeDef *GPIO_relay_SBU2; - uint8_t pin_SBU1; - uint8_t pin_SBU2; - uint8_t pin_relay_SBU1; - uint8_t pin_relay_SBU2; - uint8_t adc_channel_SBU1; - uint8_t adc_channel_SBU2; + GPIO_TypeDef * const GPIO_SBU1; + GPIO_TypeDef * const GPIO_SBU2; + GPIO_TypeDef * const GPIO_relay_SBU1; + GPIO_TypeDef * const GPIO_relay_SBU2; + const uint8_t pin_SBU1; + const uint8_t pin_SBU2; + const uint8_t pin_relay_SBU1; + const uint8_t pin_relay_SBU2; + const uint8_t adc_channel_SBU1; + const uint8_t adc_channel_SBU2; }; // The ignition relay is only used for testing purposes @@ -117,11 +117,6 @@ void harness_tick(void) { } void harness_init(void) { - // delay such that the connection is fully made before trying orientation detection - current_board->set_led(LED_BLUE, true); - delay(10000000); - current_board->set_led(LED_BLUE, false); - // try to detect orientation harness.status = harness_detect_orientation(); if (harness.status != HARNESS_STATUS_NC) { diff --git a/panda/board/drivers/interrupts.h b/panda/board/drivers/interrupts.h index d4c72be1df..77988f6e45 100644 --- a/panda/board/drivers/interrupts.h +++ b/panda/board/drivers/interrupts.h @@ -64,7 +64,7 @@ void handle_interrupt(IRQn_Type irq_type){ // Every second void interrupt_timer_handler(void) { - if (INTERRUPT_TIMER->SR != 0) { + if (INTERRUPT_TIMER->SR != 0U) { for (uint16_t i = 0U; i < NUM_INTERRUPTS; i++) { // Log IRQ call rate faults if (check_interrupt_rate && (interrupts[i].call_counter > interrupts[i].max_call_rate)) { diff --git a/panda/board/drivers/registers.h b/panda/board/drivers/registers.h index a5f8b0280c..5d5a4257d7 100644 --- a/panda/board/drivers/registers.h +++ b/panda/board/drivers/registers.h @@ -44,13 +44,13 @@ void register_set(volatile uint32_t *addr, uint32_t val, uint32_t mask){ // Set individual bits. Also add them to the check_mask. // Do not use this to change bits that get reset by the hardware void register_set_bits(volatile uint32_t *addr, uint32_t val) { - return register_set(addr, val, val); + register_set(addr, val, val); } // Clear individual bits. Also add them to the check_mask. // Do not use this to clear bits that get set by the hardware void register_clear_bits(volatile uint32_t *addr, uint32_t val) { - return register_set(addr, (~val), val); + register_set(addr, (~val), val); } // To be called periodically diff --git a/panda/board/drivers/rtc.h b/panda/board/drivers/rtc.h deleted file mode 100644 index df121e3e89..0000000000 --- a/panda/board/drivers/rtc.h +++ /dev/null @@ -1,79 +0,0 @@ -#define YEAR_OFFSET 2000U - -typedef struct __attribute__((packed)) timestamp_t { - uint16_t year; - uint8_t month; - uint8_t day; - uint8_t weekday; - uint8_t hour; - uint8_t minute; - uint8_t second; -} timestamp_t; - -uint8_t to_bcd(uint16_t value){ - return (((value / 10U) & 0x0FU) << 4U) | ((value % 10U) & 0x0FU); -} - -uint16_t from_bcd(uint8_t value){ - return (((value & 0xF0U) >> 4U) * 10U) + (value & 0x0FU); -} - -void rtc_set_time(timestamp_t time){ - print("Setting RTC time\n"); - - // Disable write protection - disable_bdomain_protection(); - RTC->WPR = 0xCA; - RTC->WPR = 0x53; - - // Enable initialization mode - register_set_bits(&(RTC->ISR), RTC_ISR_INIT); - while((RTC->ISR & RTC_ISR_INITF) == 0){} - - // Set time - RTC->TR = (to_bcd(time.hour) << RTC_TR_HU_Pos) | (to_bcd(time.minute) << RTC_TR_MNU_Pos) | (to_bcd(time.second) << RTC_TR_SU_Pos); - RTC->DR = (to_bcd(time.year - YEAR_OFFSET) << RTC_DR_YU_Pos) | (time.weekday << RTC_DR_WDU_Pos) | (to_bcd(time.month) << RTC_DR_MU_Pos) | (to_bcd(time.day) << RTC_DR_DU_Pos); - - // Set options - register_set(&(RTC->CR), 0U, 0xFCFFFFU); - - // Disable initalization mode - register_clear_bits(&(RTC->ISR), RTC_ISR_INIT); - - // Wait for synchronization - while((RTC->ISR & RTC_ISR_RSF) == 0){} - - // Re-enable write protection - RTC->WPR = 0x00; - enable_bdomain_protection(); -} - -timestamp_t rtc_get_time(void){ - timestamp_t result; - // Init with zero values in case there is no RTC running - result.year = 0U; - result.month = 0U; - result.day = 0U; - result.weekday = 0U; - result.hour = 0U; - result.minute = 0U; - result.second = 0U; - - // Wait until the register sync flag is set - while((RTC->ISR & RTC_ISR_RSF) == 0){} - - // Read time and date registers. Since our HSE > 7*LSE, this should be fine. - uint32_t time = RTC->TR; - uint32_t date = RTC->DR; - - // Parse values - result.year = from_bcd((date & (RTC_DR_YT | RTC_DR_YU)) >> RTC_DR_YU_Pos) + YEAR_OFFSET; - result.month = from_bcd((date & (RTC_DR_MT | RTC_DR_MU)) >> RTC_DR_MU_Pos); - result.day = from_bcd((date & (RTC_DR_DT | RTC_DR_DU)) >> RTC_DR_DU_Pos); - result.weekday = ((date & RTC_DR_WDU) >> RTC_DR_WDU_Pos); - result.hour = from_bcd((time & (RTC_TR_HT | RTC_TR_HU)) >> RTC_TR_HU_Pos); - result.minute = from_bcd((time & (RTC_TR_MNT | RTC_TR_MNU)) >> RTC_TR_MNU_Pos); - result.second = from_bcd((time & (RTC_TR_ST | RTC_TR_SU)) >> RTC_TR_SU_Pos); - - return result; -} diff --git a/panda/board/drivers/spi.h b/panda/board/drivers/spi.h index 1a7b9be590..61ae2aba9d 100644 --- a/panda/board/drivers/spi.h +++ b/panda/board/drivers/spi.h @@ -43,7 +43,7 @@ uint16_t spi_data_len_miso; uint16_t spi_checksum_error_count = 0; bool spi_can_tx_ready = false; -const char version_text[] = "VERSION"; +const unsigned char version_text[] = "VERSION"; #define SPI_HEADER_SIZE 7U @@ -73,10 +73,8 @@ uint16_t spi_version_packet(uint8_t *out) { uint16_t data_pos = 7U + 2U; // write serial - #ifdef UID_BASE (void)memcpy(&out[data_pos], ((uint8_t *)UID_BASE), 12); data_len += 12U; - #endif // HW type out[data_pos + data_len] = hw_type; @@ -154,7 +152,7 @@ void spi_rx_done(void) { if (checksum_valid) { if (spi_endpoint == 0U) { if (spi_data_len_mosi >= sizeof(ControlPacket_t)) { - ControlPacket_t ctrl; + ControlPacket_t ctrl = {0}; (void)memcpy(&ctrl, &spi_buf_rx[SPI_HEADER_SIZE], sizeof(ControlPacket_t)); response_len = comms_control_handler(&ctrl, &spi_buf_tx[3]); response_ack = true; @@ -184,6 +182,10 @@ void spi_rx_done(void) { } else { print("SPI: did expect data for can_write\n"); } + } else if (spi_endpoint == 0xABU) { + // test endpoint, send max response length + response_len = spi_data_len_miso; + response_ack = true; } else { print("SPI: unexpected endpoint"); puth(spi_endpoint); print("\n"); } @@ -232,7 +234,7 @@ void spi_rx_done(void) { llspi_miso_dma(spi_buf_tx, response_len); spi_state = next_rx_state; - if (!checksum_valid && (spi_checksum_error_count < __UINT16_MAX__)) { + if (!checksum_valid && (spi_checksum_error_count < UINT16_MAX)) { spi_checksum_error_count += 1U; } } diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h index 1872dd5276..e006372216 100644 --- a/panda/board/drivers/usb.h +++ b/panda/board/drivers/usb.h @@ -304,13 +304,6 @@ uint8_t binary_object_store_desc[] = { MS_VENDOR_CODE, 0x00 // vendor code, no alternate enumeration }; -uint8_t webusb_url_descriptor[] = { - 0x14, /* bLength */ - WEBUSB_DESC_TYPE_URL, // bDescriptorType - WEBUSB_URL_SCHEME_HTTPS, // bScheme - 'u', 's', 'b', 'p', 'a', 'n', 'd', 'a', '.', 'c', 'o', 'm', 'm', 'a', '.', 'a', 'i' -}; - // WinUSB 2.0 descriptor. This is what modern systems use // https://github.com/sowbug/weblight/blob/192ad7a0e903542e2aa28c607d98254a12a6399d/firmware/webusb.c // http://janaxelson.com/files/ms_os_20_descriptors.c @@ -364,10 +357,10 @@ int current_int0_alt_setting = 0; void *USB_ReadPacket(void *dest, uint16_t len) { uint32_t *dest_copy = (uint32_t *)dest; - uint32_t count32b = (len + 3U) / 4U; + uint32_t count32b = ((uint32_t)len + 3U) / 4U; for (uint32_t i = 0; i < count32b; i++) { - *dest_copy = USBx_DFIFO(0); + *dest_copy = USBx_DFIFO(0U); dest_copy++; } return ((void *)dest_copy); @@ -379,9 +372,9 @@ void USB_WritePacket(const void *src, uint16_t len, uint32_t ep) { hexdump(src, len); #endif - uint32_t numpacket = (len + (USBPACKET_MAX_SIZE - 1U)) / USBPACKET_MAX_SIZE; + uint32_t numpacket = ((uint32_t)len + (USBPACKET_MAX_SIZE - 1U)) / USBPACKET_MAX_SIZE; uint32_t count32b = 0; - count32b = (len + 3U) / 4U; + count32b = ((uint32_t)len + 3U) / 4U; // TODO: revisit this USBx_INEP(ep)->DIEPTSIZ = ((numpacket << 19) & USB_OTG_DIEPTSIZ_PKTCNT) | @@ -414,7 +407,7 @@ void USB_WritePacket_EP0(uint8_t *src, uint16_t len) { ep0_txlen = len - wplen; USBx_DEVICE->DIEPEMPMSK |= 1; } else { - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; } } @@ -431,8 +424,8 @@ void usb_reset(void) { USBx_DEVICE->DOEPMSK = 0xFFFFFFFFU; // clear interrupts - USBx_INEP(0)->DIEPINT = 0xFF; - USBx_OUTEP(0)->DOEPINT = 0xFF; + USBx_INEP(0U)->DIEPINT = 0xFF; + USBx_OUTEP(0U)->DOEPINT = 0xFF; // unset the address USBx_DEVICE->DCFG &= ~USB_OTG_DCFG_DAD; @@ -458,7 +451,7 @@ void usb_reset(void) { USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGINAK; // ready to receive setup packets - USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (3U << 3); + USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (3U << 3); } char to_hex_char(uint8_t a) { @@ -485,26 +478,26 @@ void usb_setup(void) { switch (setup.b.bRequest) { case USB_REQ_SET_CONFIGURATION: // enable other endpoints, has to be here? - USBx_INEP(1)->DIEPCTL = (0x40U & USB_OTG_DIEPCTL_MPSIZ) | (2UL << 18) | (1UL << 22) | + USBx_INEP(1U)->DIEPCTL = (0x40U & USB_OTG_DIEPCTL_MPSIZ) | (2UL << 18) | (1UL << 22) | USB_OTG_DIEPCTL_SD0PID_SEVNFRM | USB_OTG_DIEPCTL_USBAEP; - USBx_INEP(1)->DIEPINT = 0xFF; + USBx_INEP(1U)->DIEPINT = 0xFF; - USBx_OUTEP(2)->DOEPTSIZ = (1UL << 19) | 0x40U; - USBx_OUTEP(2)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | + USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; + USBx_OUTEP(2U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; - USBx_OUTEP(2)->DOEPINT = 0xFF; + USBx_OUTEP(2U)->DOEPINT = 0xFF; - USBx_OUTEP(3)->DOEPTSIZ = (32UL << 19) | 0x800U; - USBx_OUTEP(3)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | + USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; + USBx_OUTEP(3U)->DOEPCTL = (0x40U & USB_OTG_DOEPCTL_MPSIZ) | (2UL << 18) | USB_OTG_DOEPCTL_SD0PID_SEVNFRM | USB_OTG_DOEPCTL_USBAEP; - USBx_OUTEP(3)->DOEPINT = 0xFF; + USBx_OUTEP(3U)->DOEPINT = 0xFF; // mark ready to receive - USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; USB_WritePacket(0, 0, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_REQ_SET_ADDRESS: // set now? @@ -515,7 +508,7 @@ void usb_setup(void) { #endif USB_WritePacket(0, 0, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_REQ_GET_DESCRIPTOR: @@ -527,17 +520,17 @@ void usb_setup(void) { device_desc[13] = hw_type; // setup transfer USB_WritePacket(device_desc, MIN(sizeof(device_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; //print("D"); break; case USB_DESC_TYPE_CONFIGURATION: USB_WritePacket(configuration_desc, MIN(sizeof(configuration_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_DESC_TYPE_DEVICE_QUALIFIER: USB_WritePacket(device_qualifier, MIN(sizeof(device_qualifier), setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_DESC_TYPE_STRING: switch (setup.b.wValue.bw.msb) { @@ -551,23 +544,19 @@ void usb_setup(void) { USB_WritePacket((uint8_t*)string_product_desc, MIN(sizeof(string_product_desc), setup.b.wLength.w), 0); break; case STRING_OFFSET_ISERIAL: - #ifdef UID_BASE - response[0] = 0x02 + (12 * 4); - response[1] = 0x03; - - // 96 bits = 12 bytes - for (int i = 0; i < 12; i++){ - uint8_t cc = ((uint8_t *)UID_BASE)[i]; - response[2 + (i * 4)] = to_hex_char((cc >> 4) & 0xFU); - response[2 + (i * 4) + 1] = '\0'; - response[2 + (i * 4) + 2] = to_hex_char((cc >> 0) & 0xFU); - response[2 + (i * 4) + 3] = '\0'; - } - - USB_WritePacket(response, MIN(response[0], setup.b.wLength.w), 0); - #else - USB_WritePacket((const uint8_t *)string_serial_desc, MIN(sizeof(string_serial_desc), setup.b.wLength.w), 0); - #endif + response[0] = 0x02 + (12 * 4); + response[1] = 0x03; + + // 96 bits = 12 bytes + for (int i = 0; i < 12; i++){ + uint8_t cc = ((uint8_t *)UID_BASE)[i]; + response[2 + (i * 4)] = to_hex_char((cc >> 4) & 0xFU); + response[2 + (i * 4) + 1] = '\0'; + response[2 + (i * 4) + 2] = to_hex_char((cc >> 0) & 0xFU); + response[2 + (i * 4) + 3] = '\0'; + } + + USB_WritePacket(response, MIN(response[0], setup.b.wLength.w), 0); break; case STRING_OFFSET_ICONFIGURATION: USB_WritePacket((uint8_t*)string_configuration_desc, MIN(sizeof(string_configuration_desc), setup.b.wLength.w), 0); @@ -580,16 +569,16 @@ void usb_setup(void) { USB_WritePacket(0, 0, 0); break; } - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_DESC_TYPE_BINARY_OBJECT_STORE: USB_WritePacket(binary_object_store_desc, MIN(sizeof(binary_object_store_desc), setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; default: // nothing here? USB_WritePacket(0, 0, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; } break; @@ -598,26 +587,18 @@ void usb_setup(void) { response[0] = 0; response[1] = 0; USB_WritePacket((void*)&response, 2, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case USB_REQ_SET_INTERFACE: // Store the alt setting number for IN EP behavior. current_int0_alt_setting = setup.b.wValue.w; USB_WritePacket(0, 0, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case WEBUSB_VENDOR_CODE: - switch (setup.b.wIndex.w) { - case WEBUSB_REQ_GET_URL: - USB_WritePacket(webusb_url_descriptor, MIN(sizeof(webusb_url_descriptor), setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - default: - // probably asking for allowed origins, which was removed from the spec - USB_WritePacket(0, 0, 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - break; - } + // probably asking for allowed origins, which was removed from the spec + USB_WritePacket(0, 0, 0); + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; break; case MS_VENDOR_CODE: switch (setup.b.wIndex.w) { @@ -647,7 +628,7 @@ void usb_setup(void) { // response pending if -1 was returned if (resp_len != -1) { USB_WritePacket(response, MIN(resp_len, setup.b.wLength.w), 0); - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; } } } @@ -675,23 +656,23 @@ void usb_irqhandler(void) { print(" USB interrupt!\n"); #endif - if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0) { + if ((gintsts & USB_OTG_GINTSTS_CIDSCHG) != 0U) { print("connector ID status change\n"); } - if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0) { + if ((gintsts & USB_OTG_GINTSTS_USBRST) != 0U) { print("USB reset\n"); usb_reset(); } - if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0) { + if ((gintsts & USB_OTG_GINTSTS_ENUMDNE) != 0U) { print("enumeration done"); // Full speed, ENUMSPD //puth(USBx_DEVICE->DSTS); print("\n"); } - if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0) { + if ((gintsts & USB_OTG_GINTSTS_OTGINT) != 0U) { print("OTG int:"); puth(USBx->GOTGINT); print("\n"); @@ -701,7 +682,7 @@ void usb_irqhandler(void) { } // RX FIFO first - if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0) { + if ((gintsts & USB_OTG_GINTSTS_RXFLVL) != 0U) { // 1. Read the Receive status pop register volatile unsigned int rxst = USBx->GRXSTSP; int status = (rxst & USB_OTG_GRXSTSP_PKTSTS) >> 17; @@ -767,7 +748,7 @@ void usb_irqhandler(void) { USBx_DEVICE->DCTL |= USB_OTG_DCTL_CGONAK | USB_OTG_DCTL_CGINAK; } - if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0) { + if ((gintsts & USB_OTG_GINTSTS_SRQINT) != 0U) { // we want to do "A-device host negotiation protocol" since we are the A-device /*print("start request\n"); puth(USBx->GOTGCTL); @@ -778,76 +759,76 @@ void usb_irqhandler(void) { } // out endpoint hit - if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0) { + if ((gintsts & USB_OTG_GINTSTS_OEPINT) != 0U) { #ifdef DEBUG_USB print(" 0:"); - puth(USBx_OUTEP(0)->DOEPINT); + puth(USBx_OUTEP(0U)->DOEPINT); print(" 2:"); - puth(USBx_OUTEP(2)->DOEPINT); + puth(USBx_OUTEP(2U)->DOEPINT); print(" 3:"); - puth(USBx_OUTEP(3)->DOEPINT); + puth(USBx_OUTEP(3U)->DOEPINT); print(" "); - puth(USBx_OUTEP(3)->DOEPCTL); + puth(USBx_OUTEP(3U)->DOEPCTL); print(" 4:"); puth(USBx_OUTEP(4)->DOEPINT); print(" OUT ENDPOINT\n"); #endif - if ((USBx_OUTEP(2)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) { + if ((USBx_OUTEP(2U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { #ifdef DEBUG_USB print(" OUT2 PACKET XFRC\n"); #endif - USBx_OUTEP(2)->DOEPTSIZ = (1UL << 19) | 0x40U; - USBx_OUTEP(2)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(2U)->DOEPTSIZ = (1UL << 19) | 0x40U; + USBx_OUTEP(2U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; } - if ((USBx_OUTEP(3)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0) { + if ((USBx_OUTEP(3U)->DOEPINT & USB_OTG_DOEPINT_XFRC) != 0U) { #ifdef DEBUG_USB print(" OUT3 PACKET XFRC\n"); #endif // NAK cleared by process_can (if tx buffers have room) outep3_processing = false; refresh_can_tx_slots_available(); - } else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) { + } else if ((USBx_OUTEP(3U)->DOEPINT & 0x2000U) != 0U) { #ifdef DEBUG_USB print(" OUT3 PACKET WTF\n"); #endif // if NAK was set trigger this, unknown interrupt // TODO: why was this here? fires when TX buffers when we can't clear NAK - // USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; - // USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; - } else if ((USBx_OUTEP(3)->DOEPINT) != 0) { + // USBx_OUTEP(3U)->DOEPTSIZ = (1U << 19) | 0x40U; + // USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + } else if ((USBx_OUTEP(3U)->DOEPINT) != 0U) { #ifdef DEBUG_USB print("OUTEP3 error "); - puth(USBx_OUTEP(3)->DOEPINT); + puth(USBx_OUTEP(3U)->DOEPINT); print("\n"); #endif } else { - // USBx_OUTEP(3)->DOEPINT is 0, ok to skip + // USBx_OUTEP(3U)->DOEPINT is 0, ok to skip } - if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0) { + if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DIEPINT_XFRC) != 0U) { // ready for next packet - USBx_OUTEP(0)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (1U << 3); + USBx_OUTEP(0U)->DOEPTSIZ = USB_OTG_DOEPTSIZ_STUPCNT | (USB_OTG_DOEPTSIZ_PKTCNT & (1UL << 19)) | (1U << 3); } // respond to setup packets - if ((USBx_OUTEP(0)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0) { + if ((USBx_OUTEP(0U)->DOEPINT & USB_OTG_DOEPINT_STUP) != 0U) { usb_setup(); } - USBx_OUTEP(0)->DOEPINT = USBx_OUTEP(0)->DOEPINT; - USBx_OUTEP(2)->DOEPINT = USBx_OUTEP(2)->DOEPINT; - USBx_OUTEP(3)->DOEPINT = USBx_OUTEP(3)->DOEPINT; + USBx_OUTEP(0U)->DOEPINT = USBx_OUTEP(0U)->DOEPINT; + USBx_OUTEP(2U)->DOEPINT = USBx_OUTEP(2U)->DOEPINT; + USBx_OUTEP(3U)->DOEPINT = USBx_OUTEP(3U)->DOEPINT; } // interrupt endpoint hit (Page 1221) - if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0) { + if ((gintsts & USB_OTG_GINTSTS_IEPINT) != 0U) { #ifdef DEBUG_USB print(" "); - puth(USBx_INEP(0)->DIEPINT); + puth(USBx_INEP(0U)->DIEPINT); print(" "); - puth(USBx_INEP(1)->DIEPINT); + puth(USBx_INEP(1U)->DIEPINT); print(" IN ENDPOINT\n"); #endif @@ -867,7 +848,7 @@ void usb_irqhandler(void) { switch (current_int0_alt_setting) { case 0: ////// Bulk config // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { #ifdef DEBUG_USB print(" IN PACKET QUEUE\n"); #endif @@ -878,7 +859,7 @@ void usb_irqhandler(void) { case 1: ////// Interrupt config // *** IN token received when TxFIFO is empty - if ((USBx_INEP(1)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if ((USBx_INEP(1U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { #ifdef DEBUG_USB print(" IN PACKET QUEUE\n"); #endif @@ -894,12 +875,12 @@ void usb_irqhandler(void) { break; } - if ((USBx_INEP(0)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0) { + if ((USBx_INEP(0U)->DIEPINT & USB_OTG_DIEPMSK_ITTXFEMSK) != 0U) { #ifdef DEBUG_USB print(" IN PACKET QUEUE\n"); #endif - if ((ep0_txlen != 0U) && ((USBx_INEP(0)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40U)) { + if ((ep0_txlen != 0U) && ((USBx_INEP(0U)->DTXFSTS & USB_OTG_DTXFSTS_INEPTFSAV) >= 0x40U)) { uint16_t len = MIN(ep0_txlen, 0x40); USB_WritePacket(ep0_txdata, len, 0); ep0_txdata = &ep0_txdata[len]; @@ -907,14 +888,14 @@ void usb_irqhandler(void) { if (ep0_txlen == 0U) { ep0_txdata = NULL; USBx_DEVICE->DIEPEMPMSK &= ~1; - USBx_OUTEP(0)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + USBx_OUTEP(0U)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; } } } // clear interrupts - USBx_INEP(0)->DIEPINT = USBx_INEP(0)->DIEPINT; // Why ep0? - USBx_INEP(1)->DIEPINT = USBx_INEP(1)->DIEPINT; + USBx_INEP(0U)->DIEPINT = USBx_INEP(0U)->DIEPINT; // Why ep0? + USBx_INEP(1U)->DIEPINT = USBx_INEP(1U)->DIEPINT; } // clear all interrupts we handled @@ -927,9 +908,9 @@ void usb_irqhandler(void) { void can_tx_comms_resume_usb(void) { ENTER_CRITICAL(); - if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) { - USBx_OUTEP(3)->DOEPTSIZ = (32UL << 19) | 0x800U; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + if (!outep3_processing && (USBx_OUTEP(3U)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0U) { + USBx_OUTEP(3U)->DOEPTSIZ = (32UL << 19) | 0x800U; + USBx_OUTEP(3U)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; } EXIT_CRITICAL(); } diff --git a/panda/board/early_init.h b/panda/board/early_init.h index ae652aebce..0e78274c3c 100644 --- a/panda/board/early_init.h +++ b/panda/board/early_init.h @@ -6,10 +6,15 @@ extern void *g_pfnVectors; extern uint32_t enter_bootloader_mode; +typedef void (*bootloader_fcn)(void); +typedef bootloader_fcn *bootloader_fcn_ptr; + void jump_to_bootloader(void) { // do enter bootloader enter_bootloader_mode = 0; - void (*bootloader)(void) = (void (*)(void)) (*((uint32_t *)BOOTLOADER_ADDRESS)); + + bootloader_fcn_ptr bootloader_ptr = (bootloader_fcn_ptr)BOOTLOADER_ADDRESS; + bootloader_fcn bootloader = *bootloader_ptr; // jump to bootloader enable_interrupts(); diff --git a/panda/board/faults.h b/panda/board/faults.h index a7f437dee5..dc6c1f2aa4 100644 --- a/panda/board/faults.h +++ b/panda/board/faults.h @@ -9,7 +9,7 @@ #define FAULT_INTERRUPT_RATE_CAN_2 (1UL << 3) #define FAULT_INTERRUPT_RATE_CAN_3 (1UL << 4) #define FAULT_INTERRUPT_RATE_TACH (1UL << 5) -#define FAULT_INTERRUPT_RATE_GMLAN (1UL << 6) +#define FAULT_INTERRUPT_RATE_GMLAN (1UL << 6) // deprecated #define FAULT_INTERRUPT_RATE_INTERRUPTS (1UL << 7) #define FAULT_INTERRUPT_RATE_SPI_DMA (1UL << 8) #define FAULT_INTERRUPT_RATE_SPI_CS (1UL << 9) diff --git a/panda/board/flash.py b/panda/board/flash.py index d1a78e2c51..903a6a645f 100755 --- a/panda/board/flash.py +++ b/panda/board/flash.py @@ -15,3 +15,4 @@ if __name__ == "__main__": print("flashing", s) with Panda(serial=s) as p: p.flash() + exit(1 if len(serials) == 0 else 0) diff --git a/panda/board/health.h b/panda/board/health.h index fc7661ca9c..74d822dc6c 100644 --- a/panda/board/health.h +++ b/panda/board/health.h @@ -1,6 +1,6 @@ // When changing these structs, python/__init__.py needs to be kept up to date! -#define HEALTH_PACKET_VERSION 15 +#define HEALTH_PACKET_VERSION 16 struct __attribute__((packed)) health_t { uint32_t uptime_pkt; uint32_t voltage_pkt; @@ -9,7 +9,6 @@ struct __attribute__((packed)) health_t { uint32_t safety_rx_invalid_pkt; uint32_t tx_buffer_overflow_pkt; uint32_t rx_buffer_overflow_pkt; - uint32_t gmlan_send_errs_pkt; uint32_t faults_pkt; uint8_t ignition_line_pkt; uint8_t ignition_can_pkt; diff --git a/panda/board/jungle/__init__.py b/panda/board/jungle/__init__.py index 77a6e3c7cc..0f968d8026 100644 --- a/panda/board/jungle/__init__.py +++ b/panda/board/jungle/__init__.py @@ -2,7 +2,6 @@ import os import struct from functools import wraps -from typing import Optional from panda import Panda, PandaDFU from panda.python.constants import McuType @@ -57,7 +56,7 @@ class PandaJungle(Panda): fn = os.path.join(FW_PATH, self._mcu_type.config.app_fn.replace("panda", "panda_jungle")) super().flash(fn=fn, code=code, reconnect=reconnect) - def recover(self, timeout: Optional[int] = 60, reset: bool = True) -> bool: + def recover(self, timeout: int | None = 60, reset: bool = True) -> bool: dfu_serial = self.get_dfu_serial() if reset: @@ -81,6 +80,12 @@ class PandaJungle(Panda): return McuType.F4 elif hw_type in PandaJungle.H7_DEVICES: return McuType.H7 + else: + # have to assume F4, see comment in Panda.connect + # initially Jungle V1 has HW type: bytearray(b'') + if hw_type == b'' or self._assume_f4_mcu: + return McuType.F4 + raise ValueError(f"unknown HW type: {hw_type}") def up_to_date(self, fn=None) -> bool: @@ -143,6 +148,9 @@ class PandaJungle(Panda): def set_can_silent(self, silent): self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xf5, int(silent), 0, b'') + def set_generated_can(self, enabled): + self._handle.controlWrite(PandaJungle.REQUEST_OUT, 0xa4, int(enabled), 0, b'') + # ******************* serial ******************* def debug_read(self): diff --git a/panda/board/jungle/boards/board_declarations.h b/panda/board/jungle/boards/board_declarations.h index 925918921f..2f65ff4a80 100644 --- a/panda/board/jungle/boards/board_declarations.h +++ b/panda/board/jungle/boards/board_declarations.h @@ -49,8 +49,6 @@ struct board { // CAN modes #define CAN_MODE_NORMAL 0U -#define CAN_MODE_GMLAN_CAN2 1U -#define CAN_MODE_GMLAN_CAN3 2U #define CAN_MODE_OBD_CAN2 3U // Harness states diff --git a/panda/board/jungle/boards/board_v1.h b/panda/board/jungle/boards/board_v1.h index 9581686e2a..efaacf60ce 100644 --- a/panda/board/jungle/boards/board_v1.h +++ b/panda/board/jungle/boards/board_v1.h @@ -157,7 +157,7 @@ void board_v1_init(void) { void board_v1_tick(void) {} -const board board_v1 = { +board board_v1 = { .has_canfd = false, .has_sbu_sense = false, .avdd_mV = 3300U, diff --git a/panda/board/jungle/boards/board_v2.h b/panda/board/jungle/boards/board_v2.h index 095114825b..ae4b2fc8d1 100644 --- a/panda/board/jungle/boards/board_v2.h +++ b/panda/board/jungle/boards/board_v2.h @@ -2,7 +2,7 @@ // Jungle board v2 (STM32H7) // // ///////////////////////// // -const gpio_t power_pins[] = { +gpio_t power_pins[] = { {.bank = GPIOA, .pin = 0}, {.bank = GPIOA, .pin = 1}, {.bank = GPIOF, .pin = 12}, @@ -11,7 +11,7 @@ const gpio_t power_pins[] = { {.bank = GPIOB, .pin = 2}, }; -const gpio_t sbu1_ignition_pins[] = { +gpio_t sbu1_ignition_pins[] = { {.bank = GPIOD, .pin = 0}, {.bank = GPIOD, .pin = 5}, {.bank = GPIOD, .pin = 12}, @@ -20,7 +20,7 @@ const gpio_t sbu1_ignition_pins[] = { {.bank = GPIOE, .pin = 9}, }; -const gpio_t sbu1_relay_pins[] = { +gpio_t sbu1_relay_pins[] = { {.bank = GPIOD, .pin = 1}, {.bank = GPIOD, .pin = 6}, {.bank = GPIOD, .pin = 11}, @@ -29,7 +29,7 @@ const gpio_t sbu1_relay_pins[] = { {.bank = GPIOE, .pin = 10}, }; -const gpio_t sbu2_ignition_pins[] = { +gpio_t sbu2_ignition_pins[] = { {.bank = GPIOD, .pin = 3}, {.bank = GPIOD, .pin = 8}, {.bank = GPIOD, .pin = 9}, @@ -38,7 +38,7 @@ const gpio_t sbu2_ignition_pins[] = { {.bank = GPIOE, .pin = 11}, }; -const gpio_t sbu2_relay_pins[] = { +gpio_t sbu2_relay_pins[] = { {.bank = GPIOD, .pin = 4}, {.bank = GPIOD, .pin = 10}, {.bank = GPIOD, .pin = 13}, @@ -47,7 +47,7 @@ const gpio_t sbu2_relay_pins[] = { {.bank = GPIOE, .pin = 12}, }; -const adc_channel_t sbu1_channels[] = { +adc_channel_t sbu1_channels[] = { {.adc = ADC3, .channel = 12}, {.adc = ADC3, .channel = 2}, {.adc = ADC3, .channel = 4}, @@ -56,7 +56,7 @@ const adc_channel_t sbu1_channels[] = { {.adc = ADC3, .channel = 10}, }; -const adc_channel_t sbu2_channels[] = { +adc_channel_t sbu2_channels[] = { {.adc = ADC1, .channel = 13}, {.adc = ADC3, .channel = 3}, {.adc = ADC3, .channel = 5}, @@ -307,7 +307,7 @@ void board_v2_init(void) { void board_v2_tick(void) {} -const board board_v2 = { +board board_v2 = { .has_canfd = true, .has_sbu_sense = true, .avdd_mV = 3300U, diff --git a/panda/board/jungle/flash.py b/panda/board/jungle/flash.py index 75a7f0c8ee..5b6c6e9046 100755 --- a/panda/board/jungle/flash.py +++ b/panda/board/jungle/flash.py @@ -15,3 +15,5 @@ if __name__ == "__main__": print("flashing", s) with PandaJungle(serial=s) as p: p.flash() + + exit(1 if len(serials) == 0 else 0) diff --git a/panda/board/jungle/main.c b/panda/board/jungle/main.c index f0a09dfbde..40777d9965 100644 --- a/panda/board/jungle/main.c +++ b/panda/board/jungle/main.c @@ -2,7 +2,6 @@ #include "board/config.h" #include "board/safety.h" -#include "board/drivers/gmlan_alt.h" #include "board/drivers/pwm.h" #include "board/drivers/usb.h" @@ -53,6 +52,14 @@ uint32_t loop_counter = 0U; uint16_t button_press_cnt = 0U; void tick_handler(void) { if (TICK_TIMER->SR != 0) { + if (generated_can_traffic) { + for (int i = 0; i < 3; i++) { + if (can_health[i].transmit_error_cnt >= 128) { + (void)llcan_init(CANIF_FROM_CAN_NUM(i)); + } + } + } + // tick drivers at 8Hz usb_tick(); @@ -194,8 +201,32 @@ int main(void) { #endif // LED should keep on blinking all the time - uint64_t cnt = 0; + uint32_t cnt = 0; for (cnt=0;;cnt++) { + if (generated_can_traffic) { + // fill up all the queues + can_ring *qs[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; + for (int j = 0; j < 3; j++) { + for (uint16_t n = 0U; n < can_slots_empty(qs[j]); n++) { + uint16_t i = cnt % 100U; + CANPacket_t to_send; + to_send.returned = 0U; + to_send.rejected = 0U; + to_send.extended = 0U; + to_send.addr = 0x200U + i; + to_send.bus = i % 3U; + to_send.data_len_code = i % 8U; + (void)memcpy(to_send.data, "\xff\xff\xff\xff\xff\xff\xff\xff", dlc_to_len[to_send.data_len_code]); + can_set_checksum(&to_send); + + can_send(&to_send, to_send.bus, true); + } + } + + delay(1000); + continue; + } + // useful for debugging, fade breaks = panda is overloaded for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) { current_board->set_led(LED_RED, true); diff --git a/panda/board/jungle/main_comms.h b/panda/board/jungle/main_comms.h index 928c554d1c..b2a58f52d4 100644 --- a/panda/board/jungle/main_comms.h +++ b/panda/board/jungle/main_comms.h @@ -1,5 +1,7 @@ extern int _app_start[0xc000]; // Only first 3 sectors of size 0x4000 are used +bool generated_can_traffic = false; + int get_jungle_health_pkt(void *dat) { COMPILE_TIME_ASSERT(sizeof(struct jungle_health_t) <= USBPACKET_MAX_SIZE); struct jungle_health_t * health = (struct jungle_health_t*)dat; @@ -58,10 +60,14 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { case 0xa2: current_board->set_ignition((req->param1 == 1U)); break; - // **** 0xa0: Set panda power per channel by bitmask. + // **** 0xa3: Set panda power per channel by bitmask. case 0xa3: current_board->set_panda_individual_power(req->param1, (req->param2 > 0U)); break; + // **** 0xa4: Enable generated CAN traffic. + case 0xa4: + generated_can_traffic = (req->param1 > 0U); + break; // **** 0xa8: get microsecond timer case 0xa8: time = microsecond_timer_get(); diff --git a/panda/board/jungle/recover.py b/panda/board/jungle/recover.py index 98afb06748..19666c3eda 100755 --- a/panda/board/jungle/recover.py +++ b/panda/board/jungle/recover.py @@ -24,3 +24,4 @@ if __name__ == "__main__": for s in dfu_serials: print("flashing", s) PandaJungleDFU(s).recover() + exit(1 if len(dfu_serials) == 0 else 0) diff --git a/panda/board/jungle/scripts/get_version.py b/panda/board/jungle/scripts/get_version.py index ad4a1c4264..4fc9d30bef 100755 --- a/panda/board/jungle/scripts/get_version.py +++ b/panda/board/jungle/scripts/get_version.py @@ -4,6 +4,6 @@ from panda import PandaJungle if __name__ == "__main__": for p in PandaJungle.list(): pp = PandaJungle(p) - print("%s: %s" % (pp.get_serial()[0], pp.get_version())) + print(f"{pp.get_serial()[0]}: {pp.get_version()}") diff --git a/panda/board/jungle/stm32fx/board.h b/panda/board/jungle/stm32f4/board.h similarity index 100% rename from panda/board/jungle/stm32fx/board.h rename to panda/board/jungle/stm32f4/board.h diff --git a/panda/board/main.c b/panda/board/main.c index 061a913814..b481c28b46 100644 --- a/panda/board/main.c +++ b/panda/board/main.c @@ -3,7 +3,6 @@ #include "drivers/pwm.h" #include "drivers/usb.h" -#include "drivers/gmlan_alt.h" #include "drivers/simple_watchdog.h" #include "drivers/bootkick.h" @@ -34,7 +33,6 @@ bool check_started(void) { bool started = current_board->check_ignition() || ignition_can; - ignition_seen |= started; return started; } @@ -99,6 +97,9 @@ void set_safety_mode(uint16_t mode, uint16_t param) { heartbeat_counter = 0U; heartbeat_lost = false; if (current_board->has_obd) { + // Clear any pending messages in the can core (i.e. sending while comma power is unplugged) + // TODO: rewrite using hardware queues rather than fifo to cancel specific messages + llcan_clear_send(CANIF_FROM_CAN_NUM(1)); if (param == 0U) { current_board->set_can_mode(CAN_MODE_OBD_CAN2); } else { @@ -145,8 +146,10 @@ void __attribute__ ((noinline)) enable_fpu(void) { // called at 8Hz uint8_t loop_counter = 0U; +uint8_t prev_harness_status = HARNESS_STATUS_NC; void tick_handler(void) { - if (TICK_TIMER->SR != 0) { + if (TICK_TIMER->SR != 0U) { + // siren current_board->set_siren((loop_counter & 1U) && (siren_enabled || (siren_countdown > 0U))); @@ -156,6 +159,17 @@ void tick_handler(void) { harness_tick(); simple_watchdog_kick(); + // re-init everything that uses harness status + if (harness.status != prev_harness_status) { + prev_harness_status = harness.status; + can_set_orientation(harness.status == HARNESS_STATUS_FLIPPED); + + // re-init everything that uses harness status + can_init_all(); + set_safety_mode(current_safety_mode, current_safety_param); + set_power_save_state(power_save_status); + } + // decimated to 1Hz if (loop_counter == 0U) { can_live = pending_can_live; @@ -187,7 +201,7 @@ void tick_handler(void) { bootkick_tick(check_started(), recent_heartbeat); // increase heartbeat counter and cap it at the uint32 limit - if (heartbeat_counter < __UINT32_MAX__) { + if (heartbeat_counter < UINT32_MAX) { heartbeat_counter += 1U; } @@ -280,38 +294,6 @@ void tick_handler(void) { TICK_TIMER->SR = 0; } -void EXTI_IRQ_Handler(void) { - if (check_exti_irq()) { - exti_irq_clear(); - clock_init(); - - set_power_save_state(POWER_SAVE_STATUS_DISABLED); - deepsleep_allowed = false; - heartbeat_counter = 0U; - usb_soft_disconnect(false); - - NVIC_EnableIRQ(TICK_TIMER_IRQ); - } -} - -uint8_t rtc_counter = 0; -void RTC_WKUP_IRQ_Handler(void) { - exti_irq_clear(); - clock_init(); - - rtc_counter++; - if ((rtc_counter % 2U) == 0U) { - current_board->set_led(LED_BLUE, false); - } else { - current_board->set_led(LED_BLUE, true); - } - - if (rtc_counter == __UINT8_MAX__) { - rtc_counter = 1U; - } -} - - int main(void) { // Init interrupt table init_interrupts(true); @@ -413,22 +395,6 @@ int main(void) { } #endif } else { - if (deepsleep_allowed && !usb_enumerated && !check_started() && ignition_seen && (heartbeat_counter > 20U)) { - usb_soft_disconnect(true); - fan_set_power(0U); - NVIC_DisableIRQ(TICK_TIMER_IRQ); - delay(512000U); - - // Init IRQs for CAN transceiver and ignition line - exti_irq_init(); - - // Init RTC Wakeup event on EXTI22 - REGISTER_INTERRUPT(RTC_WKUP_IRQn, RTC_WKUP_IRQ_Handler, 10U, FAULT_INTERRUPT_RATE_EXTI) - rtc_wakeup_init(); - - // STOP mode - SCB->SCR |= SCB_SCR_SLEEPDEEP_Msk; - } __WFI(); SCB->SCR &= ~SCB_SCR_SLEEPDEEP_Msk; } diff --git a/panda/board/main_comms.h b/panda/board/main_comms.h index c0d5c516fb..4c5d509624 100644 --- a/panda/board/main_comms.h +++ b/panda/board/main_comms.h @@ -21,7 +21,6 @@ int get_health_pkt(void *dat) { health->safety_rx_invalid_pkt = safety_rx_invalid; health->tx_buffer_overflow_pkt = tx_buffer_overflow; health->rx_buffer_overflow_pkt = rx_buffer_overflow; - health->gmlan_send_errs_pkt = gmlan_send_errs; health->car_harness_status_pkt = harness.status; health->safety_mode_pkt = (uint8_t)(current_safety_mode); health->safety_param_pkt = current_safety_param; @@ -48,12 +47,6 @@ int get_health_pkt(void *dat) { return sizeof(*health); } -int get_rtc_pkt(void *dat) { - timestamp_t t = rtc_get_time(); - (void)memcpy(dat, &t, sizeof(t)); - return sizeof(t); -} - // send on serial, first byte to select the ring void comms_endpoint2_write(const uint8_t *data, uint32_t len) { uart_ring *ur = get_ring_by_number(data[0]); @@ -71,7 +64,6 @@ void comms_endpoint2_write(const uint8_t *data, uint32_t len) { int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { unsigned int resp_len = 0; uart_ring *ur = NULL; - timestamp_t t; uint32_t time; #ifdef DEBUG_COMMS @@ -82,52 +74,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { #endif switch (req->request) { - // **** 0xa0: get rtc time - case 0xa0: - resp_len = get_rtc_pkt(resp); - break; - // **** 0xa1: set rtc year - case 0xa1: - t = rtc_get_time(); - t.year = req->param1; - rtc_set_time(t); - break; - // **** 0xa2: set rtc month - case 0xa2: - t = rtc_get_time(); - t.month = req->param1; - rtc_set_time(t); - break; - // **** 0xa3: set rtc day - case 0xa3: - t = rtc_get_time(); - t.day = req->param1; - rtc_set_time(t); - break; - // **** 0xa4: set rtc weekday - case 0xa4: - t = rtc_get_time(); - t.weekday = req->param1; - rtc_set_time(t); - break; - // **** 0xa5: set rtc hour - case 0xa5: - t = rtc_get_time(); - t.hour = req->param1; - rtc_set_time(t); - break; - // **** 0xa6: set rtc minute - case 0xa6: - t = rtc_get_time(); - t.minute = req->param1; - rtc_set_time(t); - break; - // **** 0xa7: set rtc second - case 0xa7: - t = rtc_get_time(); - t.second = req->param1; - rtc_set_time(t); - break; // **** 0xa8: get microsecond timer case 0xa8: time = microsecond_timer_get(); @@ -264,9 +210,9 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { case 0xd8: NVIC_SystemReset(); break; - // **** 0xdb: set GMLAN (white/grey) or OBD CAN (black) multiplexing mode + // **** 0xdb: set OBD CAN multiplexing mode case 0xdb: - if(current_board->has_obd){ + if (current_board->has_obd) { if (req->param1 == 1U) { // Enable OBD CAN current_board->set_can_mode(CAN_MODE_OBD_CAN2); @@ -428,10 +374,6 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { UNUSED(ret); } break; - // **** 0xfb: allow highest power saving mode (stop) to be entered - case 0xfb: - deepsleep_allowed = true; - break; // **** 0xfc: set CAN FD non-ISO mode case 0xfc: if ((req->param1 < PANDA_CAN_CNT) && current_board->has_canfd) { diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h index 4570c6e67d..a1496c0d0a 100644 --- a/panda/board/main_declarations.h +++ b/panda/board/main_declarations.h @@ -6,13 +6,12 @@ void puth4(unsigned int i); void hexdump(const void *a, int l); typedef struct board board; typedef struct harness_configuration harness_configuration; -void can_flip_buses(uint8_t bus1, uint8_t bus2); void pwm_init(TIM_TypeDef *TIM, uint8_t channel); void pwm_set(TIM_TypeDef *TIM, uint8_t channel, uint8_t percentage); // ********************* Globals ********************** uint8_t hw_type = 0; -const board *current_board; +board *current_board; uint32_t uptime_cnt = 0; bool green_led_enabled = false; @@ -21,10 +20,6 @@ uint32_t heartbeat_counter = 0; bool heartbeat_lost = false; bool heartbeat_disabled = false; // set over USB -// Enter deep sleep mode -bool deepsleep_allowed = false; -bool ignition_seen = false; - // siren state bool siren_enabled = false; uint32_t siren_countdown = 0; // siren plays while countdown > 0 diff --git a/panda/board/provision.h b/panda/board/provision.h index 02768c93d0..2a8ce893d3 100644 --- a/panda/board/provision.h +++ b/panda/board/provision.h @@ -3,7 +3,7 @@ #define PROVISION_CHUNK_LEN 0x20 -const char unprovisioned_text[] = "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"; +const unsigned char unprovisioned_text[] = "\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff"; void get_provision_chunk(uint8_t *resp) { (void)memcpy(resp, (uint8_t *)PROVISION_CHUNK_ADDRESS, PROVISION_CHUNK_LEN); diff --git a/panda/board/recover.py b/panda/board/recover.py index 5284a4e295..0a248525a8 100755 --- a/panda/board/recover.py +++ b/panda/board/recover.py @@ -24,3 +24,4 @@ if __name__ == "__main__": for s in dfu_serials: print("flashing", s) PandaDFU(s).recover() + exit(1 if len(dfu_serials) == 0 else 0) diff --git a/panda/board/safety.h b/panda/board/safety.h index 1e95244099..048d7cca48 100644 --- a/panda/board/safety.h +++ b/panda/board/safety.h @@ -327,8 +327,6 @@ int set_safety_hooks(uint16_t mode, uint16_t param) { // reset state set by safety mode safety_mode_cnt = 0U; relay_malfunction = false; - enable_gas_interceptor = false; - gas_interceptor_prev = 0; gas_pressed = false; gas_pressed_prev = false; brake_pressed = false; @@ -543,10 +541,6 @@ bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limit return violation; } -bool longitudinal_interceptor_checks(const CANPacket_t *to_send) { - return !get_longitudinal_allowed() && (GET_BYTE(to_send, 0) || GET_BYTE(to_send, 1)); -} - // Safety checks for torque-based steering commands bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLimits limits) { bool violation = false; diff --git a/panda/board/safety/safety_chrysler.h b/panda/board/safety/safety_chrysler.h index fa0e1532d5..be27832c9d 100644 --- a/panda/board/safety/safety_chrysler.h +++ b/panda/board/safety/safety_chrysler.h @@ -123,11 +123,12 @@ RxCheck chrysler_ram_hd_rx_checks[] = { const uint32_t CHRYSLER_PARAM_RAM_DT = 1U; // set for Ram DT platform const uint32_t CHRYSLER_PARAM_RAM_HD = 2U; // set for Ram HD platform -enum { +typedef enum { CHRYSLER_RAM_DT, CHRYSLER_RAM_HD, CHRYSLER_PACIFICA, // plus Jeep -} chrysler_platform = CHRYSLER_PACIFICA; +} ChryslerPlatform; +ChryslerPlatform chrysler_platform = CHRYSLER_PACIFICA; const ChryslerAddrs *chrysler_addrs = &CHRYSLER_ADDRS; static uint32_t chrysler_get_checksum(const CANPacket_t *to_push) { diff --git a/panda/board/safety/safety_ford.h b/panda/board/safety/safety_ford.h index 2f11863b49..d6ee208036 100644 --- a/panda/board/safety/safety_ford.h +++ b/panda/board/safety/safety_ford.h @@ -59,8 +59,10 @@ const CanMsg FORD_CANFD_LONG_TX_MSGS[] = { // this may be the cause of blocked messages RxCheck ford_rx_checks[] = { {.msg = {{FORD_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}}, - // TODO: FORD_EngVehicleSpThrottle2 has a counter that skips by 2, understand and enable counter check - {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .check_checksum = true, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}}, + // FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug + // Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC + // It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that + {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .check_checksum = false, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}}, {.msg = {{FORD_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .frequency = 100U}, { 0 }, { 0 }}}, // These messages have no counter or checksum {.msg = {{FORD_EngBrakeData, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, @@ -90,9 +92,6 @@ static uint32_t ford_get_checksum(const CANPacket_t *to_push) { if (addr == FORD_BrakeSysFeatures) { // Signal: VehVActlBrk_No_Cs chksum = GET_BYTE(to_push, 3); - } else if (addr == FORD_EngVehicleSpThrottle2) { - // Signal: VehVActlEng_No_Cs - chksum = GET_BYTE(to_push, 1); } else if (addr == FORD_Yaw_Data_FD1) { // Signal: VehRollYawW_No_Cs chksum = GET_BYTE(to_push, 4); @@ -110,11 +109,6 @@ static uint32_t ford_compute_checksum(const CANPacket_t *to_push) { chksum += GET_BYTE(to_push, 2) >> 6; // VehVActlBrk_D_Qf chksum += (GET_BYTE(to_push, 2) >> 2) & 0xFU; // VehVActlBrk_No_Cnt chksum = 0xFFU - chksum; - } else if (addr == FORD_EngVehicleSpThrottle2) { - chksum += (GET_BYTE(to_push, 2) >> 3) & 0xFU; // VehVActlEng_No_Cnt - chksum += (GET_BYTE(to_push, 4) >> 5) & 0x3U; // VehVActlEng_D_Qf - chksum += GET_BYTE(to_push, 6) + GET_BYTE(to_push, 7); // Veh_V_ActlEng - chksum = 0xFFU - chksum; } else if (addr == FORD_Yaw_Data_FD1) { chksum += GET_BYTE(to_push, 0) + GET_BYTE(to_push, 1); // VehRol_W_Actl chksum += GET_BYTE(to_push, 2) + GET_BYTE(to_push, 3); // VehYaw_W_Actl diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 82a5d9cd3e..09ac34ecbd 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -29,8 +29,7 @@ const int GM_STANDSTILL_THRSLD = 10; // 0.311kph const CanMsg GM_ASCM_TX_MSGS[] = {{0x180, 0, 4}, {0x409, 0, 7}, {0x40A, 0, 7}, {0x2CB, 0, 8}, {0x370, 0, 6}, // pt bus {0xA1, 1, 7}, {0x306, 1, 8}, {0x308, 1, 7}, {0x310, 1, 2}, // obs bus - {0x315, 2, 5}, // ch bus - {0x104c006c, 3, 3}, {0x10400060, 3, 5}}; // gmlan + {0x315, 2, 5}}; // ch bus const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, // pt bus {0x1E1, 2, 7}, {0x184, 2, 8}}; // camera bus @@ -60,7 +59,11 @@ enum { GM_BTN_CANCEL = 6, }; -enum {GM_ASCM, GM_CAM} gm_hw = GM_ASCM; +typedef enum { + GM_ASCM, + GM_CAM +} GmHardware; +GmHardware gm_hw = GM_ASCM; bool gm_cam_long = false; bool gm_pcm_cruise = false; diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h index 78bbb7f0bf..630f6d731c 100644 --- a/panda/board/safety/safety_honda.h +++ b/panda/board/safety/safety_honda.h @@ -1,16 +1,9 @@ const CanMsg HONDA_N_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x30C, 0, 8}, {0x33D, 0, 5}}; -const CanMsg HONDA_N_INTERCEPTOR_TX_MSGS[] = {{0xE4, 0, 5}, {0x194, 0, 4}, {0x1FA, 0, 8}, {0x200, 0, 6}, {0x30C, 0, 8}, {0x33D, 0, 5}}; const CanMsg HONDA_BOSCH_TX_MSGS[] = {{0xE4, 0, 5}, {0xE5, 0, 8}, {0x296, 1, 4}, {0x33D, 0, 5}, {0x33DA, 0, 5}, {0x33DB, 0, 8}}; // Bosch const CanMsg HONDA_BOSCH_LONG_TX_MSGS[] = {{0xE4, 1, 5}, {0x1DF, 1, 8}, {0x1EF, 1, 8}, {0x1FA, 1, 8}, {0x30C, 1, 8}, {0x33D, 1, 5}, {0x33DA, 1, 5}, {0x33DB, 1, 8}, {0x39F, 1, 8}, {0x18DAB0F1, 1, 8}}; // Bosch w/ gas and brakes const CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5}, {0x296, 2, 4}, {0x33D, 0, 8}}; // Bosch radarless const CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5}, {0x33D, 0, 8}, {0x1C8, 0, 8}, {0x30C, 0, 8}}; // Bosch radarless w/ gas and brakes -// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches -// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state -// Threshold calculated from DBC gains: round(((83.3 / 0.253984064) + (83.3 / 0.126992032)) / 2) = 492 -const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 492; -#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks - const LongitudinalLimits HONDA_BOSCH_LONG_LIMITS = { .max_accel = 200, // accel is used for brakes .min_accel = -350, @@ -47,11 +40,6 @@ RxCheck honda_common_rx_checks[] = { HONDA_COMMON_RX_CHECKS(0) }; -RxCheck honda_common_interceptor_rx_checks[] = { - HONDA_COMMON_RX_CHECKS(0) - {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, -}; - RxCheck honda_common_alt_brake_rx_checks[] = { HONDA_COMMON_RX_CHECKS(0) HONDA_ALT_BRAKE_ADDR_CHECK(0) @@ -62,11 +50,6 @@ RxCheck honda_nidec_alt_rx_checks[] = { HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0) }; -RxCheck honda_nidec_alt_interceptor_rx_checks[] = { - HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0) - {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, -}; - // Bosch has pt on bus 1, verified 0x1A6 does not exist RxCheck honda_bosch_rx_checks[] = { HONDA_COMMON_RX_CHECKS(1) @@ -81,7 +64,6 @@ const uint16_t HONDA_PARAM_ALT_BRAKE = 1; const uint16_t HONDA_PARAM_BOSCH_LONG = 2; const uint16_t HONDA_PARAM_NIDEC_ALT = 4; const uint16_t HONDA_PARAM_RADARLESS = 8; -const uint16_t HONDA_PARAM_GAS_INTERCEPTOR = 16; enum { HONDA_BTN_NONE = 0, @@ -97,7 +79,8 @@ bool honda_alt_brake_msg = false; bool honda_fwd_brake = false; bool honda_bosch_long = false; bool honda_bosch_radarless = false; -enum {HONDA_NIDEC, HONDA_BOSCH} honda_hw = HONDA_NIDEC; +typedef enum {HONDA_NIDEC, HONDA_BOSCH} HondaHw; +HondaHw honda_hw = HONDA_NIDEC; int honda_get_pt_bus(void) { @@ -127,26 +110,15 @@ static uint32_t honda_compute_checksum(const CANPacket_t *to_push) { } static uint8_t honda_get_counter(const CANPacket_t *to_push) { - int addr = GET_ADDR(to_push); - - uint8_t cnt = 0U; - if (addr == 0x201) { - // Signal: COUNTER_PEDAL - cnt = GET_BYTE(to_push, 4) & 0x0FU; - } else { - int counter_byte = GET_LEN(to_push) - 1U; - cnt = (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U; - } - return cnt; + int counter_byte = GET_LEN(to_push) - 1U; + return (GET_BYTE(to_push, counter_byte) >> 4U) & 0x3U; } static void honda_rx_hook(const CANPacket_t *to_push) { - const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || \ - ((honda_hw == HONDA_NIDEC) && !enable_gas_interceptor); + const bool pcm_cruise = ((honda_hw == HONDA_BOSCH) && !honda_bosch_long) || (honda_hw == HONDA_NIDEC); int pt_bus = honda_get_pt_bus(); int addr = GET_ADDR(to_push); - int len = GET_LEN(to_push); int bus = GET_BUS(to_push); // sample speed @@ -218,17 +190,8 @@ static void honda_rx_hook(const CANPacket_t *to_push) { } } - // length check because bosch hardware also uses this id (0x201 w/ len = 8) - if ((addr == 0x201) && (len == 6) && enable_gas_interceptor) { - int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push); - gas_pressed = gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD; - gas_interceptor_prev = gas_interceptor; - } - - if (!enable_gas_interceptor) { - if (addr == 0x17C) { - gas_pressed = GET_BYTE(to_push, 0) != 0U; - } + if (addr == 0x17C) { + gas_pressed = GET_BYTE(to_push, 0) != 0U; } // disable stock Honda AEB in alternative experience @@ -346,13 +309,6 @@ static bool honda_tx_hook(const CANPacket_t *to_send) { } } - // GAS: safety check (interceptor) - if (addr == 0x200) { - if (longitudinal_interceptor_checks(to_send)) { - tx = false; - } - } - // FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW // ensuring that only the cancel button press is sent (VAL 2) when controls are off. // This avoids unintended engagements while still allowing resume spam @@ -380,24 +336,18 @@ static safety_config honda_nidec_init(uint16_t param) { honda_alt_brake_msg = false; honda_bosch_long = false; honda_bosch_radarless = false; - enable_gas_interceptor = GET_FLAG(param, HONDA_PARAM_GAS_INTERCEPTOR); safety_config ret; bool enable_nidec_alt = GET_FLAG(param, HONDA_PARAM_NIDEC_ALT); + if (enable_nidec_alt) { - enable_gas_interceptor ? SET_RX_CHECKS(honda_nidec_alt_interceptor_rx_checks, ret) : \ - SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret); + SET_RX_CHECKS(honda_nidec_alt_rx_checks, ret); } else { - enable_gas_interceptor ? SET_RX_CHECKS(honda_common_interceptor_rx_checks, ret) : \ - SET_RX_CHECKS(honda_common_rx_checks, ret); + SET_RX_CHECKS(honda_common_rx_checks, ret); } + SET_TX_MSGS(HONDA_N_TX_MSGS, ret); - if (enable_gas_interceptor) { - SET_TX_MSGS(HONDA_N_INTERCEPTOR_TX_MSGS, ret); - } else { - SET_TX_MSGS(HONDA_N_TX_MSGS, ret); - } return ret; } diff --git a/panda/board/safety/safety_mazda.h b/panda/board/safety/safety_mazda.h index 7c6d8be9c7..ea4ce92d00 100644 --- a/panda/board/safety/safety_mazda.h +++ b/panda/board/safety/safety_mazda.h @@ -70,11 +70,10 @@ static void mazda_rx_hook(const CANPacket_t *to_push) { static bool mazda_tx_hook(const CANPacket_t *to_send) { bool tx = true; - int addr = GET_ADDR(to_send); int bus = GET_BUS(to_send); - // Check if msg is sent on the main BUS if (bus == MAZDA_MAIN) { + int addr = GET_ADDR(to_send); // steer cmd checks if (addr == MAZDA_LKAS) { diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h index c445bc435e..581dc50b7f 100644 --- a/panda/board/safety/safety_subaru.h +++ b/panda/board/safety/safety_subaru.h @@ -28,7 +28,7 @@ const LongitudinalLimits SUBARU_LONG_LIMITS = { .max_brake = 600, // approx -3.5 m/s^2 .min_transmission_rpm = 0, - .max_transmission_rpm = 2400, + .max_transmission_rpm = 3600, }; #define MSG_SUBARU_Brake_Status 0x13c diff --git a/panda/board/safety/safety_tesla.h b/panda/board/safety/safety_tesla.h index 4fa0df84aa..652161ff2c 100644 --- a/panda/board/safety/safety_tesla.h +++ b/panda/board/safety/safety_tesla.h @@ -19,6 +19,7 @@ const LongitudinalLimits TESLA_LONG_LIMITS = { const int TESLA_FLAG_POWERTRAIN = 1; const int TESLA_FLAG_LONGITUDINAL_CONTROL = 2; +const int TESLA_FLAG_RAVEN = 4; const CanMsg TESLA_TX_MSGS[] = { {0x488, 0, 4}, // DAS_steeringControl @@ -41,6 +42,16 @@ RxCheck tesla_rx_checks[] = { {.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // GTW_carState }; +RxCheck tesla_raven_rx_checks[] = { + {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control + {.msg = {{0x131, 2, 8, .frequency = 100U}, { 0 }, { 0 }}}, // EPAS3P_sysStatus + {.msg = {{0x108, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque1 + {.msg = {{0x118, 0, 6, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque2 + {.msg = {{0x20a, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // BrakeMessage + {.msg = {{0x368, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state + {.msg = {{0x318, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // GTW_carState +}; + RxCheck tesla_pt_rx_checks[] = { {.msg = {{0x106, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque1 {.msg = {{0x116, 0, 6, .frequency = 100U}, { 0 }, { 0 }}}, // DI_torque2 @@ -51,6 +62,7 @@ RxCheck tesla_pt_rx_checks[] = { bool tesla_longitudinal = false; bool tesla_powertrain = false; // Are we the second panda intercepting the powertrain bus? +bool tesla_raven = false; bool tesla_stock_aeb = false; @@ -58,16 +70,16 @@ static void tesla_rx_hook(const CANPacket_t *to_push) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); - if(bus == 0) { - if (!tesla_powertrain) { - if(addr == 0x370) { - // Steering angle: (0.1 * val) - 819.2 in deg. - // Store it 1/10 deg to match steering request - int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U; - update_sample(&angle_meas, angle_meas_new); - } + if (!tesla_powertrain) { + if((!tesla_raven && (addr == 0x370) && (bus == 0)) || (tesla_raven && (addr == 0x131) && (bus == 2))) { + // Steering angle: (0.1 * val) - 819.2 in deg. + // Store it 1/10 deg to match steering request + int angle_meas_new = (((GET_BYTE(to_push, 4) & 0x3FU) << 8) | GET_BYTE(to_push, 5)) - 8192U; + update_sample(&angle_meas, angle_meas_new); } + } + if(bus == 0) { if(addr == (tesla_powertrain ? 0x116 : 0x118)) { // Vehicle speed: ((0.05 * val) - 25) * MPH_TO_MPS float speed = (((((GET_BYTE(to_push, 3) & 0x0FU) << 8) | (GET_BYTE(to_push, 2))) * 0.05) - 25) * 0.447; @@ -206,12 +218,15 @@ static int tesla_fwd_hook(int bus_num, int addr) { static safety_config tesla_init(uint16_t param) { tesla_powertrain = GET_FLAG(param, TESLA_FLAG_POWERTRAIN); tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL); + tesla_raven = GET_FLAG(param, TESLA_FLAG_RAVEN); tesla_stock_aeb = false; safety_config ret; if (tesla_powertrain) { ret = BUILD_SAFETY_CFG(tesla_pt_rx_checks, TESLA_PT_TX_MSGS); + } else if (tesla_raven) { + ret = BUILD_SAFETY_CFG(tesla_raven_rx_checks, TESLA_TX_MSGS); } else { ret = BUILD_SAFETY_CFG(tesla_rx_checks, TESLA_TX_MSGS); } diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 50c00b38a8..05f84e4604 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -37,12 +37,6 @@ const LongitudinalLimits TOYOTA_LONG_LIMITS = { .min_accel = -3500, // -3.5 m/s2 }; -// panda interceptor threshold needs to be equivalent to openpilot threshold to avoid controls mismatches -// If thresholds are mismatched then it is possible for panda to see the gas fall and rise while openpilot is in the pre-enabled state -// Threshold calculated from DBC gains: round((((15 + 75.555) / 0.159375) + ((15 + 151.111) / 0.159375)) / 2) = 805 -const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805; -#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2U) // avg between 2 tracks - // Stock longitudinal #define TOYOTA_COMMON_TX_MSGS \ {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8}, /* LKAS + LTA + ACC & PCM cancel cmds */ \ @@ -62,11 +56,6 @@ const CanMsg TOYOTA_LONG_TX_MSGS[] = { TOYOTA_COMMON_LONG_TX_MSGS }; -const CanMsg TOYOTA_INTERCEPTOR_TX_MSGS[] = { - TOYOTA_COMMON_LONG_TX_MSGS - {0x200, 0, 6}, // gas interceptor -}; - #define TOYOTA_COMMON_RX_CHECKS(lta) \ {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \ {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \ @@ -78,21 +67,11 @@ RxCheck toyota_lka_rx_checks[] = { TOYOTA_COMMON_RX_CHECKS(false) }; -RxCheck toyota_lka_interceptor_rx_checks[] = { - TOYOTA_COMMON_RX_CHECKS(false) - {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, -}; - // Check the quality flag for angle measurement when using LTA, since it's not set on TSS-P cars RxCheck toyota_lta_rx_checks[] = { TOYOTA_COMMON_RX_CHECKS(true) }; -RxCheck toyota_lta_interceptor_rx_checks[] = { - TOYOTA_COMMON_RX_CHECKS(true) - {.msg = {{0x201, 0, 6, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, -}; - // safety param flags // first byte is for EPS factor, second is for flags const uint32_t TOYOTA_PARAM_OFFSET = 8U; @@ -100,7 +79,6 @@ const uint32_t TOYOTA_EPS_FACTOR = (1UL << TOYOTA_PARAM_OFFSET) - 1U; const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET; -const uint32_t TOYOTA_PARAM_GAS_INTERCEPTOR = 8UL << TOYOTA_PARAM_OFFSET; bool toyota_alt_brake = false; bool toyota_stock_longitudinal = false; @@ -122,17 +100,6 @@ static uint32_t toyota_get_checksum(const CANPacket_t *to_push) { return (uint8_t)(GET_BYTE(to_push, checksum_byte)); } -static uint8_t toyota_get_counter(const CANPacket_t *to_push) { - int addr = GET_ADDR(to_push); - - uint8_t cnt = 0U; - if (addr == 0x201) { - // Signal: COUNTER_PEDAL - cnt = GET_BYTE(to_push, 4) & 0x0FU; - } - return cnt; -} - static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) { int addr = GET_ADDR(to_push); @@ -185,9 +152,7 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { pcm_cruise_check(cruise_engaged); // sample gas pedal - if (!enable_gas_interceptor) { - gas_pressed = !GET_BIT(to_push, 4U); - } + gas_pressed = !GET_BIT(to_push, 4U); } // sample speed @@ -210,15 +175,6 @@ static void toyota_rx_hook(const CANPacket_t *to_push) { brake_pressed = GET_BIT(to_push, bit); } - // sample gas interceptor - if ((addr == 0x201) && enable_gas_interceptor) { - int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push); - gas_pressed = gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD; - - // TODO: remove this, only left in for gas_interceptor_prev test - gas_interceptor_prev = gas_interceptor; - } - bool stock_ecu_detected = addr == 0x2E4; // STEERING_LKA if (!toyota_stock_longitudinal && (addr == 0x343)) { stock_ecu_detected = true; // ACC_CONTROL @@ -234,14 +190,6 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) { // Check if msg is sent on BUS 0 if (bus == 0) { - - // GAS PEDAL: safety check - if (addr == 0x200) { - if (longitudinal_interceptor_checks(to_send)) { - tx = false; - } - } - // ACCEL: safety check on byte 1-2 if (addr == 0x343) { int desired_accel = (GET_BYTE(to_send, 0) << 8) | GET_BYTE(to_send, 1); @@ -357,29 +305,17 @@ static safety_config toyota_init(uint16_t param) { toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); - enable_gas_interceptor = GET_FLAG(param, TOYOTA_PARAM_GAS_INTERCEPTOR); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; - // Gas interceptor should not be used if openpilot is not controlling longitudinal - if (toyota_stock_longitudinal) { - enable_gas_interceptor = false; - } - safety_config ret; if (toyota_stock_longitudinal) { SET_TX_MSGS(TOYOTA_TX_MSGS, ret); } else { - enable_gas_interceptor ? SET_TX_MSGS(TOYOTA_INTERCEPTOR_TX_MSGS, ret) : \ - SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); + SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); } - if (enable_gas_interceptor) { - toyota_lta ? SET_RX_CHECKS(toyota_lta_interceptor_rx_checks, ret) : \ - SET_RX_CHECKS(toyota_lka_interceptor_rx_checks, ret); - } else { - toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \ - SET_RX_CHECKS(toyota_lka_rx_checks, ret); - } + toyota_lta ? SET_RX_CHECKS(toyota_lta_rx_checks, ret) : \ + SET_RX_CHECKS(toyota_lka_rx_checks, ret); return ret; } @@ -413,6 +349,5 @@ const safety_hooks toyota_hooks = { .fwd = toyota_fwd_hook, .get_checksum = toyota_get_checksum, .compute_checksum = toyota_compute_checksum, - .get_counter = toyota_get_counter, .get_quality_flag_valid = toyota_get_quality_flag_valid, }; diff --git a/panda/board/safety/safety_volkswagen_pq.h b/panda/board/safety/safety_volkswagen_pq.h index 1922cf4671..de147cb58a 100644 --- a/panda/board/safety/safety_volkswagen_pq.h +++ b/panda/board/safety/safety_volkswagen_pq.h @@ -185,7 +185,7 @@ static bool volkswagen_pq_tx_hook(const CANPacket_t *to_send) { } uint32_t hca_status = ((GET_BYTE(to_send, 1) >> 4) & 0xFU); - bool steer_req = (hca_status == 5U); + bool steer_req = ((hca_status == 5U) || (hca_status == 7U)); if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_PQ_STEERING_LIMITS)) { tx = false; diff --git a/panda/board/safety_declarations.h b/panda/board/safety_declarations.h index 64b55f2033..e3cdc47cde 100644 --- a/panda/board/safety_declarations.h +++ b/panda/board/safety_declarations.h @@ -2,7 +2,7 @@ #define GET_BIT(msg, b) ((bool)!!(((msg)->data[((b) / 8U)] >> ((b) % 8U)) & 0x1U)) #define GET_BYTE(msg, b) ((msg)->data[(b)]) -#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask)) +#define GET_FLAG(value, mask) (((__typeof__(mask))(value) & (mask)) == (mask)) // cppcheck-suppress misra-c2012-1.2; allow __typeof__ #define BUILD_SAFETY_CFG(rx, tx) ((safety_config){(rx), (sizeof((rx)) / sizeof((rx)[0])), \ (tx), (sizeof((tx)) / sizeof((tx)[0]))}) @@ -203,7 +203,6 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits); bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits); bool longitudinal_brake_checks(int desired_brake, const LongitudinalLimits limits); -bool longitudinal_interceptor_checks(const CANPacket_t *to_send); void pcm_cruise_check(bool cruise_engaged); void safety_tick(const safety_config *safety_config); @@ -211,8 +210,6 @@ void safety_tick(const safety_config *safety_config); // This can be set by the safety hooks bool controls_allowed = false; bool relay_malfunction = false; -bool enable_gas_interceptor = false; -int gas_interceptor_prev = 0; bool gas_pressed = false; bool gas_pressed_prev = false; bool brake_pressed = false; diff --git a/panda/board/stm32fx/board.h b/panda/board/stm32f4/board.h similarity index 93% rename from panda/board/stm32fx/board.h rename to panda/board/stm32f4/board.h index 808f773829..bf95d58eaa 100644 --- a/panda/board/stm32fx/board.h +++ b/panda/board/stm32f4/board.h @@ -5,12 +5,10 @@ #include "boards/unused_funcs.h" // ///// Board definition and detection ///// // -#include "stm32fx/lladc.h" +#include "stm32f4/lladc.h" #include "drivers/harness.h" #include "drivers/fan.h" -#include "stm32fx/llfan.h" -#include "stm32fx/llrtc.h" -#include "drivers/rtc.h" +#include "stm32f4/llfan.h" #include "drivers/clock_source.h" #include "boards/white.h" #include "boards/grey.h" diff --git a/panda/board/stm32fx/clock.h b/panda/board/stm32f4/clock.h similarity index 91% rename from panda/board/stm32fx/clock.h rename to panda/board/stm32f4/clock.h index 19be574438..f0084faccb 100644 --- a/panda/board/stm32fx/clock.h +++ b/panda/board/stm32f4/clock.h @@ -1,7 +1,7 @@ void clock_init(void) { // enable external oscillator register_set_bits(&(RCC->CR), RCC_CR_HSEON); - while ((RCC->CR & RCC_CR_HSERDY) == 0); + while ((RCC->CR & RCC_CR_HSERDY) == 0U); // divide things // AHB = 96MHz @@ -20,7 +20,7 @@ void clock_init(void) { // start PLL register_set_bits(&(RCC->CR), RCC_CR_PLLON); - while ((RCC->CR & RCC_CR_PLLRDY) == 0); + while ((RCC->CR & RCC_CR_PLLRDY) == 0U); // Configure Flash prefetch, Instruction cache, Data cache and wait state // *** without this, it breaks *** diff --git a/panda/board/stm32fx/inc/cmsis_compiler.h b/panda/board/stm32f4/inc/cmsis_compiler.h similarity index 100% rename from panda/board/stm32fx/inc/cmsis_compiler.h rename to panda/board/stm32f4/inc/cmsis_compiler.h diff --git a/panda/board/stm32fx/inc/cmsis_gcc.h b/panda/board/stm32f4/inc/cmsis_gcc.h similarity index 100% rename from panda/board/stm32fx/inc/cmsis_gcc.h rename to panda/board/stm32f4/inc/cmsis_gcc.h diff --git a/panda/board/stm32fx/inc/cmsis_version.h b/panda/board/stm32f4/inc/cmsis_version.h similarity index 100% rename from panda/board/stm32fx/inc/cmsis_version.h rename to panda/board/stm32f4/inc/cmsis_version.h diff --git a/panda/board/stm32fx/inc/core_cm4.h b/panda/board/stm32f4/inc/core_cm4.h similarity index 100% rename from panda/board/stm32fx/inc/core_cm4.h rename to panda/board/stm32f4/inc/core_cm4.h diff --git a/panda/board/stm32fx/inc/mpu_armv7.h b/panda/board/stm32f4/inc/mpu_armv7.h similarity index 100% rename from panda/board/stm32fx/inc/mpu_armv7.h rename to panda/board/stm32f4/inc/mpu_armv7.h diff --git a/panda/board/stm32fx/inc/stm32f413xx.h b/panda/board/stm32f4/inc/stm32f413xx.h similarity index 100% rename from panda/board/stm32fx/inc/stm32f413xx.h rename to panda/board/stm32f4/inc/stm32f413xx.h diff --git a/panda/board/stm32fx/inc/stm32f4xx.h b/panda/board/stm32f4/inc/stm32f4xx.h similarity index 100% rename from panda/board/stm32fx/inc/stm32f4xx.h rename to panda/board/stm32f4/inc/stm32f4xx.h diff --git a/panda/board/stm32fx/inc/stm32f4xx_hal_def.h b/panda/board/stm32f4/inc/stm32f4xx_hal_def.h similarity index 100% rename from panda/board/stm32fx/inc/stm32f4xx_hal_def.h rename to panda/board/stm32f4/inc/stm32f4xx_hal_def.h diff --git a/panda/board/stm32fx/inc/stm32f4xx_hal_gpio_ex.h b/panda/board/stm32f4/inc/stm32f4xx_hal_gpio_ex.h similarity index 100% rename from panda/board/stm32fx/inc/stm32f4xx_hal_gpio_ex.h rename to panda/board/stm32f4/inc/stm32f4xx_hal_gpio_ex.h diff --git a/panda/board/stm32fx/inc/system_stm32f4xx.h b/panda/board/stm32f4/inc/system_stm32f4xx.h similarity index 100% rename from panda/board/stm32fx/inc/system_stm32f4xx.h rename to panda/board/stm32f4/inc/system_stm32f4xx.h diff --git a/panda/board/stm32fx/interrupt_handlers.h b/panda/board/stm32f4/interrupt_handlers.h similarity index 100% rename from panda/board/stm32fx/interrupt_handlers.h rename to panda/board/stm32f4/interrupt_handlers.h diff --git a/panda/board/stm32fx/lladc.h b/panda/board/stm32f4/lladc.h similarity index 100% rename from panda/board/stm32fx/lladc.h rename to panda/board/stm32f4/lladc.h diff --git a/panda/board/stm32fx/llbxcan.h b/panda/board/stm32f4/llbxcan.h similarity index 100% rename from panda/board/stm32fx/llbxcan.h rename to panda/board/stm32f4/llbxcan.h diff --git a/panda/board/stm32fx/llfan.h b/panda/board/stm32f4/llfan.h similarity index 100% rename from panda/board/stm32fx/llfan.h rename to panda/board/stm32f4/llfan.h diff --git a/panda/board/stm32fx/llflash.h b/panda/board/stm32f4/llflash.h similarity index 100% rename from panda/board/stm32fx/llflash.h rename to panda/board/stm32f4/llflash.h diff --git a/panda/board/stm32fx/llspi.h b/panda/board/stm32f4/llspi.h similarity index 100% rename from panda/board/stm32fx/llspi.h rename to panda/board/stm32f4/llspi.h diff --git a/panda/board/stm32fx/lluart.h b/panda/board/stm32f4/lluart.h similarity index 96% rename from panda/board/stm32fx/lluart.h rename to panda/board/stm32f4/lluart.h index ffe7e74da0..64094119f4 100644 --- a/panda/board/stm32fx/lluart.h +++ b/panda/board/stm32f4/lluart.h @@ -5,7 +5,7 @@ void uart_tx_ring(uart_ring *q){ // Send out next byte of TX buffer if (q->w_ptr_tx != q->r_ptr_tx) { // Only send if transmit register is empty (aka last byte has been sent) - if ((q->uart->SR & USART_SR_TXE) != 0) { + if ((q->uart->SR & USART_SR_TXE) != 0U) { q->uart->DR = q->elems_tx[q->r_ptr_tx]; // This clears TXE q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; } @@ -46,7 +46,7 @@ void uart_rx_ring(uart_ring *q){ } void uart_send_break(uart_ring *u) { - while ((u->uart->CR1 & USART_CR1_SBK) != 0); + while ((u->uart->CR1 & USART_CR1_SBK) != 0U); u->uart->CR1 |= USART_CR1_SBK; } diff --git a/panda/board/stm32fx/llusb.h b/panda/board/stm32f4/llusb.h similarity index 97% rename from panda/board/stm32fx/llusb.h rename to panda/board/stm32f4/llusb.h index 20c980864b..6f15c89e11 100644 --- a/panda/board/stm32fx/llusb.h +++ b/panda/board/stm32f4/llusb.h @@ -8,7 +8,7 @@ USB_OTG_GlobalTypeDef *USBx = USB_OTG_FS; #define USBx_PCGCCTL *(__IO uint32_t *)((uint32_t)USBx + USB_OTG_PCGCCTL_BASE) #define USBD_FS_TRDT_VALUE 5UL -#define USB_OTG_SPEED_FULL 3 +#define USB_OTG_SPEED_FULL 3UL void usb_irqhandler(void); @@ -27,7 +27,7 @@ void usb_init(void) { // full speed PHY, do reset and remove power down /*puth(USBx->GRSTCTL); print(" resetting PHY\n");*/ - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U); //print("AHB idle\n"); // reset PHY here diff --git a/panda/board/stm32fx/peripherals.h b/panda/board/stm32f4/peripherals.h similarity index 98% rename from panda/board/stm32fx/peripherals.h rename to panda/board/stm32f4/peripherals.h index 79ac3c6e4c..711f1b82a0 100644 --- a/panda/board/stm32fx/peripherals.h +++ b/panda/board/stm32f4/peripherals.h @@ -83,7 +83,6 @@ void peripherals_init(void) { RCC->APB1ENR |= RCC_APB1ENR_TIM5EN; // k-line init RCC->APB1ENR |= RCC_APB1ENR_TIM6EN; // interrupt timer RCC->APB2ENR |= RCC_APB2ENR_TIM9EN; // slow loop - RCC->APB1ENR |= RCC_APB1ENR_TIM12EN; // gmlan_alt } void enable_interrupt_timer(void) { diff --git a/panda/board/stm32fx/startup_stm32f413xx.s b/panda/board/stm32f4/startup_stm32f413xx.s similarity index 100% rename from panda/board/stm32fx/startup_stm32f413xx.s rename to panda/board/stm32f4/startup_stm32f413xx.s diff --git a/panda/board/stm32fx/stm32fx_config.h b/panda/board/stm32f4/stm32f4_config.h similarity index 79% rename from panda/board/stm32fx/stm32fx_config.h rename to panda/board/stm32f4/stm32f4_config.h index 8b59da28f0..85baff25b5 100644 --- a/panda/board/stm32fx/stm32fx_config.h +++ b/panda/board/stm32f4/stm32f4_config.h @@ -1,5 +1,5 @@ -#include "stm32fx/inc/stm32f4xx.h" -#include "stm32fx/inc/stm32f4xx_hal_gpio_ex.h" +#include "stm32f4/inc/stm32f4xx.h" +#include "stm32f4/inc/stm32f4xx_hal_gpio_ex.h" #define MCU_IDCODE 0x463U // from the linker script @@ -50,32 +50,28 @@ #include "drivers/registers.h" #include "drivers/interrupts.h" #include "drivers/gpio.h" -#include "stm32fx/peripherals.h" -#include "stm32fx/interrupt_handlers.h" +#include "stm32f4/peripherals.h" +#include "stm32f4/interrupt_handlers.h" #include "drivers/timers.h" -#include "stm32fx/board.h" -#include "stm32fx/clock.h" +#include "stm32f4/board.h" +#include "stm32f4/clock.h" #include "drivers/watchdog.h" #include "drivers/spi.h" -#include "stm32fx/llspi.h" +#include "stm32f4/llspi.h" #if !defined(BOOTSTUB) #include "drivers/uart.h" - #include "stm32fx/lluart.h" -#endif - -#if defined(PANDA) && !defined(BOOTSTUB) - #include "stm32fx/llexti.h" + #include "stm32f4/lluart.h" #endif #ifdef BOOTSTUB - #include "stm32fx/llflash.h" + #include "stm32f4/llflash.h" #else - #include "stm32fx/llbxcan.h" + #include "stm32f4/llbxcan.h" #endif -#include "stm32fx/llusb.h" +#include "stm32f4/llusb.h" void early_gpio_float(void) { RCC->AHB1ENR = RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN | RCC_AHB1ENR_GPIOCEN; diff --git a/panda/board/stm32fx/stm32f4_flash.ld b/panda/board/stm32f4/stm32f4_flash.ld similarity index 100% rename from panda/board/stm32fx/stm32f4_flash.ld rename to panda/board/stm32f4/stm32f4_flash.ld diff --git a/panda/board/stm32fx/inc/core_cm3.h b/panda/board/stm32fx/inc/core_cm3.h deleted file mode 100644 index 0918c5eb4c..0000000000 --- a/panda/board/stm32fx/inc/core_cm3.h +++ /dev/null @@ -1,1938 +0,0 @@ -/**************************************************************************//** - * @file core_cm3.h - * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File - * @version V5.1.0 - * @date 13. March 2019 - ******************************************************************************/ -/* - * Copyright (c) 2009-2019 Arm Limited. All rights reserved. - * - * SPDX-License-Identifier: Apache-2.0 - * - * Licensed under the Apache License, Version 2.0 (the License); you may - * not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an AS IS BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if defined ( __ICCARM__ ) - #pragma system_include /* treat file as system include file for MISRA check */ -#elif defined (__clang__) - #pragma clang system_header /* treat file as system include file */ -#endif - -#ifndef __CORE_CM3_H_GENERIC -#define __CORE_CM3_H_GENERIC - -#include - -#ifdef __cplusplus - extern "C" { -#endif - -/** - \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions - CMSIS violates the following MISRA-C:2004 rules: - - \li Required Rule 8.5, object/function definition in header file.
    - Function definitions in header files are used to allow 'inlining'. - - \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.
    - Unions are used for effective representation of core registers. - - \li Advisory Rule 19.7, Function-like macro defined.
    - Function-like macros are used to allow more efficient code. - */ - - -/******************************************************************************* - * CMSIS definitions - ******************************************************************************/ -/** - \ingroup Cortex_M3 - @{ - */ - -#include "cmsis_version.h" - -/* CMSIS CM3 definitions */ -#define __CM3_CMSIS_VERSION_MAIN (__CM_CMSIS_VERSION_MAIN) /*!< \deprecated [31:16] CMSIS HAL main version */ -#define __CM3_CMSIS_VERSION_SUB (__CM_CMSIS_VERSION_SUB) /*!< \deprecated [15:0] CMSIS HAL sub version */ -#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16U) | \ - __CM3_CMSIS_VERSION_SUB ) /*!< \deprecated CMSIS HAL version number */ - -#define __CORTEX_M (3U) /*!< Cortex-M Core */ - -/** __FPU_USED indicates whether an FPU is used or not. - This core does not support an FPU at all -*/ -#define __FPU_USED 0U - -#if defined ( __CC_ARM ) - #if defined __TARGET_FPU_VFP - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined (__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) - #if defined __ARM_FP - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __GNUC__ ) - #if defined (__VFP_FP__) && !defined(__SOFTFP__) - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __ICCARM__ ) - #if defined __ARMVFP__ - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __TI_ARM__ ) - #if defined __TI_VFP_SUPPORT__ - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __TASKING__ ) - #if defined __FPU_VFP__ - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#elif defined ( __CSMC__ ) - #if ( __CSMC__ & 0x400U) - #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)" - #endif - -#endif - -#include "cmsis_compiler.h" /* CMSIS compiler specific defines */ - - -#ifdef __cplusplus -} -#endif - -#endif /* __CORE_CM3_H_GENERIC */ - -#ifndef __CMSIS_GENERIC - -#ifndef __CORE_CM3_H_DEPENDANT -#define __CORE_CM3_H_DEPENDANT - -#ifdef __cplusplus - extern "C" { -#endif - -/* check device defines and use defaults */ -#if defined __CHECK_DEVICE_DEFINES - #ifndef __CM3_REV - #define __CM3_REV 0x0200U - #warning "__CM3_REV not defined in device header file; using default!" - #endif - - #ifndef __MPU_PRESENT - #define __MPU_PRESENT 0U - #warning "__MPU_PRESENT not defined in device header file; using default!" - #endif - - #ifndef __NVIC_PRIO_BITS - #define __NVIC_PRIO_BITS 3U - #warning "__NVIC_PRIO_BITS not defined in device header file; using default!" - #endif - - #ifndef __Vendor_SysTickConfig - #define __Vendor_SysTickConfig 0U - #warning "__Vendor_SysTickConfig not defined in device header file; using default!" - #endif -#endif - -/* IO definitions (access restrictions to peripheral registers) */ -/** - \defgroup CMSIS_glob_defs CMSIS Global Defines - - IO Type Qualifiers are used - \li to specify the access to peripheral variables. - \li for automatic generation of peripheral register debug information. -*/ -#ifdef __cplusplus - #define __I volatile /*!< Defines 'read only' permissions */ -#else - #define __I volatile const /*!< Defines 'read only' permissions */ -#endif -#define __O volatile /*!< Defines 'write only' permissions */ -#define __IO volatile /*!< Defines 'read / write' permissions */ - -/* following defines should be used for structure members */ -#define __IM volatile const /*! Defines 'read only' structure member permissions */ -#define __OM volatile /*! Defines 'write only' structure member permissions */ -#define __IOM volatile /*! Defines 'read / write' structure member permissions */ - -/*@} end of group Cortex_M3 */ - - - -/******************************************************************************* - * Register Abstraction - Core Register contain: - - Core Register - - Core NVIC Register - - Core SCB Register - - Core SysTick Register - - Core Debug Register - - Core MPU Register - ******************************************************************************/ -/** - \defgroup CMSIS_core_register Defines and Type Definitions - \brief Type definitions and defines for Cortex-M processor based devices. -*/ - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_CORE Status and Control Registers - \brief Core Register type definitions. - @{ - */ - -/** - \brief Union type to access the Application Program Status Register (APSR). - */ -typedef union -{ - struct - { - uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */ - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} APSR_Type; - -/* APSR Register Definitions */ -#define APSR_N_Pos 31U /*!< APSR: N Position */ -#define APSR_N_Msk (1UL << APSR_N_Pos) /*!< APSR: N Mask */ - -#define APSR_Z_Pos 30U /*!< APSR: Z Position */ -#define APSR_Z_Msk (1UL << APSR_Z_Pos) /*!< APSR: Z Mask */ - -#define APSR_C_Pos 29U /*!< APSR: C Position */ -#define APSR_C_Msk (1UL << APSR_C_Pos) /*!< APSR: C Mask */ - -#define APSR_V_Pos 28U /*!< APSR: V Position */ -#define APSR_V_Msk (1UL << APSR_V_Pos) /*!< APSR: V Mask */ - -#define APSR_Q_Pos 27U /*!< APSR: Q Position */ -#define APSR_Q_Msk (1UL << APSR_Q_Pos) /*!< APSR: Q Mask */ - - -/** - \brief Union type to access the Interrupt Program Status Register (IPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ - uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} IPSR_Type; - -/* IPSR Register Definitions */ -#define IPSR_ISR_Pos 0U /*!< IPSR: ISR Position */ -#define IPSR_ISR_Msk (0x1FFUL /*<< IPSR_ISR_Pos*/) /*!< IPSR: ISR Mask */ - - -/** - \brief Union type to access the Special-Purpose Program Status Registers (xPSR). - */ -typedef union -{ - struct - { - uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */ - uint32_t _reserved0:1; /*!< bit: 9 Reserved */ - uint32_t ICI_IT_1:6; /*!< bit: 10..15 ICI/IT part 1 */ - uint32_t _reserved1:8; /*!< bit: 16..23 Reserved */ - uint32_t T:1; /*!< bit: 24 Thumb bit */ - uint32_t ICI_IT_2:2; /*!< bit: 25..26 ICI/IT part 2 */ - uint32_t Q:1; /*!< bit: 27 Saturation condition flag */ - uint32_t V:1; /*!< bit: 28 Overflow condition code flag */ - uint32_t C:1; /*!< bit: 29 Carry condition code flag */ - uint32_t Z:1; /*!< bit: 30 Zero condition code flag */ - uint32_t N:1; /*!< bit: 31 Negative condition code flag */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} xPSR_Type; - -/* xPSR Register Definitions */ -#define xPSR_N_Pos 31U /*!< xPSR: N Position */ -#define xPSR_N_Msk (1UL << xPSR_N_Pos) /*!< xPSR: N Mask */ - -#define xPSR_Z_Pos 30U /*!< xPSR: Z Position */ -#define xPSR_Z_Msk (1UL << xPSR_Z_Pos) /*!< xPSR: Z Mask */ - -#define xPSR_C_Pos 29U /*!< xPSR: C Position */ -#define xPSR_C_Msk (1UL << xPSR_C_Pos) /*!< xPSR: C Mask */ - -#define xPSR_V_Pos 28U /*!< xPSR: V Position */ -#define xPSR_V_Msk (1UL << xPSR_V_Pos) /*!< xPSR: V Mask */ - -#define xPSR_Q_Pos 27U /*!< xPSR: Q Position */ -#define xPSR_Q_Msk (1UL << xPSR_Q_Pos) /*!< xPSR: Q Mask */ - -#define xPSR_ICI_IT_2_Pos 25U /*!< xPSR: ICI/IT part 2 Position */ -#define xPSR_ICI_IT_2_Msk (3UL << xPSR_ICI_IT_2_Pos) /*!< xPSR: ICI/IT part 2 Mask */ - -#define xPSR_T_Pos 24U /*!< xPSR: T Position */ -#define xPSR_T_Msk (1UL << xPSR_T_Pos) /*!< xPSR: T Mask */ - -#define xPSR_ICI_IT_1_Pos 10U /*!< xPSR: ICI/IT part 1 Position */ -#define xPSR_ICI_IT_1_Msk (0x3FUL << xPSR_ICI_IT_1_Pos) /*!< xPSR: ICI/IT part 1 Mask */ - -#define xPSR_ISR_Pos 0U /*!< xPSR: ISR Position */ -#define xPSR_ISR_Msk (0x1FFUL /*<< xPSR_ISR_Pos*/) /*!< xPSR: ISR Mask */ - - -/** - \brief Union type to access the Control Registers (CONTROL). - */ -typedef union -{ - struct - { - uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */ - uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */ - uint32_t _reserved1:30; /*!< bit: 2..31 Reserved */ - } b; /*!< Structure used for bit access */ - uint32_t w; /*!< Type used for word access */ -} CONTROL_Type; - -/* CONTROL Register Definitions */ -#define CONTROL_SPSEL_Pos 1U /*!< CONTROL: SPSEL Position */ -#define CONTROL_SPSEL_Msk (1UL << CONTROL_SPSEL_Pos) /*!< CONTROL: SPSEL Mask */ - -#define CONTROL_nPRIV_Pos 0U /*!< CONTROL: nPRIV Position */ -#define CONTROL_nPRIV_Msk (1UL /*<< CONTROL_nPRIV_Pos*/) /*!< CONTROL: nPRIV Mask */ - -/*@} end of group CMSIS_CORE */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC) - \brief Type definitions for the NVIC Registers - @{ - */ - -/** - \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC). - */ -typedef struct -{ - __IOM uint32_t ISER[8U]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */ - uint32_t RESERVED0[24U]; - __IOM uint32_t ICER[8U]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */ - uint32_t RESERVED1[24U]; - __IOM uint32_t ISPR[8U]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */ - uint32_t RESERVED2[24U]; - __IOM uint32_t ICPR[8U]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */ - uint32_t RESERVED3[24U]; - __IOM uint32_t IABR[8U]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */ - uint32_t RESERVED4[56U]; - __IOM uint8_t IP[240U]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */ - uint32_t RESERVED5[644U]; - __OM uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */ -} NVIC_Type; - -/* Software Triggered Interrupt Register Definitions */ -#define NVIC_STIR_INTID_Pos 0U /*!< STIR: INTLINESNUM Position */ -#define NVIC_STIR_INTID_Msk (0x1FFUL /*<< NVIC_STIR_INTID_Pos*/) /*!< STIR: INTLINESNUM Mask */ - -/*@} end of group CMSIS_NVIC */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_SCB System Control Block (SCB) - \brief Type definitions for the System Control Block Registers - @{ - */ - -/** - \brief Structure type to access the System Control Block (SCB). - */ -typedef struct -{ - __IM uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */ - __IOM uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */ - __IOM uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */ - __IOM uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */ - __IOM uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */ - __IOM uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */ - __IOM uint8_t SHP[12U]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */ - __IOM uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */ - __IOM uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */ - __IOM uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */ - __IOM uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */ - __IOM uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */ - __IOM uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */ - __IOM uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */ - __IM uint32_t PFR[2U]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */ - __IM uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */ - __IM uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */ - __IM uint32_t MMFR[4U]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */ - __IM uint32_t ISAR[5U]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */ - uint32_t RESERVED0[5U]; - __IOM uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */ -} SCB_Type; - -/* SCB CPUID Register Definitions */ -#define SCB_CPUID_IMPLEMENTER_Pos 24U /*!< SCB CPUID: IMPLEMENTER Position */ -#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */ - -#define SCB_CPUID_VARIANT_Pos 20U /*!< SCB CPUID: VARIANT Position */ -#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */ - -#define SCB_CPUID_ARCHITECTURE_Pos 16U /*!< SCB CPUID: ARCHITECTURE Position */ -#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */ - -#define SCB_CPUID_PARTNO_Pos 4U /*!< SCB CPUID: PARTNO Position */ -#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */ - -#define SCB_CPUID_REVISION_Pos 0U /*!< SCB CPUID: REVISION Position */ -#define SCB_CPUID_REVISION_Msk (0xFUL /*<< SCB_CPUID_REVISION_Pos*/) /*!< SCB CPUID: REVISION Mask */ - -/* SCB Interrupt Control State Register Definitions */ -#define SCB_ICSR_NMIPENDSET_Pos 31U /*!< SCB ICSR: NMIPENDSET Position */ -#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */ - -#define SCB_ICSR_PENDSVSET_Pos 28U /*!< SCB ICSR: PENDSVSET Position */ -#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */ - -#define SCB_ICSR_PENDSVCLR_Pos 27U /*!< SCB ICSR: PENDSVCLR Position */ -#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */ - -#define SCB_ICSR_PENDSTSET_Pos 26U /*!< SCB ICSR: PENDSTSET Position */ -#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */ - -#define SCB_ICSR_PENDSTCLR_Pos 25U /*!< SCB ICSR: PENDSTCLR Position */ -#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */ - -#define SCB_ICSR_ISRPREEMPT_Pos 23U /*!< SCB ICSR: ISRPREEMPT Position */ -#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */ - -#define SCB_ICSR_ISRPENDING_Pos 22U /*!< SCB ICSR: ISRPENDING Position */ -#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */ - -#define SCB_ICSR_VECTPENDING_Pos 12U /*!< SCB ICSR: VECTPENDING Position */ -#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */ - -#define SCB_ICSR_RETTOBASE_Pos 11U /*!< SCB ICSR: RETTOBASE Position */ -#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */ - -#define SCB_ICSR_VECTACTIVE_Pos 0U /*!< SCB ICSR: VECTACTIVE Position */ -#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL /*<< SCB_ICSR_VECTACTIVE_Pos*/) /*!< SCB ICSR: VECTACTIVE Mask */ - -/* SCB Vector Table Offset Register Definitions */ -#if defined (__CM3_REV) && (__CM3_REV < 0x0201U) /* core r2p1 */ -#define SCB_VTOR_TBLBASE_Pos 29U /*!< SCB VTOR: TBLBASE Position */ -#define SCB_VTOR_TBLBASE_Msk (1UL << SCB_VTOR_TBLBASE_Pos) /*!< SCB VTOR: TBLBASE Mask */ - -#define SCB_VTOR_TBLOFF_Pos 7U /*!< SCB VTOR: TBLOFF Position */ -#define SCB_VTOR_TBLOFF_Msk (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ -#else -#define SCB_VTOR_TBLOFF_Pos 7U /*!< SCB VTOR: TBLOFF Position */ -#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */ -#endif - -/* SCB Application Interrupt and Reset Control Register Definitions */ -#define SCB_AIRCR_VECTKEY_Pos 16U /*!< SCB AIRCR: VECTKEY Position */ -#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */ - -#define SCB_AIRCR_VECTKEYSTAT_Pos 16U /*!< SCB AIRCR: VECTKEYSTAT Position */ -#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */ - -#define SCB_AIRCR_ENDIANESS_Pos 15U /*!< SCB AIRCR: ENDIANESS Position */ -#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */ - -#define SCB_AIRCR_PRIGROUP_Pos 8U /*!< SCB AIRCR: PRIGROUP Position */ -#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ - -#define SCB_AIRCR_SYSRESETREQ_Pos 2U /*!< SCB AIRCR: SYSRESETREQ Position */ -#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */ - -#define SCB_AIRCR_VECTCLRACTIVE_Pos 1U /*!< SCB AIRCR: VECTCLRACTIVE Position */ -#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */ - -#define SCB_AIRCR_VECTRESET_Pos 0U /*!< SCB AIRCR: VECTRESET Position */ -#define SCB_AIRCR_VECTRESET_Msk (1UL /*<< SCB_AIRCR_VECTRESET_Pos*/) /*!< SCB AIRCR: VECTRESET Mask */ - -/* SCB System Control Register Definitions */ -#define SCB_SCR_SEVONPEND_Pos 4U /*!< SCB SCR: SEVONPEND Position */ -#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */ - -#define SCB_SCR_SLEEPDEEP_Pos 2U /*!< SCB SCR: SLEEPDEEP Position */ -#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */ - -#define SCB_SCR_SLEEPONEXIT_Pos 1U /*!< SCB SCR: SLEEPONEXIT Position */ -#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */ - -/* SCB Configuration Control Register Definitions */ -#define SCB_CCR_STKALIGN_Pos 9U /*!< SCB CCR: STKALIGN Position */ -#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */ - -#define SCB_CCR_BFHFNMIGN_Pos 8U /*!< SCB CCR: BFHFNMIGN Position */ -#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */ - -#define SCB_CCR_DIV_0_TRP_Pos 4U /*!< SCB CCR: DIV_0_TRP Position */ -#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */ - -#define SCB_CCR_UNALIGN_TRP_Pos 3U /*!< SCB CCR: UNALIGN_TRP Position */ -#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */ - -#define SCB_CCR_USERSETMPEND_Pos 1U /*!< SCB CCR: USERSETMPEND Position */ -#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */ - -#define SCB_CCR_NONBASETHRDENA_Pos 0U /*!< SCB CCR: NONBASETHRDENA Position */ -#define SCB_CCR_NONBASETHRDENA_Msk (1UL /*<< SCB_CCR_NONBASETHRDENA_Pos*/) /*!< SCB CCR: NONBASETHRDENA Mask */ - -/* SCB System Handler Control and State Register Definitions */ -#define SCB_SHCSR_USGFAULTENA_Pos 18U /*!< SCB SHCSR: USGFAULTENA Position */ -#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */ - -#define SCB_SHCSR_BUSFAULTENA_Pos 17U /*!< SCB SHCSR: BUSFAULTENA Position */ -#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */ - -#define SCB_SHCSR_MEMFAULTENA_Pos 16U /*!< SCB SHCSR: MEMFAULTENA Position */ -#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */ - -#define SCB_SHCSR_SVCALLPENDED_Pos 15U /*!< SCB SHCSR: SVCALLPENDED Position */ -#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */ - -#define SCB_SHCSR_BUSFAULTPENDED_Pos 14U /*!< SCB SHCSR: BUSFAULTPENDED Position */ -#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */ - -#define SCB_SHCSR_MEMFAULTPENDED_Pos 13U /*!< SCB SHCSR: MEMFAULTPENDED Position */ -#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */ - -#define SCB_SHCSR_USGFAULTPENDED_Pos 12U /*!< SCB SHCSR: USGFAULTPENDED Position */ -#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */ - -#define SCB_SHCSR_SYSTICKACT_Pos 11U /*!< SCB SHCSR: SYSTICKACT Position */ -#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */ - -#define SCB_SHCSR_PENDSVACT_Pos 10U /*!< SCB SHCSR: PENDSVACT Position */ -#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */ - -#define SCB_SHCSR_MONITORACT_Pos 8U /*!< SCB SHCSR: MONITORACT Position */ -#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */ - -#define SCB_SHCSR_SVCALLACT_Pos 7U /*!< SCB SHCSR: SVCALLACT Position */ -#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */ - -#define SCB_SHCSR_USGFAULTACT_Pos 3U /*!< SCB SHCSR: USGFAULTACT Position */ -#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */ - -#define SCB_SHCSR_BUSFAULTACT_Pos 1U /*!< SCB SHCSR: BUSFAULTACT Position */ -#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */ - -#define SCB_SHCSR_MEMFAULTACT_Pos 0U /*!< SCB SHCSR: MEMFAULTACT Position */ -#define SCB_SHCSR_MEMFAULTACT_Msk (1UL /*<< SCB_SHCSR_MEMFAULTACT_Pos*/) /*!< SCB SHCSR: MEMFAULTACT Mask */ - -/* SCB Configurable Fault Status Register Definitions */ -#define SCB_CFSR_USGFAULTSR_Pos 16U /*!< SCB CFSR: Usage Fault Status Register Position */ -#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */ - -#define SCB_CFSR_BUSFAULTSR_Pos 8U /*!< SCB CFSR: Bus Fault Status Register Position */ -#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */ - -#define SCB_CFSR_MEMFAULTSR_Pos 0U /*!< SCB CFSR: Memory Manage Fault Status Register Position */ -#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL /*<< SCB_CFSR_MEMFAULTSR_Pos*/) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */ - -/* MemManage Fault Status Register (part of SCB Configurable Fault Status Register) */ -#define SCB_CFSR_MMARVALID_Pos (SCB_SHCSR_MEMFAULTACT_Pos + 7U) /*!< SCB CFSR (MMFSR): MMARVALID Position */ -#define SCB_CFSR_MMARVALID_Msk (1UL << SCB_CFSR_MMARVALID_Pos) /*!< SCB CFSR (MMFSR): MMARVALID Mask */ - -#define SCB_CFSR_MSTKERR_Pos (SCB_SHCSR_MEMFAULTACT_Pos + 4U) /*!< SCB CFSR (MMFSR): MSTKERR Position */ -#define SCB_CFSR_MSTKERR_Msk (1UL << SCB_CFSR_MSTKERR_Pos) /*!< SCB CFSR (MMFSR): MSTKERR Mask */ - -#define SCB_CFSR_MUNSTKERR_Pos (SCB_SHCSR_MEMFAULTACT_Pos + 3U) /*!< SCB CFSR (MMFSR): MUNSTKERR Position */ -#define SCB_CFSR_MUNSTKERR_Msk (1UL << SCB_CFSR_MUNSTKERR_Pos) /*!< SCB CFSR (MMFSR): MUNSTKERR Mask */ - -#define SCB_CFSR_DACCVIOL_Pos (SCB_SHCSR_MEMFAULTACT_Pos + 1U) /*!< SCB CFSR (MMFSR): DACCVIOL Position */ -#define SCB_CFSR_DACCVIOL_Msk (1UL << SCB_CFSR_DACCVIOL_Pos) /*!< SCB CFSR (MMFSR): DACCVIOL Mask */ - -#define SCB_CFSR_IACCVIOL_Pos (SCB_SHCSR_MEMFAULTACT_Pos + 0U) /*!< SCB CFSR (MMFSR): IACCVIOL Position */ -#define SCB_CFSR_IACCVIOL_Msk (1UL /*<< SCB_CFSR_IACCVIOL_Pos*/) /*!< SCB CFSR (MMFSR): IACCVIOL Mask */ - -/* BusFault Status Register (part of SCB Configurable Fault Status Register) */ -#define SCB_CFSR_BFARVALID_Pos (SCB_CFSR_BUSFAULTSR_Pos + 7U) /*!< SCB CFSR (BFSR): BFARVALID Position */ -#define SCB_CFSR_BFARVALID_Msk (1UL << SCB_CFSR_BFARVALID_Pos) /*!< SCB CFSR (BFSR): BFARVALID Mask */ - -#define SCB_CFSR_STKERR_Pos (SCB_CFSR_BUSFAULTSR_Pos + 4U) /*!< SCB CFSR (BFSR): STKERR Position */ -#define SCB_CFSR_STKERR_Msk (1UL << SCB_CFSR_STKERR_Pos) /*!< SCB CFSR (BFSR): STKERR Mask */ - -#define SCB_CFSR_UNSTKERR_Pos (SCB_CFSR_BUSFAULTSR_Pos + 3U) /*!< SCB CFSR (BFSR): UNSTKERR Position */ -#define SCB_CFSR_UNSTKERR_Msk (1UL << SCB_CFSR_UNSTKERR_Pos) /*!< SCB CFSR (BFSR): UNSTKERR Mask */ - -#define SCB_CFSR_IMPRECISERR_Pos (SCB_CFSR_BUSFAULTSR_Pos + 2U) /*!< SCB CFSR (BFSR): IMPRECISERR Position */ -#define SCB_CFSR_IMPRECISERR_Msk (1UL << SCB_CFSR_IMPRECISERR_Pos) /*!< SCB CFSR (BFSR): IMPRECISERR Mask */ - -#define SCB_CFSR_PRECISERR_Pos (SCB_CFSR_BUSFAULTSR_Pos + 1U) /*!< SCB CFSR (BFSR): PRECISERR Position */ -#define SCB_CFSR_PRECISERR_Msk (1UL << SCB_CFSR_PRECISERR_Pos) /*!< SCB CFSR (BFSR): PRECISERR Mask */ - -#define SCB_CFSR_IBUSERR_Pos (SCB_CFSR_BUSFAULTSR_Pos + 0U) /*!< SCB CFSR (BFSR): IBUSERR Position */ -#define SCB_CFSR_IBUSERR_Msk (1UL << SCB_CFSR_IBUSERR_Pos) /*!< SCB CFSR (BFSR): IBUSERR Mask */ - -/* UsageFault Status Register (part of SCB Configurable Fault Status Register) */ -#define SCB_CFSR_DIVBYZERO_Pos (SCB_CFSR_USGFAULTSR_Pos + 9U) /*!< SCB CFSR (UFSR): DIVBYZERO Position */ -#define SCB_CFSR_DIVBYZERO_Msk (1UL << SCB_CFSR_DIVBYZERO_Pos) /*!< SCB CFSR (UFSR): DIVBYZERO Mask */ - -#define SCB_CFSR_UNALIGNED_Pos (SCB_CFSR_USGFAULTSR_Pos + 8U) /*!< SCB CFSR (UFSR): UNALIGNED Position */ -#define SCB_CFSR_UNALIGNED_Msk (1UL << SCB_CFSR_UNALIGNED_Pos) /*!< SCB CFSR (UFSR): UNALIGNED Mask */ - -#define SCB_CFSR_NOCP_Pos (SCB_CFSR_USGFAULTSR_Pos + 3U) /*!< SCB CFSR (UFSR): NOCP Position */ -#define SCB_CFSR_NOCP_Msk (1UL << SCB_CFSR_NOCP_Pos) /*!< SCB CFSR (UFSR): NOCP Mask */ - -#define SCB_CFSR_INVPC_Pos (SCB_CFSR_USGFAULTSR_Pos + 2U) /*!< SCB CFSR (UFSR): INVPC Position */ -#define SCB_CFSR_INVPC_Msk (1UL << SCB_CFSR_INVPC_Pos) /*!< SCB CFSR (UFSR): INVPC Mask */ - -#define SCB_CFSR_INVSTATE_Pos (SCB_CFSR_USGFAULTSR_Pos + 1U) /*!< SCB CFSR (UFSR): INVSTATE Position */ -#define SCB_CFSR_INVSTATE_Msk (1UL << SCB_CFSR_INVSTATE_Pos) /*!< SCB CFSR (UFSR): INVSTATE Mask */ - -#define SCB_CFSR_UNDEFINSTR_Pos (SCB_CFSR_USGFAULTSR_Pos + 0U) /*!< SCB CFSR (UFSR): UNDEFINSTR Position */ -#define SCB_CFSR_UNDEFINSTR_Msk (1UL << SCB_CFSR_UNDEFINSTR_Pos) /*!< SCB CFSR (UFSR): UNDEFINSTR Mask */ - -/* SCB Hard Fault Status Register Definitions */ -#define SCB_HFSR_DEBUGEVT_Pos 31U /*!< SCB HFSR: DEBUGEVT Position */ -#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */ - -#define SCB_HFSR_FORCED_Pos 30U /*!< SCB HFSR: FORCED Position */ -#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */ - -#define SCB_HFSR_VECTTBL_Pos 1U /*!< SCB HFSR: VECTTBL Position */ -#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */ - -/* SCB Debug Fault Status Register Definitions */ -#define SCB_DFSR_EXTERNAL_Pos 4U /*!< SCB DFSR: EXTERNAL Position */ -#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */ - -#define SCB_DFSR_VCATCH_Pos 3U /*!< SCB DFSR: VCATCH Position */ -#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */ - -#define SCB_DFSR_DWTTRAP_Pos 2U /*!< SCB DFSR: DWTTRAP Position */ -#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */ - -#define SCB_DFSR_BKPT_Pos 1U /*!< SCB DFSR: BKPT Position */ -#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */ - -#define SCB_DFSR_HALTED_Pos 0U /*!< SCB DFSR: HALTED Position */ -#define SCB_DFSR_HALTED_Msk (1UL /*<< SCB_DFSR_HALTED_Pos*/) /*!< SCB DFSR: HALTED Mask */ - -/*@} end of group CMSIS_SCB */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB) - \brief Type definitions for the System Control and ID Register not in the SCB - @{ - */ - -/** - \brief Structure type to access the System Control and ID Register not in the SCB. - */ -typedef struct -{ - uint32_t RESERVED0[1U]; - __IM uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */ -#if defined (__CM3_REV) && (__CM3_REV >= 0x200U) - __IOM uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */ -#else - uint32_t RESERVED1[1U]; -#endif -} SCnSCB_Type; - -/* Interrupt Controller Type Register Definitions */ -#define SCnSCB_ICTR_INTLINESNUM_Pos 0U /*!< ICTR: INTLINESNUM Position */ -#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL /*<< SCnSCB_ICTR_INTLINESNUM_Pos*/) /*!< ICTR: INTLINESNUM Mask */ - -/* Auxiliary Control Register Definitions */ -#if defined (__CM3_REV) && (__CM3_REV >= 0x200U) -#define SCnSCB_ACTLR_DISOOFP_Pos 9U /*!< ACTLR: DISOOFP Position */ -#define SCnSCB_ACTLR_DISOOFP_Msk (1UL << SCnSCB_ACTLR_DISOOFP_Pos) /*!< ACTLR: DISOOFP Mask */ - -#define SCnSCB_ACTLR_DISFPCA_Pos 8U /*!< ACTLR: DISFPCA Position */ -#define SCnSCB_ACTLR_DISFPCA_Msk (1UL << SCnSCB_ACTLR_DISFPCA_Pos) /*!< ACTLR: DISFPCA Mask */ - -#define SCnSCB_ACTLR_DISFOLD_Pos 2U /*!< ACTLR: DISFOLD Position */ -#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */ - -#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1U /*!< ACTLR: DISDEFWBUF Position */ -#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */ - -#define SCnSCB_ACTLR_DISMCYCINT_Pos 0U /*!< ACTLR: DISMCYCINT Position */ -#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL /*<< SCnSCB_ACTLR_DISMCYCINT_Pos*/) /*!< ACTLR: DISMCYCINT Mask */ -#endif - -/*@} end of group CMSIS_SCnotSCB */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_SysTick System Tick Timer (SysTick) - \brief Type definitions for the System Timer Registers. - @{ - */ - -/** - \brief Structure type to access the System Timer (SysTick). - */ -typedef struct -{ - __IOM uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */ - __IOM uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */ - __IOM uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */ - __IM uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */ -} SysTick_Type; - -/* SysTick Control / Status Register Definitions */ -#define SysTick_CTRL_COUNTFLAG_Pos 16U /*!< SysTick CTRL: COUNTFLAG Position */ -#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */ - -#define SysTick_CTRL_CLKSOURCE_Pos 2U /*!< SysTick CTRL: CLKSOURCE Position */ -#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */ - -#define SysTick_CTRL_TICKINT_Pos 1U /*!< SysTick CTRL: TICKINT Position */ -#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */ - -#define SysTick_CTRL_ENABLE_Pos 0U /*!< SysTick CTRL: ENABLE Position */ -#define SysTick_CTRL_ENABLE_Msk (1UL /*<< SysTick_CTRL_ENABLE_Pos*/) /*!< SysTick CTRL: ENABLE Mask */ - -/* SysTick Reload Register Definitions */ -#define SysTick_LOAD_RELOAD_Pos 0U /*!< SysTick LOAD: RELOAD Position */ -#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL /*<< SysTick_LOAD_RELOAD_Pos*/) /*!< SysTick LOAD: RELOAD Mask */ - -/* SysTick Current Register Definitions */ -#define SysTick_VAL_CURRENT_Pos 0U /*!< SysTick VAL: CURRENT Position */ -#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL /*<< SysTick_VAL_CURRENT_Pos*/) /*!< SysTick VAL: CURRENT Mask */ - -/* SysTick Calibration Register Definitions */ -#define SysTick_CALIB_NOREF_Pos 31U /*!< SysTick CALIB: NOREF Position */ -#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */ - -#define SysTick_CALIB_SKEW_Pos 30U /*!< SysTick CALIB: SKEW Position */ -#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */ - -#define SysTick_CALIB_TENMS_Pos 0U /*!< SysTick CALIB: TENMS Position */ -#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL /*<< SysTick_CALIB_TENMS_Pos*/) /*!< SysTick CALIB: TENMS Mask */ - -/*@} end of group CMSIS_SysTick */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM) - \brief Type definitions for the Instrumentation Trace Macrocell (ITM) - @{ - */ - -/** - \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM). - */ -typedef struct -{ - __OM union - { - __OM uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */ - __OM uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */ - __OM uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */ - } PORT [32U]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */ - uint32_t RESERVED0[864U]; - __IOM uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */ - uint32_t RESERVED1[15U]; - __IOM uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */ - uint32_t RESERVED2[15U]; - __IOM uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */ - uint32_t RESERVED3[32U]; - uint32_t RESERVED4[43U]; - __OM uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */ - __IM uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */ - uint32_t RESERVED5[6U]; - __IM uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */ - __IM uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */ - __IM uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */ - __IM uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */ - __IM uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */ - __IM uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */ - __IM uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */ - __IM uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */ - __IM uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */ - __IM uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */ - __IM uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */ - __IM uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */ -} ITM_Type; - -/* ITM Trace Privilege Register Definitions */ -#define ITM_TPR_PRIVMASK_Pos 0U /*!< ITM TPR: PRIVMASK Position */ -#define ITM_TPR_PRIVMASK_Msk (0xFFFFFFFFUL /*<< ITM_TPR_PRIVMASK_Pos*/) /*!< ITM TPR: PRIVMASK Mask */ - -/* ITM Trace Control Register Definitions */ -#define ITM_TCR_BUSY_Pos 23U /*!< ITM TCR: BUSY Position */ -#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */ - -#define ITM_TCR_TraceBusID_Pos 16U /*!< ITM TCR: ATBID Position */ -#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */ - -#define ITM_TCR_GTSFREQ_Pos 10U /*!< ITM TCR: Global timestamp frequency Position */ -#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */ - -#define ITM_TCR_TSPrescale_Pos 8U /*!< ITM TCR: TSPrescale Position */ -#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */ - -#define ITM_TCR_SWOENA_Pos 4U /*!< ITM TCR: SWOENA Position */ -#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */ - -#define ITM_TCR_DWTENA_Pos 3U /*!< ITM TCR: DWTENA Position */ -#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */ - -#define ITM_TCR_SYNCENA_Pos 2U /*!< ITM TCR: SYNCENA Position */ -#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */ - -#define ITM_TCR_TSENA_Pos 1U /*!< ITM TCR: TSENA Position */ -#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */ - -#define ITM_TCR_ITMENA_Pos 0U /*!< ITM TCR: ITM Enable bit Position */ -#define ITM_TCR_ITMENA_Msk (1UL /*<< ITM_TCR_ITMENA_Pos*/) /*!< ITM TCR: ITM Enable bit Mask */ - -/* ITM Lock Status Register Definitions */ -#define ITM_LSR_ByteAcc_Pos 2U /*!< ITM LSR: ByteAcc Position */ -#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */ - -#define ITM_LSR_Access_Pos 1U /*!< ITM LSR: Access Position */ -#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */ - -#define ITM_LSR_Present_Pos 0U /*!< ITM LSR: Present Position */ -#define ITM_LSR_Present_Msk (1UL /*<< ITM_LSR_Present_Pos*/) /*!< ITM LSR: Present Mask */ - -/*@}*/ /* end of group CMSIS_ITM */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT) - \brief Type definitions for the Data Watchpoint and Trace (DWT) - @{ - */ - -/** - \brief Structure type to access the Data Watchpoint and Trace Register (DWT). - */ -typedef struct -{ - __IOM uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */ - __IOM uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */ - __IOM uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */ - __IOM uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */ - __IOM uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */ - __IOM uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */ - __IOM uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */ - __IM uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */ - __IOM uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */ - __IOM uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */ - __IOM uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */ - uint32_t RESERVED0[1U]; - __IOM uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */ - __IOM uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */ - __IOM uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */ - uint32_t RESERVED1[1U]; - __IOM uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */ - __IOM uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */ - __IOM uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */ - uint32_t RESERVED2[1U]; - __IOM uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */ - __IOM uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */ - __IOM uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */ -} DWT_Type; - -/* DWT Control Register Definitions */ -#define DWT_CTRL_NUMCOMP_Pos 28U /*!< DWT CTRL: NUMCOMP Position */ -#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */ - -#define DWT_CTRL_NOTRCPKT_Pos 27U /*!< DWT CTRL: NOTRCPKT Position */ -#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */ - -#define DWT_CTRL_NOEXTTRIG_Pos 26U /*!< DWT CTRL: NOEXTTRIG Position */ -#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */ - -#define DWT_CTRL_NOCYCCNT_Pos 25U /*!< DWT CTRL: NOCYCCNT Position */ -#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */ - -#define DWT_CTRL_NOPRFCNT_Pos 24U /*!< DWT CTRL: NOPRFCNT Position */ -#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */ - -#define DWT_CTRL_CYCEVTENA_Pos 22U /*!< DWT CTRL: CYCEVTENA Position */ -#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */ - -#define DWT_CTRL_FOLDEVTENA_Pos 21U /*!< DWT CTRL: FOLDEVTENA Position */ -#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */ - -#define DWT_CTRL_LSUEVTENA_Pos 20U /*!< DWT CTRL: LSUEVTENA Position */ -#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */ - -#define DWT_CTRL_SLEEPEVTENA_Pos 19U /*!< DWT CTRL: SLEEPEVTENA Position */ -#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */ - -#define DWT_CTRL_EXCEVTENA_Pos 18U /*!< DWT CTRL: EXCEVTENA Position */ -#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */ - -#define DWT_CTRL_CPIEVTENA_Pos 17U /*!< DWT CTRL: CPIEVTENA Position */ -#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */ - -#define DWT_CTRL_EXCTRCENA_Pos 16U /*!< DWT CTRL: EXCTRCENA Position */ -#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */ - -#define DWT_CTRL_PCSAMPLENA_Pos 12U /*!< DWT CTRL: PCSAMPLENA Position */ -#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */ - -#define DWT_CTRL_SYNCTAP_Pos 10U /*!< DWT CTRL: SYNCTAP Position */ -#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */ - -#define DWT_CTRL_CYCTAP_Pos 9U /*!< DWT CTRL: CYCTAP Position */ -#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */ - -#define DWT_CTRL_POSTINIT_Pos 5U /*!< DWT CTRL: POSTINIT Position */ -#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */ - -#define DWT_CTRL_POSTPRESET_Pos 1U /*!< DWT CTRL: POSTPRESET Position */ -#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */ - -#define DWT_CTRL_CYCCNTENA_Pos 0U /*!< DWT CTRL: CYCCNTENA Position */ -#define DWT_CTRL_CYCCNTENA_Msk (0x1UL /*<< DWT_CTRL_CYCCNTENA_Pos*/) /*!< DWT CTRL: CYCCNTENA Mask */ - -/* DWT CPI Count Register Definitions */ -#define DWT_CPICNT_CPICNT_Pos 0U /*!< DWT CPICNT: CPICNT Position */ -#define DWT_CPICNT_CPICNT_Msk (0xFFUL /*<< DWT_CPICNT_CPICNT_Pos*/) /*!< DWT CPICNT: CPICNT Mask */ - -/* DWT Exception Overhead Count Register Definitions */ -#define DWT_EXCCNT_EXCCNT_Pos 0U /*!< DWT EXCCNT: EXCCNT Position */ -#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL /*<< DWT_EXCCNT_EXCCNT_Pos*/) /*!< DWT EXCCNT: EXCCNT Mask */ - -/* DWT Sleep Count Register Definitions */ -#define DWT_SLEEPCNT_SLEEPCNT_Pos 0U /*!< DWT SLEEPCNT: SLEEPCNT Position */ -#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL /*<< DWT_SLEEPCNT_SLEEPCNT_Pos*/) /*!< DWT SLEEPCNT: SLEEPCNT Mask */ - -/* DWT LSU Count Register Definitions */ -#define DWT_LSUCNT_LSUCNT_Pos 0U /*!< DWT LSUCNT: LSUCNT Position */ -#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL /*<< DWT_LSUCNT_LSUCNT_Pos*/) /*!< DWT LSUCNT: LSUCNT Mask */ - -/* DWT Folded-instruction Count Register Definitions */ -#define DWT_FOLDCNT_FOLDCNT_Pos 0U /*!< DWT FOLDCNT: FOLDCNT Position */ -#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL /*<< DWT_FOLDCNT_FOLDCNT_Pos*/) /*!< DWT FOLDCNT: FOLDCNT Mask */ - -/* DWT Comparator Mask Register Definitions */ -#define DWT_MASK_MASK_Pos 0U /*!< DWT MASK: MASK Position */ -#define DWT_MASK_MASK_Msk (0x1FUL /*<< DWT_MASK_MASK_Pos*/) /*!< DWT MASK: MASK Mask */ - -/* DWT Comparator Function Register Definitions */ -#define DWT_FUNCTION_MATCHED_Pos 24U /*!< DWT FUNCTION: MATCHED Position */ -#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */ - -#define DWT_FUNCTION_DATAVADDR1_Pos 16U /*!< DWT FUNCTION: DATAVADDR1 Position */ -#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */ - -#define DWT_FUNCTION_DATAVADDR0_Pos 12U /*!< DWT FUNCTION: DATAVADDR0 Position */ -#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */ - -#define DWT_FUNCTION_DATAVSIZE_Pos 10U /*!< DWT FUNCTION: DATAVSIZE Position */ -#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */ - -#define DWT_FUNCTION_LNK1ENA_Pos 9U /*!< DWT FUNCTION: LNK1ENA Position */ -#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */ - -#define DWT_FUNCTION_DATAVMATCH_Pos 8U /*!< DWT FUNCTION: DATAVMATCH Position */ -#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */ - -#define DWT_FUNCTION_CYCMATCH_Pos 7U /*!< DWT FUNCTION: CYCMATCH Position */ -#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */ - -#define DWT_FUNCTION_EMITRANGE_Pos 5U /*!< DWT FUNCTION: EMITRANGE Position */ -#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */ - -#define DWT_FUNCTION_FUNCTION_Pos 0U /*!< DWT FUNCTION: FUNCTION Position */ -#define DWT_FUNCTION_FUNCTION_Msk (0xFUL /*<< DWT_FUNCTION_FUNCTION_Pos*/) /*!< DWT FUNCTION: FUNCTION Mask */ - -/*@}*/ /* end of group CMSIS_DWT */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_TPI Trace Port Interface (TPI) - \brief Type definitions for the Trace Port Interface (TPI) - @{ - */ - -/** - \brief Structure type to access the Trace Port Interface Register (TPI). - */ -typedef struct -{ - __IM uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */ - __IOM uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */ - uint32_t RESERVED0[2U]; - __IOM uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */ - uint32_t RESERVED1[55U]; - __IOM uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */ - uint32_t RESERVED2[131U]; - __IM uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */ - __IOM uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */ - __IM uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */ - uint32_t RESERVED3[759U]; - __IM uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER Register */ - __IM uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */ - __IM uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */ - uint32_t RESERVED4[1U]; - __IM uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */ - __IM uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */ - __IOM uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */ - uint32_t RESERVED5[39U]; - __IOM uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */ - __IOM uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */ - uint32_t RESERVED7[8U]; - __IM uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */ - __IM uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */ -} TPI_Type; - -/* TPI Asynchronous Clock Prescaler Register Definitions */ -#define TPI_ACPR_PRESCALER_Pos 0U /*!< TPI ACPR: PRESCALER Position */ -#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL /*<< TPI_ACPR_PRESCALER_Pos*/) /*!< TPI ACPR: PRESCALER Mask */ - -/* TPI Selected Pin Protocol Register Definitions */ -#define TPI_SPPR_TXMODE_Pos 0U /*!< TPI SPPR: TXMODE Position */ -#define TPI_SPPR_TXMODE_Msk (0x3UL /*<< TPI_SPPR_TXMODE_Pos*/) /*!< TPI SPPR: TXMODE Mask */ - -/* TPI Formatter and Flush Status Register Definitions */ -#define TPI_FFSR_FtNonStop_Pos 3U /*!< TPI FFSR: FtNonStop Position */ -#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */ - -#define TPI_FFSR_TCPresent_Pos 2U /*!< TPI FFSR: TCPresent Position */ -#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */ - -#define TPI_FFSR_FtStopped_Pos 1U /*!< TPI FFSR: FtStopped Position */ -#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */ - -#define TPI_FFSR_FlInProg_Pos 0U /*!< TPI FFSR: FlInProg Position */ -#define TPI_FFSR_FlInProg_Msk (0x1UL /*<< TPI_FFSR_FlInProg_Pos*/) /*!< TPI FFSR: FlInProg Mask */ - -/* TPI Formatter and Flush Control Register Definitions */ -#define TPI_FFCR_TrigIn_Pos 8U /*!< TPI FFCR: TrigIn Position */ -#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */ - -#define TPI_FFCR_EnFCont_Pos 1U /*!< TPI FFCR: EnFCont Position */ -#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */ - -/* TPI TRIGGER Register Definitions */ -#define TPI_TRIGGER_TRIGGER_Pos 0U /*!< TPI TRIGGER: TRIGGER Position */ -#define TPI_TRIGGER_TRIGGER_Msk (0x1UL /*<< TPI_TRIGGER_TRIGGER_Pos*/) /*!< TPI TRIGGER: TRIGGER Mask */ - -/* TPI Integration ETM Data Register Definitions (FIFO0) */ -#define TPI_FIFO0_ITM_ATVALID_Pos 29U /*!< TPI FIFO0: ITM_ATVALID Position */ -#define TPI_FIFO0_ITM_ATVALID_Msk (0x1UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */ - -#define TPI_FIFO0_ITM_bytecount_Pos 27U /*!< TPI FIFO0: ITM_bytecount Position */ -#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */ - -#define TPI_FIFO0_ETM_ATVALID_Pos 26U /*!< TPI FIFO0: ETM_ATVALID Position */ -#define TPI_FIFO0_ETM_ATVALID_Msk (0x1UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */ - -#define TPI_FIFO0_ETM_bytecount_Pos 24U /*!< TPI FIFO0: ETM_bytecount Position */ -#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */ - -#define TPI_FIFO0_ETM2_Pos 16U /*!< TPI FIFO0: ETM2 Position */ -#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */ - -#define TPI_FIFO0_ETM1_Pos 8U /*!< TPI FIFO0: ETM1 Position */ -#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */ - -#define TPI_FIFO0_ETM0_Pos 0U /*!< TPI FIFO0: ETM0 Position */ -#define TPI_FIFO0_ETM0_Msk (0xFFUL /*<< TPI_FIFO0_ETM0_Pos*/) /*!< TPI FIFO0: ETM0 Mask */ - -/* TPI ITATBCTR2 Register Definitions */ -#define TPI_ITATBCTR2_ATREADY2_Pos 0U /*!< TPI ITATBCTR2: ATREADY2 Position */ -#define TPI_ITATBCTR2_ATREADY2_Msk (0x1UL /*<< TPI_ITATBCTR2_ATREADY2_Pos*/) /*!< TPI ITATBCTR2: ATREADY2 Mask */ - -#define TPI_ITATBCTR2_ATREADY1_Pos 0U /*!< TPI ITATBCTR2: ATREADY1 Position */ -#define TPI_ITATBCTR2_ATREADY1_Msk (0x1UL /*<< TPI_ITATBCTR2_ATREADY1_Pos*/) /*!< TPI ITATBCTR2: ATREADY1 Mask */ - -/* TPI Integration ITM Data Register Definitions (FIFO1) */ -#define TPI_FIFO1_ITM_ATVALID_Pos 29U /*!< TPI FIFO1: ITM_ATVALID Position */ -#define TPI_FIFO1_ITM_ATVALID_Msk (0x1UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */ - -#define TPI_FIFO1_ITM_bytecount_Pos 27U /*!< TPI FIFO1: ITM_bytecount Position */ -#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */ - -#define TPI_FIFO1_ETM_ATVALID_Pos 26U /*!< TPI FIFO1: ETM_ATVALID Position */ -#define TPI_FIFO1_ETM_ATVALID_Msk (0x1UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */ - -#define TPI_FIFO1_ETM_bytecount_Pos 24U /*!< TPI FIFO1: ETM_bytecount Position */ -#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */ - -#define TPI_FIFO1_ITM2_Pos 16U /*!< TPI FIFO1: ITM2 Position */ -#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */ - -#define TPI_FIFO1_ITM1_Pos 8U /*!< TPI FIFO1: ITM1 Position */ -#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */ - -#define TPI_FIFO1_ITM0_Pos 0U /*!< TPI FIFO1: ITM0 Position */ -#define TPI_FIFO1_ITM0_Msk (0xFFUL /*<< TPI_FIFO1_ITM0_Pos*/) /*!< TPI FIFO1: ITM0 Mask */ - -/* TPI ITATBCTR0 Register Definitions */ -#define TPI_ITATBCTR0_ATREADY2_Pos 0U /*!< TPI ITATBCTR0: ATREADY2 Position */ -#define TPI_ITATBCTR0_ATREADY2_Msk (0x1UL /*<< TPI_ITATBCTR0_ATREADY2_Pos*/) /*!< TPI ITATBCTR0: ATREADY2 Mask */ - -#define TPI_ITATBCTR0_ATREADY1_Pos 0U /*!< TPI ITATBCTR0: ATREADY1 Position */ -#define TPI_ITATBCTR0_ATREADY1_Msk (0x1UL /*<< TPI_ITATBCTR0_ATREADY1_Pos*/) /*!< TPI ITATBCTR0: ATREADY1 Mask */ - -/* TPI Integration Mode Control Register Definitions */ -#define TPI_ITCTRL_Mode_Pos 0U /*!< TPI ITCTRL: Mode Position */ -#define TPI_ITCTRL_Mode_Msk (0x3UL /*<< TPI_ITCTRL_Mode_Pos*/) /*!< TPI ITCTRL: Mode Mask */ - -/* TPI DEVID Register Definitions */ -#define TPI_DEVID_NRZVALID_Pos 11U /*!< TPI DEVID: NRZVALID Position */ -#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */ - -#define TPI_DEVID_MANCVALID_Pos 10U /*!< TPI DEVID: MANCVALID Position */ -#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */ - -#define TPI_DEVID_PTINVALID_Pos 9U /*!< TPI DEVID: PTINVALID Position */ -#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */ - -#define TPI_DEVID_MinBufSz_Pos 6U /*!< TPI DEVID: MinBufSz Position */ -#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */ - -#define TPI_DEVID_AsynClkIn_Pos 5U /*!< TPI DEVID: AsynClkIn Position */ -#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */ - -#define TPI_DEVID_NrTraceInput_Pos 0U /*!< TPI DEVID: NrTraceInput Position */ -#define TPI_DEVID_NrTraceInput_Msk (0x1FUL /*<< TPI_DEVID_NrTraceInput_Pos*/) /*!< TPI DEVID: NrTraceInput Mask */ - -/* TPI DEVTYPE Register Definitions */ -#define TPI_DEVTYPE_SubType_Pos 4U /*!< TPI DEVTYPE: SubType Position */ -#define TPI_DEVTYPE_SubType_Msk (0xFUL /*<< TPI_DEVTYPE_SubType_Pos*/) /*!< TPI DEVTYPE: SubType Mask */ - -#define TPI_DEVTYPE_MajorType_Pos 0U /*!< TPI DEVTYPE: MajorType Position */ -#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */ - -/*@}*/ /* end of group CMSIS_TPI */ - - -#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U) -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_MPU Memory Protection Unit (MPU) - \brief Type definitions for the Memory Protection Unit (MPU) - @{ - */ - -/** - \brief Structure type to access the Memory Protection Unit (MPU). - */ -typedef struct -{ - __IM uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */ - __IOM uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */ - __IOM uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */ - __IOM uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */ - __IOM uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */ - __IOM uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */ - __IOM uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */ - __IOM uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */ - __IOM uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */ - __IOM uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */ - __IOM uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */ -} MPU_Type; - -#define MPU_TYPE_RALIASES 4U - -/* MPU Type Register Definitions */ -#define MPU_TYPE_IREGION_Pos 16U /*!< MPU TYPE: IREGION Position */ -#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */ - -#define MPU_TYPE_DREGION_Pos 8U /*!< MPU TYPE: DREGION Position */ -#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */ - -#define MPU_TYPE_SEPARATE_Pos 0U /*!< MPU TYPE: SEPARATE Position */ -#define MPU_TYPE_SEPARATE_Msk (1UL /*<< MPU_TYPE_SEPARATE_Pos*/) /*!< MPU TYPE: SEPARATE Mask */ - -/* MPU Control Register Definitions */ -#define MPU_CTRL_PRIVDEFENA_Pos 2U /*!< MPU CTRL: PRIVDEFENA Position */ -#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */ - -#define MPU_CTRL_HFNMIENA_Pos 1U /*!< MPU CTRL: HFNMIENA Position */ -#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */ - -#define MPU_CTRL_ENABLE_Pos 0U /*!< MPU CTRL: ENABLE Position */ -#define MPU_CTRL_ENABLE_Msk (1UL /*<< MPU_CTRL_ENABLE_Pos*/) /*!< MPU CTRL: ENABLE Mask */ - -/* MPU Region Number Register Definitions */ -#define MPU_RNR_REGION_Pos 0U /*!< MPU RNR: REGION Position */ -#define MPU_RNR_REGION_Msk (0xFFUL /*<< MPU_RNR_REGION_Pos*/) /*!< MPU RNR: REGION Mask */ - -/* MPU Region Base Address Register Definitions */ -#define MPU_RBAR_ADDR_Pos 5U /*!< MPU RBAR: ADDR Position */ -#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */ - -#define MPU_RBAR_VALID_Pos 4U /*!< MPU RBAR: VALID Position */ -#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */ - -#define MPU_RBAR_REGION_Pos 0U /*!< MPU RBAR: REGION Position */ -#define MPU_RBAR_REGION_Msk (0xFUL /*<< MPU_RBAR_REGION_Pos*/) /*!< MPU RBAR: REGION Mask */ - -/* MPU Region Attribute and Size Register Definitions */ -#define MPU_RASR_ATTRS_Pos 16U /*!< MPU RASR: MPU Region Attribute field Position */ -#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */ - -#define MPU_RASR_XN_Pos 28U /*!< MPU RASR: ATTRS.XN Position */ -#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */ - -#define MPU_RASR_AP_Pos 24U /*!< MPU RASR: ATTRS.AP Position */ -#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */ - -#define MPU_RASR_TEX_Pos 19U /*!< MPU RASR: ATTRS.TEX Position */ -#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */ - -#define MPU_RASR_S_Pos 18U /*!< MPU RASR: ATTRS.S Position */ -#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */ - -#define MPU_RASR_C_Pos 17U /*!< MPU RASR: ATTRS.C Position */ -#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */ - -#define MPU_RASR_B_Pos 16U /*!< MPU RASR: ATTRS.B Position */ -#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */ - -#define MPU_RASR_SRD_Pos 8U /*!< MPU RASR: Sub-Region Disable Position */ -#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */ - -#define MPU_RASR_SIZE_Pos 1U /*!< MPU RASR: Region Size Field Position */ -#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */ - -#define MPU_RASR_ENABLE_Pos 0U /*!< MPU RASR: Region enable bit Position */ -#define MPU_RASR_ENABLE_Msk (1UL /*<< MPU_RASR_ENABLE_Pos*/) /*!< MPU RASR: Region enable bit Disable Mask */ - -/*@} end of group CMSIS_MPU */ -#endif - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug) - \brief Type definitions for the Core Debug Registers - @{ - */ - -/** - \brief Structure type to access the Core Debug Register (CoreDebug). - */ -typedef struct -{ - __IOM uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */ - __OM uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */ - __IOM uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */ - __IOM uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */ -} CoreDebug_Type; - -/* Debug Halting Control and Status Register Definitions */ -#define CoreDebug_DHCSR_DBGKEY_Pos 16U /*!< CoreDebug DHCSR: DBGKEY Position */ -#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */ - -#define CoreDebug_DHCSR_S_RESET_ST_Pos 25U /*!< CoreDebug DHCSR: S_RESET_ST Position */ -#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */ - -#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24U /*!< CoreDebug DHCSR: S_RETIRE_ST Position */ -#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */ - -#define CoreDebug_DHCSR_S_LOCKUP_Pos 19U /*!< CoreDebug DHCSR: S_LOCKUP Position */ -#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */ - -#define CoreDebug_DHCSR_S_SLEEP_Pos 18U /*!< CoreDebug DHCSR: S_SLEEP Position */ -#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */ - -#define CoreDebug_DHCSR_S_HALT_Pos 17U /*!< CoreDebug DHCSR: S_HALT Position */ -#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */ - -#define CoreDebug_DHCSR_S_REGRDY_Pos 16U /*!< CoreDebug DHCSR: S_REGRDY Position */ -#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */ - -#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5U /*!< CoreDebug DHCSR: C_SNAPSTALL Position */ -#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */ - -#define CoreDebug_DHCSR_C_MASKINTS_Pos 3U /*!< CoreDebug DHCSR: C_MASKINTS Position */ -#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */ - -#define CoreDebug_DHCSR_C_STEP_Pos 2U /*!< CoreDebug DHCSR: C_STEP Position */ -#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */ - -#define CoreDebug_DHCSR_C_HALT_Pos 1U /*!< CoreDebug DHCSR: C_HALT Position */ -#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */ - -#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0U /*!< CoreDebug DHCSR: C_DEBUGEN Position */ -#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL /*<< CoreDebug_DHCSR_C_DEBUGEN_Pos*/) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */ - -/* Debug Core Register Selector Register Definitions */ -#define CoreDebug_DCRSR_REGWnR_Pos 16U /*!< CoreDebug DCRSR: REGWnR Position */ -#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */ - -#define CoreDebug_DCRSR_REGSEL_Pos 0U /*!< CoreDebug DCRSR: REGSEL Position */ -#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL /*<< CoreDebug_DCRSR_REGSEL_Pos*/) /*!< CoreDebug DCRSR: REGSEL Mask */ - -/* Debug Exception and Monitor Control Register Definitions */ -#define CoreDebug_DEMCR_TRCENA_Pos 24U /*!< CoreDebug DEMCR: TRCENA Position */ -#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */ - -#define CoreDebug_DEMCR_MON_REQ_Pos 19U /*!< CoreDebug DEMCR: MON_REQ Position */ -#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */ - -#define CoreDebug_DEMCR_MON_STEP_Pos 18U /*!< CoreDebug DEMCR: MON_STEP Position */ -#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */ - -#define CoreDebug_DEMCR_MON_PEND_Pos 17U /*!< CoreDebug DEMCR: MON_PEND Position */ -#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */ - -#define CoreDebug_DEMCR_MON_EN_Pos 16U /*!< CoreDebug DEMCR: MON_EN Position */ -#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */ - -#define CoreDebug_DEMCR_VC_HARDERR_Pos 10U /*!< CoreDebug DEMCR: VC_HARDERR Position */ -#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */ - -#define CoreDebug_DEMCR_VC_INTERR_Pos 9U /*!< CoreDebug DEMCR: VC_INTERR Position */ -#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */ - -#define CoreDebug_DEMCR_VC_BUSERR_Pos 8U /*!< CoreDebug DEMCR: VC_BUSERR Position */ -#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */ - -#define CoreDebug_DEMCR_VC_STATERR_Pos 7U /*!< CoreDebug DEMCR: VC_STATERR Position */ -#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */ - -#define CoreDebug_DEMCR_VC_CHKERR_Pos 6U /*!< CoreDebug DEMCR: VC_CHKERR Position */ -#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */ - -#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5U /*!< CoreDebug DEMCR: VC_NOCPERR Position */ -#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */ - -#define CoreDebug_DEMCR_VC_MMERR_Pos 4U /*!< CoreDebug DEMCR: VC_MMERR Position */ -#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */ - -#define CoreDebug_DEMCR_VC_CORERESET_Pos 0U /*!< CoreDebug DEMCR: VC_CORERESET Position */ -#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL /*<< CoreDebug_DEMCR_VC_CORERESET_Pos*/) /*!< CoreDebug DEMCR: VC_CORERESET Mask */ - -/*@} end of group CMSIS_CoreDebug */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_core_bitfield Core register bit field macros - \brief Macros for use with bit field definitions (xxx_Pos, xxx_Msk). - @{ - */ - -/** - \brief Mask and shift a bit field value for use in a register bit range. - \param[in] field Name of the register bit field. - \param[in] value Value of the bit field. This parameter is interpreted as an uint32_t type. - \return Masked and shifted value. -*/ -#define _VAL2FLD(field, value) (((uint32_t)(value) << field ## _Pos) & field ## _Msk) - -/** - \brief Mask and shift a register value to extract a bit filed value. - \param[in] field Name of the register bit field. - \param[in] value Value of register. This parameter is interpreted as an uint32_t type. - \return Masked and shifted bit field value. -*/ -#define _FLD2VAL(field, value) (((uint32_t)(value) & field ## _Msk) >> field ## _Pos) - -/*@} end of group CMSIS_core_bitfield */ - - -/** - \ingroup CMSIS_core_register - \defgroup CMSIS_core_base Core Definitions - \brief Definitions for base addresses, unions, and structures. - @{ - */ - -/* Memory mapping of Core Hardware */ -#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */ -#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */ -#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */ -#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */ -#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */ -#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */ -#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */ -#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */ - -#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */ -#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */ -#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */ -#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */ -#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */ -#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */ -#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */ -#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */ - -#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U) - #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */ - #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */ -#endif - -/*@} */ - - - -/******************************************************************************* - * Hardware Abstraction Layer - Core Function Interface contains: - - Core NVIC Functions - - Core SysTick Functions - - Core Debug Functions - - Core Register Access Functions - ******************************************************************************/ -/** - \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference -*/ - - - -/* ########################## NVIC functions #################################### */ -/** - \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_NVICFunctions NVIC Functions - \brief Functions that manage interrupts and exceptions via the NVIC. - @{ - */ - -#ifdef CMSIS_NVIC_VIRTUAL - #ifndef CMSIS_NVIC_VIRTUAL_HEADER_FILE - #define CMSIS_NVIC_VIRTUAL_HEADER_FILE "cmsis_nvic_virtual.h" - #endif - #include CMSIS_NVIC_VIRTUAL_HEADER_FILE -#else - #define NVIC_SetPriorityGrouping __NVIC_SetPriorityGrouping - #define NVIC_GetPriorityGrouping __NVIC_GetPriorityGrouping - #define NVIC_EnableIRQ __NVIC_EnableIRQ - #define NVIC_GetEnableIRQ __NVIC_GetEnableIRQ - #define NVIC_DisableIRQ __NVIC_DisableIRQ - #define NVIC_GetPendingIRQ __NVIC_GetPendingIRQ - #define NVIC_SetPendingIRQ __NVIC_SetPendingIRQ - #define NVIC_ClearPendingIRQ __NVIC_ClearPendingIRQ - #define NVIC_GetActive __NVIC_GetActive - #define NVIC_SetPriority __NVIC_SetPriority - #define NVIC_GetPriority __NVIC_GetPriority - #define NVIC_SystemReset __NVIC_SystemReset -#endif /* CMSIS_NVIC_VIRTUAL */ - -#ifdef CMSIS_VECTAB_VIRTUAL - #ifndef CMSIS_VECTAB_VIRTUAL_HEADER_FILE - #define CMSIS_VECTAB_VIRTUAL_HEADER_FILE "cmsis_vectab_virtual.h" - #endif - #include CMSIS_VECTAB_VIRTUAL_HEADER_FILE -#else - #define NVIC_SetVector __NVIC_SetVector - #define NVIC_GetVector __NVIC_GetVector -#endif /* (CMSIS_VECTAB_VIRTUAL) */ - -#define NVIC_USER_IRQ_OFFSET 16 - - -/* The following EXC_RETURN values are saved the LR on exception entry */ -#define EXC_RETURN_HANDLER (0xFFFFFFF1UL) /* return to Handler mode, uses MSP after return */ -#define EXC_RETURN_THREAD_MSP (0xFFFFFFF9UL) /* return to Thread mode, uses MSP after return */ -#define EXC_RETURN_THREAD_PSP (0xFFFFFFFDUL) /* return to Thread mode, uses PSP after return */ - - -/** - \brief Set Priority Grouping - \details Sets the priority grouping field using the required unlock sequence. - The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field. - Only values from 0..7 are used. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. - \param [in] PriorityGroup Priority grouping field. - */ -__STATIC_INLINE void __NVIC_SetPriorityGrouping(uint32_t PriorityGroup) -{ - uint32_t reg_value; - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL); /* only values 0..7 are used */ - - reg_value = SCB->AIRCR; /* read old register configuration */ - reg_value &= ~((uint32_t)(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk)); /* clear bits to change */ - reg_value = (reg_value | - ((uint32_t)0x5FAUL << SCB_AIRCR_VECTKEY_Pos) | - (PriorityGroupTmp << SCB_AIRCR_PRIGROUP_Pos) ); /* Insert write key and priority group */ - SCB->AIRCR = reg_value; -} - - -/** - \brief Get Priority Grouping - \details Reads the priority grouping field from the NVIC Interrupt Controller. - \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field). - */ -__STATIC_INLINE uint32_t __NVIC_GetPriorityGrouping(void) -{ - return ((uint32_t)((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos)); -} - - -/** - \brief Enable Interrupt - \details Enables a device specific interrupt in the NVIC interrupt controller. - \param [in] IRQn Device specific interrupt number. - \note IRQn must not be negative. - */ -__STATIC_INLINE void __NVIC_EnableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - __COMPILER_BARRIER(); - NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - __COMPILER_BARRIER(); - } -} - - -/** - \brief Get Interrupt Enable status - \details Returns a device specific interrupt enable status from the NVIC interrupt controller. - \param [in] IRQn Device specific interrupt number. - \return 0 Interrupt is not enabled. - \return 1 Interrupt is enabled. - \note IRQn must not be negative. - */ -__STATIC_INLINE uint32_t __NVIC_GetEnableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - return((uint32_t)(((NVIC->ISER[(((uint32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL)); - } - else - { - return(0U); - } -} - - -/** - \brief Disable Interrupt - \details Disables a device specific interrupt in the NVIC interrupt controller. - \param [in] IRQn Device specific interrupt number. - \note IRQn must not be negative. - */ -__STATIC_INLINE void __NVIC_DisableIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - NVIC->ICER[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - __DSB(); - __ISB(); - } -} - - -/** - \brief Get Pending Interrupt - \details Reads the NVIC pending register and returns the pending bit for the specified device specific interrupt. - \param [in] IRQn Device specific interrupt number. - \return 0 Interrupt status is not pending. - \return 1 Interrupt status is pending. - \note IRQn must not be negative. - */ -__STATIC_INLINE uint32_t __NVIC_GetPendingIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - return((uint32_t)(((NVIC->ISPR[(((uint32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL)); - } - else - { - return(0U); - } -} - - -/** - \brief Set Pending Interrupt - \details Sets the pending bit of a device specific interrupt in the NVIC pending register. - \param [in] IRQn Device specific interrupt number. - \note IRQn must not be negative. - */ -__STATIC_INLINE void __NVIC_SetPendingIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - NVIC->ISPR[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - } -} - - -/** - \brief Clear Pending Interrupt - \details Clears the pending bit of a device specific interrupt in the NVIC pending register. - \param [in] IRQn Device specific interrupt number. - \note IRQn must not be negative. - */ -__STATIC_INLINE void __NVIC_ClearPendingIRQ(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - NVIC->ICPR[(((uint32_t)IRQn) >> 5UL)] = (uint32_t)(1UL << (((uint32_t)IRQn) & 0x1FUL)); - } -} - - -/** - \brief Get Active Interrupt - \details Reads the active register in the NVIC and returns the active bit for the device specific interrupt. - \param [in] IRQn Device specific interrupt number. - \return 0 Interrupt status is not active. - \return 1 Interrupt status is active. - \note IRQn must not be negative. - */ -__STATIC_INLINE uint32_t __NVIC_GetActive(IRQn_Type IRQn) -{ - if ((int32_t)(IRQn) >= 0) - { - return((uint32_t)(((NVIC->IABR[(((uint32_t)IRQn) >> 5UL)] & (1UL << (((uint32_t)IRQn) & 0x1FUL))) != 0UL) ? 1UL : 0UL)); - } - else - { - return(0U); - } -} - - -/** - \brief Set Interrupt Priority - \details Sets the priority of a device specific interrupt or a processor exception. - The interrupt number can be positive to specify a device specific interrupt, - or negative to specify a processor exception. - \param [in] IRQn Interrupt number. - \param [in] priority Priority to set. - \note The priority cannot be set for every processor exception. - */ -__STATIC_INLINE void __NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority) -{ - if ((int32_t)(IRQn) >= 0) - { - NVIC->IP[((uint32_t)IRQn)] = (uint8_t)((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL); - } - else - { - SCB->SHP[(((uint32_t)IRQn) & 0xFUL)-4UL] = (uint8_t)((priority << (8U - __NVIC_PRIO_BITS)) & (uint32_t)0xFFUL); - } -} - - -/** - \brief Get Interrupt Priority - \details Reads the priority of a device specific interrupt or a processor exception. - The interrupt number can be positive to specify a device specific interrupt, - or negative to specify a processor exception. - \param [in] IRQn Interrupt number. - \return Interrupt Priority. - Value is aligned automatically to the implemented priority bits of the microcontroller. - */ -__STATIC_INLINE uint32_t __NVIC_GetPriority(IRQn_Type IRQn) -{ - - if ((int32_t)(IRQn) >= 0) - { - return(((uint32_t)NVIC->IP[((uint32_t)IRQn)] >> (8U - __NVIC_PRIO_BITS))); - } - else - { - return(((uint32_t)SCB->SHP[(((uint32_t)IRQn) & 0xFUL)-4UL] >> (8U - __NVIC_PRIO_BITS))); - } -} - - -/** - \brief Encode Priority - \details Encodes the priority for an interrupt with the given priority group, - preemptive priority value, and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set. - \param [in] PriorityGroup Used priority group. - \param [in] PreemptPriority Preemptive priority value (starting from 0). - \param [in] SubPriority Subpriority value (starting from 0). - \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority(). - */ -__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp); - SubPriorityBits = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS)); - - return ( - ((PreemptPriority & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL)) << SubPriorityBits) | - ((SubPriority & (uint32_t)((1UL << (SubPriorityBits )) - 1UL))) - ); -} - - -/** - \brief Decode Priority - \details Decodes an interrupt priority value with a given priority group to - preemptive priority value and subpriority value. - In case of a conflict between priority grouping and available - priority bits (__NVIC_PRIO_BITS) the smallest possible priority group is set. - \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority(). - \param [in] PriorityGroup Used priority group. - \param [out] pPreemptPriority Preemptive priority value (starting from 0). - \param [out] pSubPriority Subpriority value (starting from 0). - */ -__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* const pPreemptPriority, uint32_t* const pSubPriority) -{ - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07UL); /* only values 0..7 are used */ - uint32_t PreemptPriorityBits; - uint32_t SubPriorityBits; - - PreemptPriorityBits = ((7UL - PriorityGroupTmp) > (uint32_t)(__NVIC_PRIO_BITS)) ? (uint32_t)(__NVIC_PRIO_BITS) : (uint32_t)(7UL - PriorityGroupTmp); - SubPriorityBits = ((PriorityGroupTmp + (uint32_t)(__NVIC_PRIO_BITS)) < (uint32_t)7UL) ? (uint32_t)0UL : (uint32_t)((PriorityGroupTmp - 7UL) + (uint32_t)(__NVIC_PRIO_BITS)); - - *pPreemptPriority = (Priority >> SubPriorityBits) & (uint32_t)((1UL << (PreemptPriorityBits)) - 1UL); - *pSubPriority = (Priority ) & (uint32_t)((1UL << (SubPriorityBits )) - 1UL); -} - - -/** - \brief Set Interrupt Vector - \details Sets an interrupt vector in SRAM based interrupt vector table. - The interrupt number can be positive to specify a device specific interrupt, - or negative to specify a processor exception. - VTOR must been relocated to SRAM before. - \param [in] IRQn Interrupt number - \param [in] vector Address of interrupt handler function - */ -__STATIC_INLINE void __NVIC_SetVector(IRQn_Type IRQn, uint32_t vector) -{ - uint32_t vectors = (uint32_t )SCB->VTOR; - (* (int *) (vectors + ((int32_t)IRQn + NVIC_USER_IRQ_OFFSET) * 4)) = vector; - /* ARM Application Note 321 states that the M3 does not require the architectural barrier */ -} - - -/** - \brief Get Interrupt Vector - \details Reads an interrupt vector from interrupt vector table. - The interrupt number can be positive to specify a device specific interrupt, - or negative to specify a processor exception. - \param [in] IRQn Interrupt number. - \return Address of interrupt handler function - */ -__STATIC_INLINE uint32_t __NVIC_GetVector(IRQn_Type IRQn) -{ - uint32_t vectors = (uint32_t )SCB->VTOR; - return (uint32_t)(* (int *) (vectors + ((int32_t)IRQn + NVIC_USER_IRQ_OFFSET) * 4)); -} - - -/** - \brief System Reset - \details Initiates a system reset request to reset the MCU. - */ -__NO_RETURN __STATIC_INLINE void __NVIC_SystemReset(void) -{ - __DSB(); /* Ensure all outstanding memory accesses included - buffered write are completed before reset */ - SCB->AIRCR = (uint32_t)((0x5FAUL << SCB_AIRCR_VECTKEY_Pos) | - (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) | - SCB_AIRCR_SYSRESETREQ_Msk ); /* Keep priority group unchanged */ - __DSB(); /* Ensure completion of memory access */ - - for(;;) /* wait until reset */ - { - __NOP(); - } -} - -/*@} end of CMSIS_Core_NVICFunctions */ - -/* ########################## MPU functions #################################### */ - -#if defined (__MPU_PRESENT) && (__MPU_PRESENT == 1U) - -#include "mpu_armv7.h" - -#endif - - -/* ########################## FPU functions #################################### */ -/** - \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_FpuFunctions FPU Functions - \brief Function that provides FPU type. - @{ - */ - -/** - \brief get FPU type - \details returns the FPU type - \returns - - \b 0: No FPU - - \b 1: Single precision FPU - - \b 2: Double + Single precision FPU - */ -__STATIC_INLINE uint32_t SCB_GetFPUType(void) -{ - return 0U; /* No FPU */ -} - - -/*@} end of CMSIS_Core_FpuFunctions */ - - - -/* ################################## SysTick function ############################################ */ -/** - \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_Core_SysTickFunctions SysTick Functions - \brief Functions that configure the System. - @{ - */ - -#if defined (__Vendor_SysTickConfig) && (__Vendor_SysTickConfig == 0U) - -/** - \brief System Tick Configuration - \details Initializes the System Timer and its interrupt, and starts the System Tick Timer. - Counter is in free running mode to generate periodic interrupts. - \param [in] ticks Number of ticks between two interrupts. - \return 0 Function succeeded. - \return 1 Function failed. - \note When the variable __Vendor_SysTickConfig is set to 1, then the - function SysTick_Config is not included. In this case, the file device.h - must contain a vendor-specific implementation of this function. - */ -__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks) -{ - if ((ticks - 1UL) > SysTick_LOAD_RELOAD_Msk) - { - return (1UL); /* Reload value impossible */ - } - - SysTick->LOAD = (uint32_t)(ticks - 1UL); /* set reload register */ - NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */ - SysTick->VAL = 0UL; /* Load the SysTick Counter Value */ - SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk | - SysTick_CTRL_TICKINT_Msk | - SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ - return (0UL); /* Function successful */ -} - -#endif - -/*@} end of CMSIS_Core_SysTickFunctions */ - - - -/* ##################################### Debug In/Output function ########################################### */ -/** - \ingroup CMSIS_Core_FunctionInterface - \defgroup CMSIS_core_DebugFunctions ITM Functions - \brief Functions that access the ITM debug interface. - @{ - */ - -extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */ -#define ITM_RXBUFFER_EMPTY ((int32_t)0x5AA55AA5U) /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */ - - -/** - \brief ITM Send Character - \details Transmits a character via the ITM channel 0, and - \li Just returns when no debugger is connected that has booked the output. - \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted. - \param [in] ch Character to transmit. - \returns Character to transmit. - */ -__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch) -{ - if (((ITM->TCR & ITM_TCR_ITMENA_Msk) != 0UL) && /* ITM enabled */ - ((ITM->TER & 1UL ) != 0UL) ) /* ITM Port #0 enabled */ - { - while (ITM->PORT[0U].u32 == 0UL) - { - __NOP(); - } - ITM->PORT[0U].u8 = (uint8_t)ch; - } - return (ch); -} - - -/** - \brief ITM Receive Character - \details Inputs a character via the external variable \ref ITM_RxBuffer. - \return Received character. - \return -1 No character pending. - */ -__STATIC_INLINE int32_t ITM_ReceiveChar (void) -{ - int32_t ch = -1; /* no character available */ - - if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) - { - ch = ITM_RxBuffer; - ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */ - } - - return (ch); -} - - -/** - \brief ITM Check Character - \details Checks whether a character is pending for reading in the variable \ref ITM_RxBuffer. - \return 0 No character available. - \return 1 Character available. - */ -__STATIC_INLINE int32_t ITM_CheckChar (void) -{ - - if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) - { - return (0); /* no character available */ - } - else - { - return (1); /* character available */ - } -} - -/*@} end of CMSIS_core_DebugFunctions */ - - - - -#ifdef __cplusplus -} -#endif - -#endif /* __CORE_CM3_H_DEPENDANT */ - -#endif /* __CMSIS_GENERIC */ - diff --git a/panda/board/stm32fx/llexti.h b/panda/board/stm32fx/llexti.h deleted file mode 100644 index 6de13ab7f4..0000000000 --- a/panda/board/stm32fx/llexti.h +++ /dev/null @@ -1,56 +0,0 @@ -void EXTI_IRQ_Handler(void); - -void exti_irq_init(void) { - SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI8_Msk); - if (harness.status == HARNESS_STATUS_FLIPPED) { - // CAN2_RX - current_board->enable_can_transceiver(3U, false); - SYSCFG->EXTICR[2] |= (SYSCFG_EXTICR3_EXTI8_PA); - - // IRQ on falling edge for PC3 (SBU2, EXTI3) - SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI3_Msk); - SYSCFG->EXTICR[0] |= (SYSCFG_EXTICR1_EXTI3_PC); - EXTI->IMR |= EXTI_IMR_MR3; - EXTI->RTSR &= ~EXTI_RTSR_TR3; // rising edge - EXTI->FTSR |= EXTI_FTSR_TR3; // falling edge - REGISTER_INTERRUPT(EXTI3_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI3_IRQn); - } else { - // CAN0_RX - current_board->enable_can_transceiver(1U, false); - SYSCFG->EXTICR[2] |= (SYSCFG_EXTICR3_EXTI8_PB); - - // IRQ on falling edge for PC0 (SBU1, EXTI0) - SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI0_Msk); - SYSCFG->EXTICR[0] |= (SYSCFG_EXTICR1_EXTI0_PC); - EXTI->IMR |= EXTI_IMR_MR0; - EXTI->RTSR &= ~EXTI_RTSR_TR0; // rising edge - EXTI->FTSR |= EXTI_FTSR_TR0; // falling edge - REGISTER_INTERRUPT(EXTI0_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI0_IRQn); - } - // CAN0 or CAN2 IRQ on falling edge (EXTI8) - EXTI->IMR |= EXTI_IMR_MR8; - EXTI->RTSR &= ~EXTI_RTSR_TR8; // rising edge - EXTI->FTSR |= EXTI_FTSR_TR8; // falling edge - REGISTER_INTERRUPT(EXTI9_5_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI9_5_IRQn); -} - -bool check_exti_irq(void) { - return ((EXTI->PR & EXTI_PR_PR8) || (EXTI->PR & EXTI_PR_PR3) || (EXTI->PR & EXTI_PR_PR0)); -} - -void exti_irq_clear(void) { - // Clear pending bits - EXTI->PR |= EXTI_PR_PR8; - EXTI->PR |= EXTI_PR_PR0; - EXTI->PR |= EXTI_PR_PR3; - EXTI->PR |= EXTI_PR_PR22; - - // Disable all active EXTI IRQs - EXTI->IMR &= ~EXTI_IMR_MR8; - EXTI->IMR &= ~EXTI_IMR_MR0; - EXTI->IMR &= ~EXTI_IMR_MR3; - EXTI->IMR &= ~EXTI_IMR_MR22; -} diff --git a/panda/board/stm32fx/llrtc.h b/panda/board/stm32fx/llrtc.h deleted file mode 100644 index a9b61917e7..0000000000 --- a/panda/board/stm32fx/llrtc.h +++ /dev/null @@ -1,69 +0,0 @@ -void enable_bdomain_protection(void) { - register_clear_bits(&(PWR->CR), PWR_CR_DBP); -} - -void disable_bdomain_protection(void) { - register_set_bits(&(PWR->CR), PWR_CR_DBP); -} - -void rtc_init(void){ - uint32_t bdcr_opts = RCC_BDCR_RTCEN; - uint32_t bdcr_mask = (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL); - if (current_board->has_rtc_battery) { - bdcr_opts |= (RCC_BDCR_RTCSEL_0 | RCC_BDCR_LSEON); - bdcr_mask |= (RCC_BDCR_LSEMOD | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON); - } else { - bdcr_opts |= RCC_BDCR_RTCSEL_1; - RCC->CSR |= RCC_CSR_LSION; - while((RCC->CSR & RCC_CSR_LSIRDY) == 0){} - } - - // Initialize RTC module and clock if not done already. - if((RCC->BDCR & bdcr_mask) != bdcr_opts){ - print("Initializing RTC\n"); - // Reset backup domain - register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Disable write protection - disable_bdomain_protection(); - - // Clear backup domain reset - register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Set RTC options - register_set(&(RCC->BDCR), bdcr_opts, bdcr_mask); - - // Enable write protection - enable_bdomain_protection(); - } -} - -void rtc_wakeup_init(void) { - EXTI->IMR |= EXTI_IMR_MR22; - EXTI->RTSR |= EXTI_RTSR_TR22; // rising edge - EXTI->FTSR &= ~EXTI_FTSR_TR22; // falling edge - - NVIC_DisableIRQ(RTC_WKUP_IRQn); - - // Disable write protection - disable_bdomain_protection(); - RTC->WPR = 0xCA; - RTC->WPR = 0x53; - - RTC->CR &= ~RTC_CR_WUTE; - while((RTC->ISR & RTC_ISR_WUTWF) == 0){} - - RTC->CR &= ~RTC_CR_WUTIE; - RTC->ISR &= ~RTC_ISR_WUTF; - //PWR->CR |= PWR_CR_CWUF; - - RTC->WUTR = DEEPSLEEP_WAKEUP_DELAY; - // Wakeup timer interrupt enable, wakeup timer enable, select 1Hz rate - RTC->CR |= RTC_CR_WUTE | RTC_CR_WUTIE | RTC_CR_WUCKSEL_2; - - // Re-enable write protection - RTC->WPR = 0x00; - enable_bdomain_protection(); - - NVIC_EnableIRQ(RTC_WKUP_IRQn); -} diff --git a/panda/board/stm32h7/board.h b/panda/board/stm32h7/board.h index d293a54cfe..f5a8e55aa8 100644 --- a/panda/board/stm32h7/board.h +++ b/panda/board/stm32h7/board.h @@ -9,10 +9,8 @@ #include "drivers/harness.h" #include "drivers/fan.h" #include "stm32h7/llfan.h" -#include "stm32h7/llrtc.h" #include "stm32h7/lldac.h" #include "drivers/fake_siren.h" -#include "drivers/rtc.h" #include "drivers/clock_source.h" #include "boards/red.h" #include "boards/red_chiplet.h" diff --git a/panda/board/stm32h7/clock.h b/panda/board/stm32h7/clock.h index 7d65d080ec..2e3ab701d8 100644 --- a/panda/board/stm32h7/clock.h +++ b/panda/board/stm32h7/clock.h @@ -21,20 +21,22 @@ void clock_init(void) { // Set power mode to direct SMPS power supply(depends on the board layout) #ifndef STM32H723 register_set(&(PWR->CR3), PWR_CR3_SMPSEN, 0xFU); // powered only by SMPS +#else + register_set(&(PWR->CR3), PWR_CR3_LDOEN, 0xFU); #endif // Set VOS level (VOS3 to 170Mhz, VOS2 to 300Mhz, VOS1 to 400Mhz, VOS0 to 550Mhz) register_set(&(PWR->D3CR), PWR_D3CR_VOS_1 | PWR_D3CR_VOS_0, 0xC000U); //VOS1, needed for 80Mhz CAN FD - while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0); + while ((PWR->CSR1 & PWR_CSR1_ACTVOSRDY) == 0U); while ((PWR->CSR1 & PWR_CSR1_ACTVOS) != (PWR->D3CR & PWR_D3CR_VOS)); // check that VOS level was actually set // Configure Flash ACR register LATENCY and WRHIGHFREQ (VOS0 range!) register_set(&(FLASH->ACR), FLASH_ACR_LATENCY_2WS | 0x20U, 0x3FU); // VOS2, AXI 100MHz-150MHz // enable external oscillator HSE register_set_bits(&(RCC->CR), RCC_CR_HSEON); - while ((RCC->CR & RCC_CR_HSERDY) == 0); + while ((RCC->CR & RCC_CR_HSERDY) == 0U); // enable internal HSI48 for USB FS kernel register_set_bits(&(RCC->CR), RCC_CR_HSI48ON); - while ((RCC->CR & RCC_CR_HSI48RDY) == 0); + while ((RCC->CR & RCC_CR_HSI48RDY) == 0U); // Specify the frequency source for PLL1, divider for DIVM1, DIVM2, DIVM3 : HSE, 5, 5, 5 register_set(&(RCC->PLLCKSELR), RCC_PLLCKSELR_PLLSRC_HSE | RCC_PLLCKSELR_DIVM1_0 | RCC_PLLCKSELR_DIVM1_2 | RCC_PLLCKSELR_DIVM2_0 | RCC_PLLCKSELR_DIVM2_2 | RCC_PLLCKSELR_DIVM3_0 | RCC_PLLCKSELR_DIVM3_2, 0x3F3F3F3U); @@ -45,7 +47,7 @@ void clock_init(void) { register_set(&(RCC->PLLCFGR), RCC_PLLCFGR_PLL1RGE_2 | RCC_PLLCFGR_DIVP1EN | RCC_PLLCFGR_DIVQ1EN | RCC_PLLCFGR_DIVR1EN, 0x7000CU); // Enable PLL1 register_set_bits(&(RCC->CR), RCC_CR_PLL1ON); - while((RCC->CR & RCC_CR_PLL1RDY) == 0); + while((RCC->CR & RCC_CR_PLL1RDY) == 0U); // *** PLL1 end *** //////////////OTHER CLOCKS//////////////////// diff --git a/panda/board/stm32h7/inc/mpu_armv7.h b/panda/board/stm32h7/inc/mpu_armv7.h new file mode 100644 index 0000000000..e72cc4623d --- /dev/null +++ b/panda/board/stm32h7/inc/mpu_armv7.h @@ -0,0 +1,273 @@ +/****************************************************************************** + * @file mpu_armv7.h + * @brief CMSIS MPU API for Armv7-M MPU + * @version V5.1.0 + * @date 08. March 2019 + ******************************************************************************/ +/* + * Copyright (c) 2017-2019 Arm Limited. All rights reserved. + * + * SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the License); you may + * not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an AS IS BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#if defined ( __ICCARM__ ) + #pragma system_include /* treat file as system include file for MISRA check */ +#elif defined (__clang__) + #pragma clang system_header /* treat file as system include file */ +#endif + +#ifndef ARM_MPU_ARMV7_H +#define ARM_MPU_ARMV7_H + +#define ARM_MPU_REGION_SIZE_32B ((uint8_t)0x04U) ///!< MPU Region Size 32 Bytes +#define ARM_MPU_REGION_SIZE_64B ((uint8_t)0x05U) ///!< MPU Region Size 64 Bytes +#define ARM_MPU_REGION_SIZE_128B ((uint8_t)0x06U) ///!< MPU Region Size 128 Bytes +#define ARM_MPU_REGION_SIZE_256B ((uint8_t)0x07U) ///!< MPU Region Size 256 Bytes +#define ARM_MPU_REGION_SIZE_512B ((uint8_t)0x08U) ///!< MPU Region Size 512 Bytes +#define ARM_MPU_REGION_SIZE_1KB ((uint8_t)0x09U) ///!< MPU Region Size 1 KByte +#define ARM_MPU_REGION_SIZE_2KB ((uint8_t)0x0AU) ///!< MPU Region Size 2 KBytes +#define ARM_MPU_REGION_SIZE_4KB ((uint8_t)0x0BU) ///!< MPU Region Size 4 KBytes +#define ARM_MPU_REGION_SIZE_8KB ((uint8_t)0x0CU) ///!< MPU Region Size 8 KBytes +#define ARM_MPU_REGION_SIZE_16KB ((uint8_t)0x0DU) ///!< MPU Region Size 16 KBytes +#define ARM_MPU_REGION_SIZE_32KB ((uint8_t)0x0EU) ///!< MPU Region Size 32 KBytes +#define ARM_MPU_REGION_SIZE_64KB ((uint8_t)0x0FU) ///!< MPU Region Size 64 KBytes +#define ARM_MPU_REGION_SIZE_128KB ((uint8_t)0x10U) ///!< MPU Region Size 128 KBytes +#define ARM_MPU_REGION_SIZE_256KB ((uint8_t)0x11U) ///!< MPU Region Size 256 KBytes +#define ARM_MPU_REGION_SIZE_512KB ((uint8_t)0x12U) ///!< MPU Region Size 512 KBytes +#define ARM_MPU_REGION_SIZE_1MB ((uint8_t)0x13U) ///!< MPU Region Size 1 MByte +#define ARM_MPU_REGION_SIZE_2MB ((uint8_t)0x14U) ///!< MPU Region Size 2 MBytes +#define ARM_MPU_REGION_SIZE_4MB ((uint8_t)0x15U) ///!< MPU Region Size 4 MBytes +#define ARM_MPU_REGION_SIZE_8MB ((uint8_t)0x16U) ///!< MPU Region Size 8 MBytes +#define ARM_MPU_REGION_SIZE_16MB ((uint8_t)0x17U) ///!< MPU Region Size 16 MBytes +#define ARM_MPU_REGION_SIZE_32MB ((uint8_t)0x18U) ///!< MPU Region Size 32 MBytes +#define ARM_MPU_REGION_SIZE_64MB ((uint8_t)0x19U) ///!< MPU Region Size 64 MBytes +#define ARM_MPU_REGION_SIZE_128MB ((uint8_t)0x1AU) ///!< MPU Region Size 128 MBytes +#define ARM_MPU_REGION_SIZE_256MB ((uint8_t)0x1BU) ///!< MPU Region Size 256 MBytes +#define ARM_MPU_REGION_SIZE_512MB ((uint8_t)0x1CU) ///!< MPU Region Size 512 MBytes +#define ARM_MPU_REGION_SIZE_1GB ((uint8_t)0x1DU) ///!< MPU Region Size 1 GByte +#define ARM_MPU_REGION_SIZE_2GB ((uint8_t)0x1EU) ///!< MPU Region Size 2 GBytes +#define ARM_MPU_REGION_SIZE_4GB ((uint8_t)0x1FU) ///!< MPU Region Size 4 GBytes + +#define ARM_MPU_AP_NONE 0U ///!< MPU Access Permission no access +#define ARM_MPU_AP_PRIV 1U ///!< MPU Access Permission privileged access only +#define ARM_MPU_AP_URO 2U ///!< MPU Access Permission unprivileged access read-only +#define ARM_MPU_AP_FULL 3U ///!< MPU Access Permission full access +#define ARM_MPU_AP_PRO 5U ///!< MPU Access Permission privileged access read-only +#define ARM_MPU_AP_RO 6U ///!< MPU Access Permission read-only access + +/** MPU Region Base Address Register Value +* +* \param Region The region to be configured, number 0 to 15. +* \param BaseAddress The base address for the region. +*/ +#define ARM_MPU_RBAR(Region, BaseAddress) \ + (((BaseAddress) & MPU_RBAR_ADDR_Msk) | \ + ((Region) & MPU_RBAR_REGION_Msk) | \ + (MPU_RBAR_VALID_Msk)) + +/** +* MPU Memory Access Attributes +* +* \param TypeExtField Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral. +* \param IsShareable Region is shareable between multiple bus masters. +* \param IsCacheable Region is cacheable, i.e. its value may be kept in cache. +* \param IsBufferable Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy. +*/ +#define ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable) \ + ((((TypeExtField) << MPU_RASR_TEX_Pos) & MPU_RASR_TEX_Msk) | \ + (((IsShareable) << MPU_RASR_S_Pos) & MPU_RASR_S_Msk) | \ + (((IsCacheable) << MPU_RASR_C_Pos) & MPU_RASR_C_Msk) | \ + (((IsBufferable) << MPU_RASR_B_Pos) & MPU_RASR_B_Msk)) + +/** +* MPU Region Attribute and Size Register Value +* +* \param DisableExec Instruction access disable bit, 1= disable instruction fetches. +* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode. +* \param AccessAttributes Memory access attribution, see \ref ARM_MPU_ACCESS_. +* \param SubRegionDisable Sub-region disable field. +* \param Size Region size of the region to be configured, for example 4K, 8K. +*/ +#define ARM_MPU_RASR_EX(DisableExec, AccessPermission, AccessAttributes, SubRegionDisable, Size) \ + ((((DisableExec) << MPU_RASR_XN_Pos) & MPU_RASR_XN_Msk) | \ + (((AccessPermission) << MPU_RASR_AP_Pos) & MPU_RASR_AP_Msk) | \ + (((AccessAttributes) & (MPU_RASR_TEX_Msk | MPU_RASR_S_Msk | MPU_RASR_C_Msk | MPU_RASR_B_Msk))) | \ + (((SubRegionDisable) << MPU_RASR_SRD_Pos) & MPU_RASR_SRD_Msk) | \ + (((Size) << MPU_RASR_SIZE_Pos) & MPU_RASR_SIZE_Msk) | \ + (((MPU_RASR_ENABLE_Msk)))) + +/** +* MPU Region Attribute and Size Register Value +* +* \param DisableExec Instruction access disable bit, 1= disable instruction fetches. +* \param AccessPermission Data access permissions, allows you to configure read/write access for User and Privileged mode. +* \param TypeExtField Type extension field, allows you to configure memory access type, for example strongly ordered, peripheral. +* \param IsShareable Region is shareable between multiple bus masters. +* \param IsCacheable Region is cacheable, i.e. its value may be kept in cache. +* \param IsBufferable Region is bufferable, i.e. using write-back caching. Cacheable but non-bufferable regions use write-through policy. +* \param SubRegionDisable Sub-region disable field. +* \param Size Region size of the region to be configured, for example 4K, 8K. +*/ +#define ARM_MPU_RASR(DisableExec, AccessPermission, TypeExtField, IsShareable, IsCacheable, IsBufferable, SubRegionDisable, Size) \ + ARM_MPU_RASR_EX(DisableExec, AccessPermission, ARM_MPU_ACCESS_(TypeExtField, IsShareable, IsCacheable, IsBufferable), SubRegionDisable, Size) + +/** +* MPU Memory Access Attribute for strongly ordered memory. +* - TEX: 000b +* - Shareable +* - Non-cacheable +* - Non-bufferable +*/ +#define ARM_MPU_ACCESS_ORDERED ARM_MPU_ACCESS_(0U, 1U, 0U, 0U) + +/** +* MPU Memory Access Attribute for device memory. +* - TEX: 000b (if shareable) or 010b (if non-shareable) +* - Shareable or non-shareable +* - Non-cacheable +* - Bufferable (if shareable) or non-bufferable (if non-shareable) +* +* \param IsShareable Configures the device memory as shareable or non-shareable. +*/ +#define ARM_MPU_ACCESS_DEVICE(IsShareable) ((IsShareable) ? ARM_MPU_ACCESS_(0U, 1U, 0U, 1U) : ARM_MPU_ACCESS_(2U, 0U, 0U, 0U)) + +/** +* MPU Memory Access Attribute for normal memory. +* - TEX: 1BBb (reflecting outer cacheability rules) +* - Shareable or non-shareable +* - Cacheable or non-cacheable (reflecting inner cacheability rules) +* - Bufferable or non-bufferable (reflecting inner cacheability rules) +* +* \param OuterCp Configures the outer cache policy. +* \param InnerCp Configures the inner cache policy. +* \param IsShareable Configures the memory as shareable or non-shareable. +*/ +#define ARM_MPU_ACCESS_NORMAL(OuterCp, InnerCp, IsShareable) ARM_MPU_ACCESS_((4U | (OuterCp)), IsShareable, ((InnerCp) & 2U), ((InnerCp) & 1U)) + +/** +* MPU Memory Access Attribute non-cacheable policy. +*/ +#define ARM_MPU_CACHEP_NOCACHE 0U + +/** +* MPU Memory Access Attribute write-back, write and read allocate policy. +*/ +#define ARM_MPU_CACHEP_WB_WRA 1U + +/** +* MPU Memory Access Attribute write-through, no write allocate policy. +*/ +#define ARM_MPU_CACHEP_WT_NWA 2U + +/** +* MPU Memory Access Attribute write-back, no write allocate policy. +*/ +#define ARM_MPU_CACHEP_WB_NWA 3U + + +/** +* Struct for a single MPU Region +*/ +typedef struct { + uint32_t RBAR; //!< The region base address register value (RBAR) + uint32_t RASR; //!< The region attribute and size register value (RASR) \ref MPU_RASR +} ARM_MPU_Region_t; + +/** Enable the MPU. +* \param MPU_Control Default access permissions for unconfigured regions. +*/ +__STATIC_INLINE void ARM_MPU_Enable(uint32_t MPU_Control) +{ + MPU->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk; +#ifdef SCB_SHCSR_MEMFAULTENA_Msk + SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk; +#endif + __DSB(); + __ISB(); +} + +/** Disable the MPU. +*/ +__STATIC_INLINE void ARM_MPU_Disable(void) +{ + __DMB(); +#ifdef SCB_SHCSR_MEMFAULTENA_Msk + SCB->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk; +#endif + MPU->CTRL &= ~MPU_CTRL_ENABLE_Msk; +} + +/** Clear and disable the given MPU region. +* \param rnr Region number to be cleared. +*/ +__STATIC_INLINE void ARM_MPU_ClrRegion(uint32_t rnr) +{ + MPU->RNR = rnr; + MPU->RASR = 0U; +} + +/** Configure an MPU region. +* \param rbar Value for RBAR register. +* \param rsar Value for RSAR register. +*/ +__STATIC_INLINE void ARM_MPU_SetRegion(uint32_t rbar, uint32_t rasr) +{ + MPU->RBAR = rbar; + MPU->RASR = rasr; +} + +/** Configure the given MPU region. +* \param rnr Region number to be configured. +* \param rbar Value for RBAR register. +* \param rsar Value for RSAR register. +*/ +__STATIC_INLINE void ARM_MPU_SetRegionEx(uint32_t rnr, uint32_t rbar, uint32_t rasr) +{ + MPU->RNR = rnr; + MPU->RBAR = rbar; + MPU->RASR = rasr; +} + +/** Memcopy with strictly ordered memory access, e.g. for register targets. +* \param dst Destination data is copied to. +* \param src Source data is copied from. +* \param len Amount of data words to be copied. +*/ +__STATIC_INLINE void ARM_MPU_OrderedMemcpy(volatile uint32_t* dst, const uint32_t* __RESTRICT src, uint32_t len) +{ + uint32_t i; + for (i = 0U; i < len; ++i) + { + dst[i] = src[i]; + } +} + +/** Load the given number of MPU regions from a table. +* \param table Pointer to the MPU configuration table. +* \param cnt Amount of regions to be configured. +*/ +__STATIC_INLINE void ARM_MPU_Load(ARM_MPU_Region_t const* table, uint32_t cnt) +{ + const uint32_t rowWordSize = sizeof(ARM_MPU_Region_t)/4U; + while (cnt > MPU_TYPE_RALIASES) { + ARM_MPU_OrderedMemcpy(&(MPU->RBAR), &(table->RBAR), MPU_TYPE_RALIASES*rowWordSize); + table += MPU_TYPE_RALIASES; + cnt -= MPU_TYPE_RALIASES; + } + ARM_MPU_OrderedMemcpy(&(MPU->RBAR), &(table->RBAR), cnt*rowWordSize); +} + +#endif + diff --git a/panda/board/stm32h7/inc/mpu_armv8.h b/panda/board/stm32h7/inc/mpu_armv8.h deleted file mode 100644 index 2fe28b687f..0000000000 --- a/panda/board/stm32h7/inc/mpu_armv8.h +++ /dev/null @@ -1,346 +0,0 @@ -/****************************************************************************** - * @file mpu_armv8.h - * @brief CMSIS MPU API for Armv8-M and Armv8.1-M MPU - * @version V5.1.0 - * @date 08. March 2019 - ******************************************************************************/ -/* - * Copyright (c) 2017-2019 Arm Limited. All rights reserved. - * - * SPDX-License-Identifier: Apache-2.0 - * - * Licensed under the Apache License, Version 2.0 (the License); you may - * not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an AS IS BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#if defined ( __ICCARM__ ) - #pragma system_include /* treat file as system include file for MISRA check */ -#elif defined (__clang__) - #pragma clang system_header /* treat file as system include file */ -#endif - -#ifndef ARM_MPU_ARMV8_H -#define ARM_MPU_ARMV8_H - -/** \brief Attribute for device memory (outer only) */ -#define ARM_MPU_ATTR_DEVICE ( 0U ) - -/** \brief Attribute for non-cacheable, normal memory */ -#define ARM_MPU_ATTR_NON_CACHEABLE ( 4U ) - -/** \brief Attribute for normal memory (outer and inner) -* \param NT Non-Transient: Set to 1 for non-transient data. -* \param WB Write-Back: Set to 1 to use write-back update policy. -* \param RA Read Allocation: Set to 1 to use cache allocation on read miss. -* \param WA Write Allocation: Set to 1 to use cache allocation on write miss. -*/ -#define ARM_MPU_ATTR_MEMORY_(NT, WB, RA, WA) \ - (((NT & 1U) << 3U) | ((WB & 1U) << 2U) | ((RA & 1U) << 1U) | (WA & 1U)) - -/** \brief Device memory type non Gathering, non Re-ordering, non Early Write Acknowledgement */ -#define ARM_MPU_ATTR_DEVICE_nGnRnE (0U) - -/** \brief Device memory type non Gathering, non Re-ordering, Early Write Acknowledgement */ -#define ARM_MPU_ATTR_DEVICE_nGnRE (1U) - -/** \brief Device memory type non Gathering, Re-ordering, Early Write Acknowledgement */ -#define ARM_MPU_ATTR_DEVICE_nGRE (2U) - -/** \brief Device memory type Gathering, Re-ordering, Early Write Acknowledgement */ -#define ARM_MPU_ATTR_DEVICE_GRE (3U) - -/** \brief Memory Attribute -* \param O Outer memory attributes -* \param I O == ARM_MPU_ATTR_DEVICE: Device memory attributes, else: Inner memory attributes -*/ -#define ARM_MPU_ATTR(O, I) (((O & 0xFU) << 4U) | (((O & 0xFU) != 0U) ? (I & 0xFU) : ((I & 0x3U) << 2U))) - -/** \brief Normal memory non-shareable */ -#define ARM_MPU_SH_NON (0U) - -/** \brief Normal memory outer shareable */ -#define ARM_MPU_SH_OUTER (2U) - -/** \brief Normal memory inner shareable */ -#define ARM_MPU_SH_INNER (3U) - -/** \brief Memory access permissions -* \param RO Read-Only: Set to 1 for read-only memory. -* \param NP Non-Privileged: Set to 1 for non-privileged memory. -*/ -#define ARM_MPU_AP_(RO, NP) (((RO & 1U) << 1U) | (NP & 1U)) - -/** \brief Region Base Address Register value -* \param BASE The base address bits [31:5] of a memory region. The value is zero extended. Effective address gets 32 byte aligned. -* \param SH Defines the Shareability domain for this memory region. -* \param RO Read-Only: Set to 1 for a read-only memory region. -* \param NP Non-Privileged: Set to 1 for a non-privileged memory region. -* \oaram XN eXecute Never: Set to 1 for a non-executable memory region. -*/ -#define ARM_MPU_RBAR(BASE, SH, RO, NP, XN) \ - ((BASE & MPU_RBAR_BASE_Msk) | \ - ((SH << MPU_RBAR_SH_Pos) & MPU_RBAR_SH_Msk) | \ - ((ARM_MPU_AP_(RO, NP) << MPU_RBAR_AP_Pos) & MPU_RBAR_AP_Msk) | \ - ((XN << MPU_RBAR_XN_Pos) & MPU_RBAR_XN_Msk)) - -/** \brief Region Limit Address Register value -* \param LIMIT The limit address bits [31:5] for this memory region. The value is one extended. -* \param IDX The attribute index to be associated with this memory region. -*/ -#define ARM_MPU_RLAR(LIMIT, IDX) \ - ((LIMIT & MPU_RLAR_LIMIT_Msk) | \ - ((IDX << MPU_RLAR_AttrIndx_Pos) & MPU_RLAR_AttrIndx_Msk) | \ - (MPU_RLAR_EN_Msk)) - -#if defined(MPU_RLAR_PXN_Pos) - -/** \brief Region Limit Address Register with PXN value -* \param LIMIT The limit address bits [31:5] for this memory region. The value is one extended. -* \param PXN Privileged execute never. Defines whether code can be executed from this privileged region. -* \param IDX The attribute index to be associated with this memory region. -*/ -#define ARM_MPU_RLAR_PXN(LIMIT, PXN, IDX) \ - ((LIMIT & MPU_RLAR_LIMIT_Msk) | \ - ((PXN << MPU_RLAR_PXN_Pos) & MPU_RLAR_PXN_Msk) | \ - ((IDX << MPU_RLAR_AttrIndx_Pos) & MPU_RLAR_AttrIndx_Msk) | \ - (MPU_RLAR_EN_Msk)) - -#endif - -/** -* Struct for a single MPU Region -*/ -typedef struct { - uint32_t RBAR; /*!< Region Base Address Register value */ - uint32_t RLAR; /*!< Region Limit Address Register value */ -} ARM_MPU_Region_t; - -/** Enable the MPU. -* \param MPU_Control Default access permissions for unconfigured regions. -*/ -__STATIC_INLINE void ARM_MPU_Enable(uint32_t MPU_Control) -{ - MPU->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk; -#ifdef SCB_SHCSR_MEMFAULTENA_Msk - SCB->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk; -#endif - __DSB(); - __ISB(); -} - -/** Disable the MPU. -*/ -__STATIC_INLINE void ARM_MPU_Disable(void) -{ - __DMB(); -#ifdef SCB_SHCSR_MEMFAULTENA_Msk - SCB->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk; -#endif - MPU->CTRL &= ~MPU_CTRL_ENABLE_Msk; -} - -#ifdef MPU_NS -/** Enable the Non-secure MPU. -* \param MPU_Control Default access permissions for unconfigured regions. -*/ -__STATIC_INLINE void ARM_MPU_Enable_NS(uint32_t MPU_Control) -{ - MPU_NS->CTRL = MPU_Control | MPU_CTRL_ENABLE_Msk; -#ifdef SCB_SHCSR_MEMFAULTENA_Msk - SCB_NS->SHCSR |= SCB_SHCSR_MEMFAULTENA_Msk; -#endif - __DSB(); - __ISB(); -} - -/** Disable the Non-secure MPU. -*/ -__STATIC_INLINE void ARM_MPU_Disable_NS(void) -{ - __DMB(); -#ifdef SCB_SHCSR_MEMFAULTENA_Msk - SCB_NS->SHCSR &= ~SCB_SHCSR_MEMFAULTENA_Msk; -#endif - MPU_NS->CTRL &= ~MPU_CTRL_ENABLE_Msk; -} -#endif - -/** Set the memory attribute encoding to the given MPU. -* \param mpu Pointer to the MPU to be configured. -* \param idx The attribute index to be set [0-7] -* \param attr The attribute value to be set. -*/ -__STATIC_INLINE void ARM_MPU_SetMemAttrEx(MPU_Type* mpu, uint8_t idx, uint8_t attr) -{ - const uint8_t reg = idx / 4U; - const uint32_t pos = ((idx % 4U) * 8U); - const uint32_t mask = 0xFFU << pos; - - if (reg >= (sizeof(mpu->MAIR) / sizeof(mpu->MAIR[0]))) { - return; // invalid index - } - - mpu->MAIR[reg] = ((mpu->MAIR[reg] & ~mask) | ((attr << pos) & mask)); -} - -/** Set the memory attribute encoding. -* \param idx The attribute index to be set [0-7] -* \param attr The attribute value to be set. -*/ -__STATIC_INLINE void ARM_MPU_SetMemAttr(uint8_t idx, uint8_t attr) -{ - ARM_MPU_SetMemAttrEx(MPU, idx, attr); -} - -#ifdef MPU_NS -/** Set the memory attribute encoding to the Non-secure MPU. -* \param idx The attribute index to be set [0-7] -* \param attr The attribute value to be set. -*/ -__STATIC_INLINE void ARM_MPU_SetMemAttr_NS(uint8_t idx, uint8_t attr) -{ - ARM_MPU_SetMemAttrEx(MPU_NS, idx, attr); -} -#endif - -/** Clear and disable the given MPU region of the given MPU. -* \param mpu Pointer to MPU to be used. -* \param rnr Region number to be cleared. -*/ -__STATIC_INLINE void ARM_MPU_ClrRegionEx(MPU_Type* mpu, uint32_t rnr) -{ - mpu->RNR = rnr; - mpu->RLAR = 0U; -} - -/** Clear and disable the given MPU region. -* \param rnr Region number to be cleared. -*/ -__STATIC_INLINE void ARM_MPU_ClrRegion(uint32_t rnr) -{ - ARM_MPU_ClrRegionEx(MPU, rnr); -} - -#ifdef MPU_NS -/** Clear and disable the given Non-secure MPU region. -* \param rnr Region number to be cleared. -*/ -__STATIC_INLINE void ARM_MPU_ClrRegion_NS(uint32_t rnr) -{ - ARM_MPU_ClrRegionEx(MPU_NS, rnr); -} -#endif - -/** Configure the given MPU region of the given MPU. -* \param mpu Pointer to MPU to be used. -* \param rnr Region number to be configured. -* \param rbar Value for RBAR register. -* \param rlar Value for RLAR register. -*/ -__STATIC_INLINE void ARM_MPU_SetRegionEx(MPU_Type* mpu, uint32_t rnr, uint32_t rbar, uint32_t rlar) -{ - mpu->RNR = rnr; - mpu->RBAR = rbar; - mpu->RLAR = rlar; -} - -/** Configure the given MPU region. -* \param rnr Region number to be configured. -* \param rbar Value for RBAR register. -* \param rlar Value for RLAR register. -*/ -__STATIC_INLINE void ARM_MPU_SetRegion(uint32_t rnr, uint32_t rbar, uint32_t rlar) -{ - ARM_MPU_SetRegionEx(MPU, rnr, rbar, rlar); -} - -#ifdef MPU_NS -/** Configure the given Non-secure MPU region. -* \param rnr Region number to be configured. -* \param rbar Value for RBAR register. -* \param rlar Value for RLAR register. -*/ -__STATIC_INLINE void ARM_MPU_SetRegion_NS(uint32_t rnr, uint32_t rbar, uint32_t rlar) -{ - ARM_MPU_SetRegionEx(MPU_NS, rnr, rbar, rlar); -} -#endif - -/** Memcopy with strictly ordered memory access, e.g. for register targets. -* \param dst Destination data is copied to. -* \param src Source data is copied from. -* \param len Amount of data words to be copied. -*/ -__STATIC_INLINE void ARM_MPU_OrderedMemcpy(volatile uint32_t* dst, const uint32_t* __RESTRICT src, uint32_t len) -{ - uint32_t i; - for (i = 0U; i < len; ++i) - { - dst[i] = src[i]; - } -} - -/** Load the given number of MPU regions from a table to the given MPU. -* \param mpu Pointer to the MPU registers to be used. -* \param rnr First region number to be configured. -* \param table Pointer to the MPU configuration table. -* \param cnt Amount of regions to be configured. -*/ -__STATIC_INLINE void ARM_MPU_LoadEx(MPU_Type* mpu, uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) -{ - const uint32_t rowWordSize = sizeof(ARM_MPU_Region_t)/4U; - if (cnt == 1U) { - mpu->RNR = rnr; - ARM_MPU_OrderedMemcpy(&(mpu->RBAR), &(table->RBAR), rowWordSize); - } else { - uint32_t rnrBase = rnr & ~(MPU_TYPE_RALIASES-1U); - uint32_t rnrOffset = rnr % MPU_TYPE_RALIASES; - - mpu->RNR = rnrBase; - while ((rnrOffset + cnt) > MPU_TYPE_RALIASES) { - uint32_t c = MPU_TYPE_RALIASES - rnrOffset; - ARM_MPU_OrderedMemcpy(&(mpu->RBAR)+(rnrOffset*2U), &(table->RBAR), c*rowWordSize); - table += c; - cnt -= c; - rnrOffset = 0U; - rnrBase += MPU_TYPE_RALIASES; - mpu->RNR = rnrBase; - } - - ARM_MPU_OrderedMemcpy(&(mpu->RBAR)+(rnrOffset*2U), &(table->RBAR), cnt*rowWordSize); - } -} - -/** Load the given number of MPU regions from a table. -* \param rnr First region number to be configured. -* \param table Pointer to the MPU configuration table. -* \param cnt Amount of regions to be configured. -*/ -__STATIC_INLINE void ARM_MPU_Load(uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) -{ - ARM_MPU_LoadEx(MPU, rnr, table, cnt); -} - -#ifdef MPU_NS -/** Load the given number of MPU regions from a table to the Non-secure MPU. -* \param rnr First region number to be configured. -* \param table Pointer to the MPU configuration table. -* \param cnt Amount of regions to be configured. -*/ -__STATIC_INLINE void ARM_MPU_Load_NS(uint32_t rnr, ARM_MPU_Region_t const* table, uint32_t cnt) -{ - ARM_MPU_LoadEx(MPU_NS, rnr, table, cnt); -} -#endif - -#endif - diff --git a/panda/board/stm32h7/lladc.h b/panda/board/stm32h7/lladc.h index 01342a4d05..7d818f27af 100644 --- a/panda/board/stm32h7/lladc.h +++ b/panda/board/stm32h7/lladc.h @@ -7,7 +7,7 @@ void adc_init(void) { ADC1->CR &= ~(ADC_CR_ADCALDIF); // Choose single-ended calibration ADC1->CR |= ADC_CR_ADCALLIN; // Lineriality calibration ADC1->CR |= ADC_CR_ADCAL; // Start calibrtation - while((ADC1->CR & ADC_CR_ADCAL) != 0); + while((ADC1->CR & ADC_CR_ADCAL) != 0U); ADC1->ISR |= ADC_ISR_ADRDY; ADC1->CR |= ADC_CR_ADEN; @@ -17,11 +17,11 @@ void adc_init(void) { uint16_t adc_get_raw(uint8_t channel) { uint16_t res = 0U; ADC1->SQR1 &= ~(ADC_SQR1_L); - ADC1->SQR1 = ((uint32_t) channel << 6U); + ADC1->SQR1 = (uint32_t)channel << 6U; - ADC1->SMPR1 = (0x2U << (channel * 3U)); + ADC1->SMPR1 = 0x2UL << (channel * 3UL); ADC1->PCSEL_RES0 = (0x1UL << channel); - ADC1->CFGR2 = (127U << ADC_CFGR2_OVSR_Pos) | (0x7U << ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_ROVSE; + ADC1->CFGR2 = (127UL << ADC_CFGR2_OVSR_Pos) | (0x7U << ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_ROVSE; ADC1->CR |= ADC_CR_ADSTART; while (!(ADC1->ISR & ADC_ISR_EOC)); diff --git a/panda/board/stm32h7/llexti.h b/panda/board/stm32h7/llexti.h deleted file mode 100644 index 46d0acab67..0000000000 --- a/panda/board/stm32h7/llexti.h +++ /dev/null @@ -1,63 +0,0 @@ -void EXTI_IRQ_Handler(void); - -void exti_irq_init(void) { - if (harness.status == HARNESS_STATUS_FLIPPED) { - // CAN2_RX IRQ on falling edge (EXTI10) - current_board->enable_can_transceiver(3U, false); - SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI10_Msk); - SYSCFG->EXTICR[2] |= (SYSCFG_EXTICR3_EXTI10_PG); - EXTI->IMR1 |= EXTI_IMR1_IM10; - EXTI->RTSR1 &= ~EXTI_RTSR1_TR10; // rising edge - EXTI->FTSR1 |= EXTI_FTSR1_TR10; // falling edge - - // IRQ on falling edge for PA1 (SBU2, EXTI1) - SYSCFG->EXTICR[0] &= ~(SYSCFG_EXTICR1_EXTI1_Msk); - SYSCFG->EXTICR[0] |= (SYSCFG_EXTICR1_EXTI1_PA); - EXTI->IMR1 |= EXTI_IMR1_IM1; - EXTI->RTSR1 &= ~EXTI_RTSR1_TR1; // rising edge - EXTI->FTSR1 |= EXTI_FTSR1_TR1; // falling edge - REGISTER_INTERRUPT(EXTI1_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI1_IRQn); - REGISTER_INTERRUPT(EXTI15_10_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI15_10_IRQn); - } else { - // CAN0_RX IRQ on falling edge (EXTI8) - current_board->enable_can_transceiver(1U, false); - SYSCFG->EXTICR[2] &= ~(SYSCFG_EXTICR3_EXTI8_Msk); - SYSCFG->EXTICR[2] |= (SYSCFG_EXTICR3_EXTI8_PB); - EXTI->IMR1 |= EXTI_IMR1_IM8; - EXTI->RTSR1 &= ~EXTI_RTSR1_TR8; // rising edge - EXTI->FTSR1 |= EXTI_FTSR1_TR8; // falling edge - - // IRQ on falling edge for PC4 (SBU1, EXTI4) - SYSCFG->EXTICR[1] &= ~(SYSCFG_EXTICR2_EXTI4_Msk); - SYSCFG->EXTICR[1] |= (SYSCFG_EXTICR2_EXTI4_PC); - EXTI->IMR1 |= EXTI_IMR1_IM4; - EXTI->RTSR1 &= ~EXTI_RTSR1_TR4; // rising edge - EXTI->FTSR1 |= EXTI_FTSR1_TR4; // falling edge - REGISTER_INTERRUPT(EXTI4_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI4_IRQn); - REGISTER_INTERRUPT(EXTI9_5_IRQn, EXTI_IRQ_Handler, 100U, FAULT_INTERRUPT_RATE_EXTI) - NVIC_EnableIRQ(EXTI9_5_IRQn); - } -} - -bool check_exti_irq(void) { - return ((EXTI->PR1 & EXTI_PR1_PR8) || (EXTI->PR1 & EXTI_PR1_PR10) || (EXTI->PR1 & EXTI_PR1_PR1) || (EXTI->PR1 & EXTI_PR1_PR4)); -} - -void exti_irq_clear(void) { - // Clear pending bits - EXTI->PR1 |= EXTI_PR1_PR8; - EXTI->PR1 |= EXTI_PR1_PR10; - EXTI->PR1 |= EXTI_PR1_PR4; - EXTI->PR1 |= EXTI_PR1_PR1; // works - EXTI->PR1 |= EXTI_PR1_PR19; // works - - // Disable all active EXTI IRQs - EXTI->IMR1 &= ~EXTI_IMR1_IM8; - EXTI->IMR1 &= ~EXTI_IMR1_IM10; - EXTI->IMR1 &= ~EXTI_IMR1_IM4; - EXTI->IMR1 &= ~EXTI_IMR1_IM1; - EXTI->IMR1 &= ~EXTI_IMR1_IM19; -} diff --git a/panda/board/stm32h7/llfdcan.h b/panda/board/stm32h7/llfdcan.h index 4bb6d3d04e..db42024c43 100644 --- a/panda/board/stm32h7/llfdcan.h +++ b/panda/board/stm32h7/llfdcan.h @@ -55,7 +55,7 @@ bool fdcan_request_init(FDCAN_GlobalTypeDef *FDCANx) { // Request init uint32_t timeout_counter = 0U; FDCANx->CCCR |= FDCAN_CCCR_INIT; - while ((FDCANx->CCCR & FDCAN_CCCR_INIT) == 0) { + while ((FDCANx->CCCR & FDCAN_CCCR_INIT) == 0U) { // Delay for about 1ms delay(10000); timeout_counter++; @@ -73,7 +73,7 @@ bool fdcan_exit_init(FDCAN_GlobalTypeDef *FDCANx) { FDCANx->CCCR &= ~(FDCAN_CCCR_INIT); uint32_t timeout_counter = 0U; - while ((FDCANx->CCCR & FDCAN_CCCR_INIT) != 0) { + while ((FDCANx->CCCR & FDCAN_CCCR_INIT) != 0U) { // Delay for about 1ms delay(10000); timeout_counter++; @@ -118,7 +118,7 @@ bool llcan_set_speed(FDCAN_GlobalTypeDef *FDCANx, uint32_t speed, uint32_t data_ uint32_t seg2 = CAN_SEG2(tq, sp); uint8_t sjw = MIN(127U, seg2); - FDCANx->NBTP = (((sjw & 0x7FU)-1U)<NBTP = (((sjw & 0x7FUL)-1U)<DBTP = (((sjw & 0xFU)-1U)<DBTP = (((sjw & 0xFUL)-1U)<CR2, I2C_CR2_ADD10); - I2C->CR2 = ((addr << 1U) & I2C_CR2_SADD_Msk); + I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); register_set_bits(&I2C->CR2, I2C_CR2_AUTOEND); - I2C->CR2 |= (2 << I2C_CR2_NBYTES_Pos); + I2C->CR2 |= 2UL << I2C_CR2_NBYTES_Pos; I2C->CR2 |= I2C_CR2_START; if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { @@ -61,10 +61,10 @@ bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value) { bool ret = false; for(uint32_t i=0U; i<10U; i++) { register_clear_bits(&I2C->CR2, I2C_CR2_ADD10); - I2C->CR2 = ((addr << 1U) & I2C_CR2_SADD_Msk); + I2C->CR2 = ((uint32_t)addr << 1U) & I2C_CR2_SADD_Msk; register_clear_bits(&I2C->CR2, I2C_CR2_RD_WRN); register_clear_bits(&I2C->CR2, I2C_CR2_AUTOEND); - I2C->CR2 |= (1 << I2C_CR2_NBYTES_Pos); + I2C->CR2 |= 1UL << I2C_CR2_NBYTES_Pos; I2C->CR2 |= I2C_CR2_START; if(!i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U)) { @@ -92,7 +92,7 @@ bool i2c_read_reg(I2C_TypeDef *I2C, uint8_t addr, uint8_t reg, uint8_t *value) { I2C->TXDR = reg; // Restart - I2C->CR2 = (((addr << 1) | 0x1U) & I2C_CR2_SADD_Msk) | (1U << I2C_CR2_NBYTES_Pos) | I2C_CR2_RD_WRN | I2C_CR2_START; + I2C->CR2 = (((addr << 1) | 0x1U) & I2C_CR2_SADD_Msk) | (1UL << I2C_CR2_NBYTES_Pos) | I2C_CR2_RD_WRN | I2C_CR2_START; ret = i2c_status_wait(&I2C->CR2, I2C_CR2_START, 0U); if(!ret) { goto end; diff --git a/panda/board/stm32h7/llrtc.h b/panda/board/stm32h7/llrtc.h deleted file mode 100644 index 03787d0db2..0000000000 --- a/panda/board/stm32h7/llrtc.h +++ /dev/null @@ -1,69 +0,0 @@ -void enable_bdomain_protection(void) { - register_clear_bits(&(PWR->CR1), PWR_CR1_DBP); -} - -void disable_bdomain_protection(void) { - register_set_bits(&(PWR->CR1), PWR_CR1_DBP); -} - -void rtc_init(void){ - uint32_t bdcr_opts = RCC_BDCR_RTCEN; - uint32_t bdcr_mask = (RCC_BDCR_RTCEN | RCC_BDCR_RTCSEL); - if (current_board->has_rtc_battery) { - bdcr_opts |= (RCC_BDCR_LSEDRV_1 | RCC_BDCR_RTCSEL_0 | RCC_BDCR_LSEON); - bdcr_mask |= (RCC_BDCR_LSEDRV | RCC_BDCR_LSEBYP | RCC_BDCR_LSEON); - } else { - bdcr_opts |= RCC_BDCR_RTCSEL_1; - RCC->CSR |= RCC_CSR_LSION; - while((RCC->CSR & RCC_CSR_LSIRDY) == 0){} - } - - // Initialize RTC module and clock if not done already. - if((RCC->BDCR & bdcr_mask) != bdcr_opts){ - print("Initializing RTC\n"); - // Reset backup domain - register_set_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Disable write protection - disable_bdomain_protection(); - - // Clear backup domain reset - register_clear_bits(&(RCC->BDCR), RCC_BDCR_BDRST); - - // Set RTC options - register_set(&(RCC->BDCR), bdcr_opts, bdcr_mask); - - // Enable write protection - enable_bdomain_protection(); - } -} - -void rtc_wakeup_init(void) { - EXTI->IMR1 |= EXTI_IMR1_IM19; - EXTI->RTSR1 |= EXTI_RTSR1_TR19; // rising edge - EXTI->FTSR1 &= ~EXTI_FTSR1_TR19; // falling edge - - NVIC_DisableIRQ(RTC_WKUP_IRQn); - - // Disable write protection - disable_bdomain_protection(); - RTC->WPR = 0xCA; - RTC->WPR = 0x53; - - RTC->CR &= ~RTC_CR_WUTE; - while((RTC->ISR & RTC_ISR_WUTWF) == 0){} - - RTC->CR &= ~RTC_CR_WUTIE; - RTC->ISR &= ~RTC_ISR_WUTF; - //PWR->CR1 |= PWR_CR1_CWUF; - - RTC->WUTR = DEEPSLEEP_WAKEUP_DELAY; - // Wakeup timer interrupt enable, wakeup timer enable, select 1Hz rate - RTC->CR |= RTC_CR_WUTE | RTC_CR_WUTIE | RTC_CR_WUCKSEL_2; - - // Re-enable write protection - RTC->WPR = 0x00; - enable_bdomain_protection(); - - NVIC_EnableIRQ(RTC_WKUP_IRQn); -} diff --git a/panda/board/stm32h7/llspi.h b/panda/board/stm32h7/llspi.h index 1947803ac2..903f6a5ecc 100644 --- a/panda/board/stm32h7/llspi.h +++ b/panda/board/stm32h7/llspi.h @@ -71,7 +71,7 @@ void SPI4_IRQ_Handler(void) { // clear flag SPI4->IFCR |= (0x1FFU << 3U); - if (spi_tx_dma_done && ((SPI4->SR & SPI_SR_TXC) != 0)) { + if (spi_tx_dma_done && ((SPI4->SR & SPI_SR_TXC) != 0U)) { spi_tx_dma_done = false; spi_tx_done(false); } diff --git a/panda/board/stm32h7/lluart.h b/panda/board/stm32h7/lluart.h index 0ad7b6a867..6ca6dcec3d 100644 --- a/panda/board/stm32h7/lluart.h +++ b/panda/board/stm32h7/lluart.h @@ -29,7 +29,7 @@ void uart_tx_ring(uart_ring *q){ // Send out next byte of TX buffer if (q->w_ptr_tx != q->r_ptr_tx) { // Only send if transmit register is empty (aka last byte has been sent) - if ((q->uart->ISR & USART_ISR_TXE_TXFNF) != 0) { + if ((q->uart->ISR & USART_ISR_TXE_TXFNF) != 0U) { q->uart->TDR = q->elems_tx[q->r_ptr_tx]; // This clears TXE q->r_ptr_tx = (q->r_ptr_tx + 1U) % q->tx_fifo_size; } diff --git a/panda/board/stm32h7/llusb.h b/panda/board/stm32h7/llusb.h index ada1630f8b..2975f62be6 100644 --- a/panda/board/stm32h7/llusb.h +++ b/panda/board/stm32h7/llusb.h @@ -32,7 +32,7 @@ void usb_init(void) { USBx->GUSBCFG |= USB_OTG_GUSBCFG_FDMOD; delay(250000); // Wait for about 25ms (explicitly stated in H7 ref manual) // Wait for AHB master IDLE state. - while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0); + while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_AHBIDL) == 0U); // Core Soft Reset USBx->GRSTCTL |= USB_OTG_GRSTCTL_CSRST; while ((USBx->GRSTCTL & USB_OTG_GRSTCTL_CSRST) == USB_OTG_GRSTCTL_CSRST); diff --git a/panda/board/stm32h7/stm32h7_config.h b/panda/board/stm32h7/stm32h7_config.h index 6f36a5aaf4..64f149848d 100644 --- a/panda/board/stm32h7/stm32h7_config.h +++ b/panda/board/stm32h7/stm32h7_config.h @@ -76,10 +76,6 @@ separate IRQs for RX and TX. #include "stm32h7/board.h" #include "stm32h7/clock.h" -#if !defined(BOOTSTUB) && defined(PANDA) - #include "stm32h7/llexti.h" -#endif - #ifdef BOOTSTUB #include "stm32h7/llflash.h" #else diff --git a/panda/board/utils.h b/panda/board/utils.h index fd69db1bbf..f355ce8c2f 100644 --- a/panda/board/utils.h +++ b/panda/board/utils.h @@ -1,15 +1,18 @@ +// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension #define MIN(a, b) ({ \ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ (_a < _b) ? _a : _b; \ }) +// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension #define MAX(a, b) ({ \ __typeof__ (a) _a = (a); \ __typeof__ (b) _b = (b); \ (_a > _b) ? _a : _b; \ }) +// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension #define CLAMP(x, low, high) ({ \ __typeof__(x) __x = (x); \ __typeof__(low) __low = (low);\ @@ -17,6 +20,7 @@ (__x > __high) ? __high : ((__x < __low) ? __low : __x); \ }) +// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension #define ABS(a) ({ \ __typeof__ (a) _a = (a); \ (_a > 0) ? _a : (-_a); \ diff --git a/panda/docs/CANPacket_structure.png b/panda/docs/CANPacket_structure.png new file mode 100644 index 0000000000..7dd1343161 Binary files /dev/null and b/panda/docs/CANPacket_structure.png differ diff --git a/panda/docs/USB_packet_structure.png b/panda/docs/USB_packet_structure.png new file mode 100644 index 0000000000..30e848c3f6 Binary files /dev/null and b/panda/docs/USB_packet_structure.png differ diff --git a/panda/examples/query_fw_versions.py b/panda/examples/query_fw_versions.py deleted file mode 100755 index 4f4e3fa66b..0000000000 --- a/panda/examples/query_fw_versions.py +++ /dev/null @@ -1,115 +0,0 @@ -#!/usr/bin/env python3 -import argparse -from typing import List, Optional -from tqdm import tqdm -from panda import Panda -from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, InvalidSubAddressError, \ - SESSION_TYPE, DATA_IDENTIFIER_TYPE - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("--rxoffset", default="") - parser.add_argument("--nonstandard", action="store_true") - parser.add_argument("--no-obd", action="store_true", help="Bus 1 will not be multiplexed to the OBD-II port") - parser.add_argument("--debug", action="store_true") - parser.add_argument("--addr") - parser.add_argument("--sub_addr", "--subaddr", help="A hex sub-address or `scan` to scan the full sub-address range") - parser.add_argument("--bus") - parser.add_argument('-s', '--serial', help="Serial number of panda to use") - args = parser.parse_args() - - if args.addr: - addrs = [int(args.addr, base=16)] - else: - addrs = [0x700 + i for i in range(256)] - addrs += [0x18da0000 + (i << 8) + 0xf1 for i in range(256)] - results = {} - - sub_addrs: List[Optional[int]] = [None] - if args.sub_addr: - if args.sub_addr == "scan": - sub_addrs = list(range(0xff + 1)) - else: - sub_addrs = [int(args.sub_addr, base=16)] - if sub_addrs[0] > 0xff: # type: ignore - print(f"Invalid sub-address: 0x{sub_addrs[0]:X}, needs to be in range 0x0 to 0xff") - parser.print_help() - exit() - - uds_data_ids = {} - for std_id in DATA_IDENTIFIER_TYPE: - uds_data_ids[std_id.value] = std_id.name - if args.nonstandard: - for uds_id in range(0xf100, 0xf180): - uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_VEHICLE_MANUFACTURER_SPECIFIC_DATA_IDENTIFIER" - for uds_id in range(0xf1a0, 0xf1f0): - uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_VEHICLE_MANUFACTURER_SPECIFIC" - for uds_id in range(0xf1f0, 0xf200): - uds_data_ids[uds_id] = "IDENTIFICATION_OPTION_SYSTEM_SUPPLIER_SPECIFIC" - - panda_serials = Panda.list() - if args.serial is None and len(panda_serials) > 1: - print("\nMultiple pandas found, choose one:") - for serial in panda_serials: - with Panda(serial) as panda: - print(f" {serial}: internal={panda.is_internal()}") - print() - parser.print_help() - exit() - - panda = Panda(serial=args.serial) - panda.set_safety_mode(Panda.SAFETY_ELM327, 1 if args.no_obd else 0) - print("querying addresses ...") - with tqdm(addrs) as t: - for addr in t: - # skip functional broadcast addrs - if addr == 0x7df or addr == 0x18db33f1: - continue - - if args.bus: - bus = int(args.bus) - else: - bus = 1 if panda.has_obd() else 0 - rx_addr = addr + int(args.rxoffset, base=16) if args.rxoffset else None - - # Try all sub-addresses for addr. By default, this is None - for sub_addr in sub_addrs: - sub_addr_str = hex(sub_addr) if sub_addr is not None else None - t.set_description(f"{hex(addr)}, {sub_addr_str}") - uds_client = UdsClient(panda, addr, rx_addr, bus, sub_addr=sub_addr, timeout=0.2, debug=args.debug) - # Check for anything alive at this address, and switch to the highest - # available diagnostic session without security access - try: - uds_client.tester_present() - uds_client.diagnostic_session_control(SESSION_TYPE.DEFAULT) - uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) - except NegativeResponseError: - pass - except MessageTimeoutError: - continue - except InvalidSubAddressError as e: - print(f'*** Skipping address {hex(addr)}: {e}') - break - - # Run queries against all standard UDS data identifiers, plus selected - # non-standardized identifier ranges if requested - resp = {} - for uds_data_id in sorted(uds_data_ids): - try: - data = uds_client.read_data_by_identifier(uds_data_id) # type: ignore - if data: - resp[uds_data_id] = data - except (NegativeResponseError, MessageTimeoutError, InvalidSubAddressError): - pass - - if resp.keys(): - results[(addr, sub_addr)] = resp - - if len(results.items()): - for (addr, sub_addr), resp in results.items(): - sub_addr_str = f", sub-address 0x{sub_addr:X}" if sub_addr is not None else "" - print(f"\n\n*** Results for address 0x{addr:X}{sub_addr_str} ***\n\n") - for rid, dat in resp.items(): - print(f"0x{rid:02X} {uds_data_ids[rid]}: {dat}") - else: - print("no fw versions found!") diff --git a/panda/mypy.ini b/panda/mypy.ini new file mode 100644 index 0000000000..0b6de30d80 --- /dev/null +++ b/panda/mypy.ini @@ -0,0 +1,11 @@ +[mypy] +; third-party packages +ignore_missing_imports = True + +; helpful warnings +warn_redundant_casts = True +warn_unreachable = True +warn_unused_ignores = True + +; restrict dynamic typing +warn_return_any = True diff --git a/panda/panda.png b/panda/panda.png new file mode 100644 index 0000000000..e18137d872 Binary files /dev/null and b/panda/panda.png differ diff --git a/panda/pyproject.toml b/panda/pyproject.toml new file mode 100644 index 0000000000..375885e54d --- /dev/null +++ b/panda/pyproject.toml @@ -0,0 +1,15 @@ +# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml +[tool.ruff] +line-length = 160 +target-version="py311" + +[tool.ruff.lint] +select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"] +ignore = ["W292", "E741", "E402", "C408", "ISC003"] +flake8-implicit-str-concat.allow-multiline=false + +[tool.ruff.lint.flake8-tidy-imports.banned-api] +"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!" + +[tool.pytest.ini_options] +addopts = "-n auto" diff --git a/panda/python/__init__.py b/panda/python/__init__.py index acf6ea4834..a18fdc5f91 100644 --- a/panda/python/__init__.py +++ b/panda/python/__init__.py @@ -6,10 +6,8 @@ import usb1 import struct import hashlib import binascii -import datetime import logging from functools import wraps, partial -from typing import Optional from itertools import accumulate from .base import BaseHandle @@ -150,9 +148,6 @@ class Panda: SERIAL_LIN2 = 3 SERIAL_SOM_DEBUG = 4 - GMLAN_CAN2 = 1 - GMLAN_CAN3 = 2 - USB_PIDS = (0xddee, 0xddcc) REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE REQUEST_OUT = usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE @@ -170,9 +165,9 @@ class Panda: HW_TYPE_CUATRO = b'\x0a' CAN_PACKET_VERSION = 4 - HEALTH_PACKET_VERSION = 15 + HEALTH_PACKET_VERSION = 16 CAN_HEALTH_PACKET_VERSION = 5 - HEALTH_STRUCT = struct.Struct(" bool: + return isinstance(self._handle, PandaSpiHandle) + @classmethod def spi_connect(cls, serial, ignore_version=False): # get UID to confirm slave is present and up @@ -363,6 +361,7 @@ class Panda: try: this_serial = device.getSerialNumber() except Exception: + logging.exception("failed to get serial number of panda") continue if serial is None or this_serial == serial: @@ -414,7 +413,7 @@ class Panda: else: logging.warning(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning) except Exception: - continue + logging.exception("error connecting to panda") except Exception: logging.exception("exception while listing pandas") return ret @@ -439,6 +438,8 @@ class Panda: self._handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'', timeout=timeout, expect_disconnect=True) except Exception: pass + + self.close() if not enter_bootloader and reconnect: self.reconnect() @@ -530,7 +531,7 @@ class Panda: if reconnect: self.reconnect() - def recover(self, timeout: Optional[int] = 60, reset: bool = True) -> bool: + def recover(self, timeout: int | None = 60, reset: bool = True) -> bool: dfu_serial = self.get_dfu_serial() if reset: @@ -549,7 +550,7 @@ class Panda: return True @staticmethod - def wait_for_dfu(dfu_serial: Optional[str], timeout: Optional[int] = None) -> bool: + def wait_for_dfu(dfu_serial: str | None, timeout: int | None = None) -> bool: t_start = time.monotonic() dfu_list = PandaDFU.list() while (dfu_serial is None and len(dfu_list) == 0) or (dfu_serial is not None and dfu_serial not in dfu_list): @@ -561,7 +562,7 @@ class Panda: return True @staticmethod - def wait_for_panda(serial: Optional[str], timeout: int) -> bool: + def wait_for_panda(serial: str | None, timeout: int) -> bool: t_start = time.monotonic() serials = Panda.list() while (serial is None and len(serials) == 0) or (serial is not None and serial not in serials): @@ -596,26 +597,25 @@ class Panda: "safety_rx_invalid": a[4], "tx_buffer_overflow": a[5], "rx_buffer_overflow": a[6], - "gmlan_send_errs": a[7], - "faults": a[8], - "ignition_line": a[9], - "ignition_can": a[10], - "controls_allowed": a[11], - "car_harness_status": a[12], - "safety_mode": a[13], - "safety_param": a[14], - "fault_status": a[15], - "power_save_enabled": a[16], - "heartbeat_lost": a[17], - "alternative_experience": a[18], - "interrupt_load": a[19], - "fan_power": a[20], - "safety_rx_checks_invalid": a[21], - "spi_checksum_error_count": a[22], - "fan_stall_count": a[23], - "sbu1_voltage_mV": a[24], - "sbu2_voltage_mV": a[25], - "som_reset_triggered": a[26], + "faults": a[7], + "ignition_line": a[8], + "ignition_can": a[9], + "controls_allowed": a[10], + "car_harness_status": a[11], + "safety_mode": a[12], + "safety_param": a[13], + "fault_status": a[14], + "power_save_enabled": a[15], + "heartbeat_lost": a[16], + "alternative_experience": a[17], + "interrupt_load": a[18], + "fan_power": a[19], + "safety_rx_checks_invalid": a[20], + "spi_checksum_error_count": a[21], + "fan_stall_count": a[22], + "sbu1_voltage_mV": a[23], + "sbu2_voltage_mV": a[24], + "som_reset_triggered": a[25], } @ensure_can_health_packet_version @@ -752,21 +752,10 @@ class Panda: def set_power_save(self, power_save_enabled=0): self._handle.controlWrite(Panda.REQUEST_OUT, 0xe7, int(power_save_enabled), 0, b'') - def enable_deepsleep(self): - self._handle.controlWrite(Panda.REQUEST_OUT, 0xfb, 0, 0, b'') - def set_safety_mode(self, mode=SAFETY_SILENT, param=0): self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, param, b'') - def set_gmlan(self, bus=2): - # TODO: check panda type - if bus is None: - self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 0, 0, b'') - elif bus in (Panda.GMLAN_CAN2, Panda.GMLAN_CAN3): - self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'') - def set_obd(self, obd): - # TODO: check panda type self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, int(obd), 0, b'') def set_can_loopback(self, enable): @@ -894,21 +883,6 @@ class Panda: def set_heartbeat_disabled(self): self._handle.controlWrite(Panda.REQUEST_OUT, 0xf8, 0, 0, b'') - # ******************* RTC ******************* - def set_datetime(self, dt): - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa1, int(dt.year), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa2, int(dt.month), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa3, int(dt.day), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa4, int(dt.isoweekday()), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa5, int(dt.hour), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa6, int(dt.minute), 0, b'') - self._handle.controlWrite(Panda.REQUEST_OUT, 0xa7, int(dt.second), 0, b'') - - def get_datetime(self): - dat = self._handle.controlRead(Panda.REQUEST_IN, 0xa0, 0, 0, 8) - a = struct.unpack("HBBBBBB", dat) - return datetime.datetime(a[0], a[1], a[2], a[4], a[5], a[6]) - # ****************** Timer ***************** def get_microsecond_timer(self): dat = self._handle.controlRead(Panda.REQUEST_IN, 0xa8, 0, 0, 4) diff --git a/panda/python/constants.py b/panda/python/constants.py index fede52424c..8b09593084 100644 --- a/panda/python/constants.py +++ b/panda/python/constants.py @@ -1,6 +1,6 @@ import os import enum -from typing import List, NamedTuple +from typing import NamedTuple BASEDIR = os.path.join(os.path.dirname(os.path.realpath(__file__)), "../") FW_PATH = os.path.join(BASEDIR, "board/obj/") @@ -10,7 +10,7 @@ USBPACKET_MAX_SIZE = 0x40 class McuConfig(NamedTuple): mcu: str mcu_idcode: int - sector_sizes: List[int] + sector_sizes: list[int] sector_count: int # total sector count, used for MCU identification in DFU mode uid_address: int block_size: int diff --git a/panda/python/dfu.py b/panda/python/dfu.py index 01ff037f84..9beba45e55 100644 --- a/panda/python/dfu.py +++ b/panda/python/dfu.py @@ -2,7 +2,6 @@ import os import usb1 import struct import binascii -from typing import List, Optional from .base import BaseSTBootloaderHandle from .spi import STBootloaderSPIHandle, PandaSpiException @@ -11,9 +10,9 @@ from .constants import FW_PATH, McuType class PandaDFU: - def __init__(self, dfu_serial: Optional[str]): + def __init__(self, dfu_serial: str | None): # try USB, then SPI - handle: Optional[BaseSTBootloaderHandle] + handle: BaseSTBootloaderHandle | None self._context, handle = PandaDFU.usb_connect(dfu_serial) if handle is None: self._context, handle = PandaDFU.spi_connect(dfu_serial) @@ -38,7 +37,7 @@ class PandaDFU: self._context.close() @staticmethod - def usb_connect(dfu_serial: Optional[str]): + def usb_connect(dfu_serial: str | None): handle = None context = usb1.USBContext() context.open() @@ -56,7 +55,7 @@ class PandaDFU: return context, handle @staticmethod - def spi_connect(dfu_serial: Optional[str]): + def spi_connect(dfu_serial: str | None): handle = None this_dfu_serial = None @@ -72,13 +71,7 @@ class PandaDFU: return None, handle @staticmethod - def list() -> List[str]: - ret = PandaDFU.usb_list() - ret += PandaDFU.spi_list() - return list(set(ret)) - - @staticmethod - def usb_list() -> List[str]: + def usb_list() -> list[str]: dfu_serials = [] try: with usb1.USBContext() as context: @@ -93,7 +86,7 @@ class PandaDFU: return dfu_serials @staticmethod - def spi_list() -> List[str]: + def spi_list() -> list[str]: try: _, h = PandaDFU.spi_connect(None) if h is not None: @@ -134,3 +127,9 @@ class PandaDFU: code = f.read() self.program_bootstub(code) self.reset() + + @staticmethod + def list() -> list[str]: + ret = PandaDFU.usb_list() + ret += PandaDFU.spi_list() + return list(set(ret)) diff --git a/panda/python/isotp.py b/panda/python/isotp.py index 3334deb8ed..d0bef7d942 100644 --- a/panda/python/isotp.py +++ b/panda/python/isotp.py @@ -79,10 +79,10 @@ def isotp_send(panda, x, addr, bus=0, recvaddr=None, subaddr=None, rate=None): sends = [] while len(x) > 0: if subaddr: - sends.append(((bytes([subaddr, 0x20 + (idx & 0xF)]) + x[0:6]).ljust(8, b"\x00"))) + sends.append((bytes([subaddr, 0x20 + (idx & 0xF)]) + x[0:6]).ljust(8, b"\x00")) x = x[6:] else: - sends.append(((bytes([0x20 + (idx & 0xF)]) + x[0:7]).ljust(8, b"\x00"))) + sends.append((bytes([0x20 + (idx & 0xF)]) + x[0:7]).ljust(8, b"\x00")) x = x[7:] idx += 1 diff --git a/panda/python/serial.py b/panda/python/serial.py index 9ac58862b5..c2e965b76c 100644 --- a/panda/python/serial.py +++ b/panda/python/serial.py @@ -1,5 +1,5 @@ # mimic a python serial port -class PandaSerial(object): +class PandaSerial: def __init__(self, panda, port, baud): self.panda = panda self.port = port diff --git a/panda/python/spi.py b/panda/python/spi.py index 48dc84d49e..be4f7dcf46 100644 --- a/panda/python/spi.py +++ b/panda/python/spi.py @@ -9,7 +9,7 @@ import logging import threading from contextlib import contextmanager from functools import reduce -from typing import Callable, List, Optional +from collections.abc import Callable from .base import BaseHandle, BaseSTBootloaderHandle, TIMEOUT from .constants import McuType, MCU_TYPE_BY_IDCODE, USBPACKET_MAX_SIZE @@ -251,7 +251,7 @@ class PandaSpiHandle(BaseHandle): version_bytes = spi.readbytes(len(vers_str) + 2) if bytes(version_bytes).startswith(vers_str): break - if (time.monotonic() - start) > 0.01: + if (time.monotonic() - start) > 0.001: raise PandaSpiMissingAck rlen = struct.unpack(" bytes: + def _cmd_no_retry(self, cmd: int, data: list[bytes] | None = None, read_bytes: int = 0, predata=None) -> bytes: ret = b"" with self.dev.acquire() as spi: # sync + command spi.xfer([self.SYNC, ]) spi.xfer([cmd, cmd ^ 0xFF]) - self._get_ack(spi, timeout=0.1) + self._get_ack(spi, timeout=0.01) # "predata" - for commands that send the first data without a checksum if predata is not None: @@ -371,7 +371,7 @@ class STBootloaderSPIHandle(BaseSTBootloaderHandle): return bytes(ret) - def _cmd(self, cmd: int, data: Optional[List[bytes]] = None, read_bytes: int = 0, predata=None) -> bytes: + def _cmd(self, cmd: int, data: list[bytes] | None = None, read_bytes: int = 0, predata=None) -> bytes: exc = PandaSpiException() for n in range(MAX_XFER_RETRY_COUNT): try: diff --git a/panda/python/uds.py b/panda/python/uds.py index aaa0697f90..32b0de0961 100644 --- a/panda/python/uds.py +++ b/panda/python/uds.py @@ -1,7 +1,8 @@ import time import struct from collections import deque -from typing import Callable, NamedTuple, Tuple, List, Deque, Generator, Optional, cast +from typing import NamedTuple, Deque, cast +from collections.abc import Callable, Generator from enum import IntEnum from functools import partial @@ -300,8 +301,8 @@ def get_dtc_status_names(status): return result class CanClient(): - def __init__(self, can_send: Callable[[int, bytes, int], None], can_recv: Callable[[], List[Tuple[int, int, bytes, int]]], - tx_addr: int, rx_addr: int, bus: int, sub_addr: Optional[int] = None, debug: bool = False): + def __init__(self, can_send: Callable[[int, bytes, int], None], can_recv: Callable[[], list[tuple[int, int, bytes, int]]], + tx_addr: int, rx_addr: int, bus: int, sub_addr: int | None = None, debug: bool = False): self.tx = can_send self.rx = can_recv self.tx_addr = tx_addr @@ -335,7 +336,7 @@ class CanClient(): msgs = self.rx() if drain: if self.debug: - print("CAN-RX: drain - {}".format(len(msgs))) + print(f"CAN-RX: drain - {len(msgs)}") self.rx_buff.clear() else: for rx_addr, _, rx_data, rx_bus in msgs or []: @@ -366,7 +367,7 @@ class CanClient(): except IndexError: pass # empty - def send(self, msgs: List[bytes], delay: float = 0) -> None: + def send(self, msgs: list[bytes], delay: float = 0) -> None: for i, msg in enumerate(msgs): if delay and i != 0: if self.debug: @@ -443,7 +444,7 @@ class IsoTpMessage(): if not setup_only: self._can_client.send([msg]) - def recv(self, timeout=None) -> Tuple[Optional[bytes], bool]: + def recv(self, timeout=None) -> tuple[bytes | None, bool]: if timeout is None: timeout = self.timeout @@ -454,7 +455,8 @@ class IsoTpMessage(): for msg in self._can_client.recv(): frame_type = self._isotp_rx_next(msg) start_time = time.monotonic() - rx_in_progress = frame_type == ISOTP_FRAME_TYPE.CONSECUTIVE + # Anything that signifies we're building a response + rx_in_progress = frame_type in (ISOTP_FRAME_TYPE.FIRST, ISOTP_FRAME_TYPE.CONSECUTIVE) if self.tx_done and self.rx_done: return self.rx_dat, False # no timeout indicates non-blocking @@ -472,6 +474,7 @@ class IsoTpMessage(): # assert len(rx_data) == self.max_len, f"isotp - rx: invalid CAN frame length: {len(rx_data)}" if rx_data[0] >> 4 == ISOTP_FRAME_TYPE.SINGLE: + assert self.rx_dat == b"" or self.rx_done, "isotp - rx: single frame with active frame" self.rx_len = rx_data[0] & 0x0F assert self.rx_len < self.max_len, f"isotp - rx: invalid single frame length: {self.rx_len}" self.rx_dat = rx_data[1:1 + self.rx_len] @@ -482,8 +485,11 @@ class IsoTpMessage(): return ISOTP_FRAME_TYPE.SINGLE elif rx_data[0] >> 4 == ISOTP_FRAME_TYPE.FIRST: + # Once a first frame is received, further frames must be consecutive + assert self.rx_dat == b"" or self.rx_done, "isotp - rx: first frame with active frame" self.rx_len = ((rx_data[0] & 0x0F) << 8) + rx_data[1] - assert self.max_len <= self.rx_len, f"isotp - rx: invalid first frame length: {self.rx_len}" + assert self.rx_len >= self.max_len, f"isotp - rx: invalid first frame length: {self.rx_len}" + assert len(rx_data) == self.max_len, f"isotp - rx: invalid CAN frame length: {len(rx_data)}" self.rx_dat = rx_data[2:] self.rx_idx = 0 self.rx_done = False @@ -566,11 +572,11 @@ def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8): # standard 29 bit response addr (flip last two bytes) return (tx_addr & 0xFFFF0000) + (tx_addr << 8 & 0xFF00) + (tx_addr >> 8 & 0xFF) - raise ValueError("invalid tx_addr: {}".format(tx_addr)) + raise ValueError(f"invalid tx_addr: {tx_addr}") class UdsClient(): - def __init__(self, panda, tx_addr: int, rx_addr: Optional[int] = None, bus: int = 0, sub_addr: Optional[int] = None, timeout: float = 1, + def __init__(self, panda, tx_addr: int, rx_addr: int | None = None, bus: int = 0, sub_addr: int | None = None, timeout: float = 1, debug: bool = False, tx_timeout: float = 1, response_pending_timeout: float = 10): self.bus = bus self.tx_addr = tx_addr @@ -583,7 +589,7 @@ class UdsClient(): self.response_pending_timeout = response_pending_timeout # generic uds request - def _uds_request(self, service_type: SERVICE_TYPE, subfunction: Optional[int] = None, data: Optional[bytes] = None) -> bytes: + def _uds_request(self, service_type: SERVICE_TYPE, subfunction: int | None = None, data: bytes | None = None) -> bytes: req = bytes([service_type]) if subfunction is not None: req += bytes([subfunction]) @@ -623,12 +629,12 @@ class UdsClient(): if self.debug: print("UDS-RX: response pending") continue - raise NegativeResponseError('{} - {}'.format(service_desc, error_desc), service_id, error_code) + raise NegativeResponseError(f'{service_desc} - {error_desc}', service_id, error_code) # positive response if service_type + 0x40 != resp_sid: resp_sid_hex = hex(resp_sid) if resp_sid is not None else None - raise InvalidServiceIdError('invalid response service id: {}'.format(resp_sid_hex)) + raise InvalidServiceIdError(f'invalid response service id: {resp_sid_hex}') if subfunction is not None: resp_sfn = resp[1] if len(resp) > 1 else None @@ -671,7 +677,7 @@ class UdsClient(): def tester_present(self, ): self._uds_request(SERVICE_TYPE.TESTER_PRESENT, subfunction=0x00) - def access_timing_parameter(self, timing_parameter_type: TIMING_PARAMETER_TYPE, parameter_values: Optional[bytes] = None): + def access_timing_parameter(self, timing_parameter_type: TIMING_PARAMETER_TYPE, parameter_values: bytes | None = None): write_custom_values = timing_parameter_type == TIMING_PARAMETER_TYPE.SET_TO_GIVEN_VALUES read_values = (timing_parameter_type == TIMING_PARAMETER_TYPE.READ_CURRENTLY_ACTIVE or timing_parameter_type == TIMING_PARAMETER_TYPE.READ_EXTENDED_SET) @@ -714,8 +720,8 @@ class UdsClient(): "data": resp[2:], # TODO: parse the reset of response } - def link_control(self, link_control_type: LINK_CONTROL_TYPE, baud_rate_type: Optional[BAUD_RATE_TYPE] = None): - data: Optional[bytes] + def link_control(self, link_control_type: LINK_CONTROL_TYPE, baud_rate_type: BAUD_RATE_TYPE | None = None): + data: bytes | None if link_control_type == LINK_CONTROL_TYPE.VERIFY_BAUDRATE_TRANSITION_WITH_FIXED_BAUDRATE: # baud_rate_type = BAUD_RATE_TYPE @@ -733,21 +739,21 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.READ_DATA_BY_IDENTIFIER, subfunction=None, data=data) resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None if resp_id != data_identifier_type: - raise ValueError('invalid response data identifier: {} expected: {}'.format(hex(resp_id), hex(data_identifier_type))) + raise ValueError(f'invalid response data identifier: {hex(resp_id)} expected: {hex(data_identifier_type)}') return resp[2:] def read_memory_by_address(self, memory_address: int, memory_size: int, memory_address_bytes: int = 4, memory_size_bytes: int = 1): if memory_address_bytes < 1 or memory_address_bytes > 4: - raise ValueError('invalid memory_address_bytes: {}'.format(memory_address_bytes)) + raise ValueError(f'invalid memory_address_bytes: {memory_address_bytes}') if memory_size_bytes < 1 or memory_size_bytes > 4: - raise ValueError('invalid memory_size_bytes: {}'.format(memory_size_bytes)) + raise ValueError(f'invalid memory_size_bytes: {memory_size_bytes}') data = bytes([memory_size_bytes << 4 | memory_address_bytes]) if memory_address >= 1 << (memory_address_bytes * 8): - raise ValueError('invalid memory_address: {}'.format(memory_address)) + raise ValueError(f'invalid memory_address: {memory_address}') data += struct.pack('!I', memory_address)[4 - memory_address_bytes:] if memory_size >= 1 << (memory_size_bytes * 8): - raise ValueError('invalid memory_size: {}'.format(memory_size)) + raise ValueError(f'invalid memory_size: {memory_size}') data += struct.pack('!I', memory_size)[4 - memory_size_bytes:] resp = self._uds_request(SERVICE_TYPE.READ_MEMORY_BY_ADDRESS, subfunction=None, data=data) @@ -758,7 +764,7 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.READ_SCALING_DATA_BY_IDENTIFIER, subfunction=None, data=data) resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None if resp_id != data_identifier_type: - raise ValueError('invalid response data identifier: {}'.format(hex(resp_id))) + raise ValueError(f'invalid response data identifier: {hex(resp_id)}') return resp[2:] # TODO: parse the response def read_data_by_periodic_identifier(self, transmission_mode_type: TRANSMISSION_MODE_TYPE, periodic_data_identifier: int): @@ -767,11 +773,11 @@ class UdsClient(): self._uds_request(SERVICE_TYPE.READ_DATA_BY_PERIODIC_IDENTIFIER, subfunction=None, data=data) def dynamically_define_data_identifier(self, dynamic_definition_type: DYNAMIC_DEFINITION_TYPE, dynamic_data_identifier: int, - source_definitions: List[DynamicSourceDefinition], memory_address_bytes: int = 4, memory_size_bytes: int = 1): + source_definitions: list[DynamicSourceDefinition], memory_address_bytes: int = 4, memory_size_bytes: int = 1): if memory_address_bytes < 1 or memory_address_bytes > 4: - raise ValueError('invalid memory_address_bytes: {}'.format(memory_address_bytes)) + raise ValueError(f'invalid memory_address_bytes: {memory_address_bytes}') if memory_size_bytes < 1 or memory_size_bytes > 4: - raise ValueError('invalid memory_size_bytes: {}'.format(memory_size_bytes)) + raise ValueError(f'invalid memory_size_bytes: {memory_size_bytes}') data = struct.pack('!H', dynamic_data_identifier) if dynamic_definition_type == DYNAMIC_DEFINITION_TYPE.DEFINE_BY_IDENTIFIER: @@ -781,15 +787,15 @@ class UdsClient(): data += bytes([memory_size_bytes << 4 | memory_address_bytes]) for s in source_definitions: if s.memory_address >= 1 << (memory_address_bytes * 8): - raise ValueError('invalid memory_address: {}'.format(s.memory_address)) + raise ValueError(f'invalid memory_address: {s.memory_address}') data += struct.pack('!I', s.memory_address)[4 - memory_address_bytes:] if s.memory_size >= 1 << (memory_size_bytes * 8): - raise ValueError('invalid memory_size: {}'.format(s.memory_size)) + raise ValueError(f'invalid memory_size: {s.memory_size}') data += struct.pack('!I', s.memory_size)[4 - memory_size_bytes:] elif dynamic_definition_type == DYNAMIC_DEFINITION_TYPE.CLEAR_DYNAMICALLY_DEFINED_DATA_IDENTIFIER: pass else: - raise ValueError('invalid dynamic identifier type: {}'.format(hex(dynamic_definition_type))) + raise ValueError(f'invalid dynamic identifier type: {hex(dynamic_definition_type)}') self._uds_request(SERVICE_TYPE.DYNAMICALLY_DEFINE_DATA_IDENTIFIER, subfunction=dynamic_definition_type, data=data) def write_data_by_identifier(self, data_identifier_type: DATA_IDENTIFIER_TYPE, data_record: bytes): @@ -797,20 +803,20 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.WRITE_DATA_BY_IDENTIFIER, subfunction=None, data=data) resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None if resp_id != data_identifier_type: - raise ValueError('invalid response data identifier: {}'.format(hex(resp_id))) + raise ValueError(f'invalid response data identifier: {hex(resp_id)}') def write_memory_by_address(self, memory_address: int, memory_size: int, data_record: bytes, memory_address_bytes: int = 4, memory_size_bytes: int = 1): if memory_address_bytes < 1 or memory_address_bytes > 4: - raise ValueError('invalid memory_address_bytes: {}'.format(memory_address_bytes)) + raise ValueError(f'invalid memory_address_bytes: {memory_address_bytes}') if memory_size_bytes < 1 or memory_size_bytes > 4: - raise ValueError('invalid memory_size_bytes: {}'.format(memory_size_bytes)) + raise ValueError(f'invalid memory_size_bytes: {memory_size_bytes}') data = bytes([memory_size_bytes << 4 | memory_address_bytes]) if memory_address >= 1 << (memory_address_bytes * 8): - raise ValueError('invalid memory_address: {}'.format(memory_address)) + raise ValueError(f'invalid memory_address: {memory_address}') data += struct.pack('!I', memory_address)[4 - memory_address_bytes:] if memory_size >= 1 << (memory_size_bytes * 8): - raise ValueError('invalid memory_size: {}'.format(memory_size)) + raise ValueError(f'invalid memory_size: {memory_size}') data += struct.pack('!I', memory_size)[4 - memory_size_bytes:] data += data_record @@ -864,7 +870,7 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.INPUT_OUTPUT_CONTROL_BY_IDENTIFIER, subfunction=None, data=data) resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None if resp_id != data_identifier_type: - raise ValueError('invalid response data identifier: {}'.format(hex(resp_id))) + raise ValueError(f'invalid response data identifier: {hex(resp_id)}') return resp[2:] def routine_control(self, routine_control_type: ROUTINE_CONTROL_TYPE, routine_identifier_type: ROUTINE_IDENTIFIER_TYPE, routine_option_record: bytes = b''): @@ -872,23 +878,23 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.ROUTINE_CONTROL, subfunction=routine_control_type, data=data) resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None if resp_id != routine_identifier_type: - raise ValueError('invalid response routine identifier: {}'.format(hex(resp_id))) + raise ValueError(f'invalid response routine identifier: {hex(resp_id)}') return resp[2:] def request_download(self, memory_address: int, memory_size: int, memory_address_bytes: int = 4, memory_size_bytes: int = 4, data_format: int = 0x00): data = bytes([data_format]) if memory_address_bytes < 1 or memory_address_bytes > 4: - raise ValueError('invalid memory_address_bytes: {}'.format(memory_address_bytes)) + raise ValueError(f'invalid memory_address_bytes: {memory_address_bytes}') if memory_size_bytes < 1 or memory_size_bytes > 4: - raise ValueError('invalid memory_size_bytes: {}'.format(memory_size_bytes)) + raise ValueError(f'invalid memory_size_bytes: {memory_size_bytes}') data += bytes([memory_size_bytes << 4 | memory_address_bytes]) if memory_address >= 1 << (memory_address_bytes * 8): - raise ValueError('invalid memory_address: {}'.format(memory_address)) + raise ValueError(f'invalid memory_address: {memory_address}') data += struct.pack('!I', memory_address)[4 - memory_address_bytes:] if memory_size >= 1 << (memory_size_bytes * 8): - raise ValueError('invalid memory_size: {}'.format(memory_size)) + raise ValueError(f'invalid memory_size: {memory_size}') data += struct.pack('!I', memory_size)[4 - memory_size_bytes:] resp = self._uds_request(SERVICE_TYPE.REQUEST_DOWNLOAD, subfunction=None, data=data) @@ -896,7 +902,7 @@ class UdsClient(): if max_num_bytes_len >= 1 and max_num_bytes_len <= 4: max_num_bytes = struct.unpack('!I', (b"\x00" * (4 - max_num_bytes_len)) + resp[1:max_num_bytes_len + 1])[0] else: - raise ValueError('invalid max_num_bytes_len: {}'.format(max_num_bytes_len)) + raise ValueError(f'invalid max_num_bytes_len: {max_num_bytes_len}') return max_num_bytes # max number of bytes per transfer data request @@ -904,16 +910,16 @@ class UdsClient(): data = bytes([data_format]) if memory_address_bytes < 1 or memory_address_bytes > 4: - raise ValueError('invalid memory_address_bytes: {}'.format(memory_address_bytes)) + raise ValueError(f'invalid memory_address_bytes: {memory_address_bytes}') if memory_size_bytes < 1 or memory_size_bytes > 4: - raise ValueError('invalid memory_size_bytes: {}'.format(memory_size_bytes)) + raise ValueError(f'invalid memory_size_bytes: {memory_size_bytes}') data += bytes([memory_size_bytes << 4 | memory_address_bytes]) if memory_address >= 1 << (memory_address_bytes * 8): - raise ValueError('invalid memory_address: {}'.format(memory_address)) + raise ValueError(f'invalid memory_address: {memory_address}') data += struct.pack('!I', memory_address)[4 - memory_address_bytes:] if memory_size >= 1 << (memory_size_bytes * 8): - raise ValueError('invalid memory_size: {}'.format(memory_size)) + raise ValueError(f'invalid memory_size: {memory_size}') data += struct.pack('!I', memory_size)[4 - memory_size_bytes:] resp = self._uds_request(SERVICE_TYPE.REQUEST_UPLOAD, subfunction=None, data=data) @@ -921,7 +927,7 @@ class UdsClient(): if max_num_bytes_len >= 1 and max_num_bytes_len <= 4: max_num_bytes = struct.unpack('!I', (b"\x00" * (4 - max_num_bytes_len)) + resp[1:max_num_bytes_len + 1])[0] else: - raise ValueError('invalid max_num_bytes_len: {}'.format(max_num_bytes_len)) + raise ValueError(f'invalid max_num_bytes_len: {max_num_bytes_len}') return max_num_bytes # max number of bytes per transfer data request @@ -930,7 +936,7 @@ class UdsClient(): resp = self._uds_request(SERVICE_TYPE.TRANSFER_DATA, subfunction=None, data=data) resp_id = resp[0] if len(resp) > 0 else None if resp_id != block_sequence_count: - raise ValueError('invalid block_sequence_count: {}'.format(resp_id)) + raise ValueError(f'invalid block_sequence_count: {resp_id}') return resp[1:] def request_transfer_exit(self): diff --git a/panda/python/xcp.py b/panda/python/xcp.py new file mode 100644 index 0000000000..bb294046ec --- /dev/null +++ b/panda/python/xcp.py @@ -0,0 +1,258 @@ +import sys +import time +import struct +from enum import IntEnum + +class COMMAND_CODE(IntEnum): + CONNECT = 0xFF + DISCONNECT = 0xFE + GET_STATUS = 0xFD + SYNCH = 0xFC + GET_COMM_MODE_INFO = 0xFB + GET_ID = 0xFA + SET_REQUEST = 0xF9 + GET_SEED = 0xF8 + UNLOCK = 0xF7 + SET_MTA = 0xF6 + UPLOAD = 0xF5 + SHORT_UPLOAD = 0xF4 + BUILD_CHECKSUM = 0xF3 + TRANSPORT_LAYER_CMD = 0xF2 + USER_CMD = 0xF1 + DOWNLOAD = 0xF0 + DOWNLOAD_NEXT = 0xEF + DOWNLOAD_MAX = 0xEE + SHORT_DOWNLOAD = 0xED + MODIFY_BITS = 0xEC + SET_CAL_PAGE = 0xEB + GET_CAL_PAGE = 0xEA + GET_PAG_PROCESSOR_INFO = 0xE9 + GET_SEGMENT_INFO = 0xE8 + GET_PAGE_INFO = 0xE7 + SET_SEGMENT_MODE = 0xE6 + GET_SEGMENT_MODE = 0xE5 + COPY_CAL_PAGE = 0xE4 + CLEAR_DAQ_LIST = 0xE3 + SET_DAQ_PTR = 0xE2 + WRITE_DAQ = 0xE1 + SET_DAQ_LIST_MODE = 0xE0 + GET_DAQ_LIST_MODE = 0xDF + START_STOP_DAQ_LIST = 0xDE + START_STOP_SYNCH = 0xDD + GET_DAQ_CLOCK = 0xDC + READ_DAQ = 0xDB + GET_DAQ_PROCESSOR_INFO = 0xDA + GET_DAQ_RESOLUTION_INFO = 0xD9 + GET_DAQ_LIST_INFO = 0xD8 + GET_DAQ_EVENT_INFO = 0xD7 + FREE_DAQ = 0xD6 + ALLOC_DAQ = 0xD5 + ALLOC_ODT = 0xD4 + ALLOC_ODT_ENTRY = 0xD3 + PROGRAM_START = 0xD2 + PROGRAM_CLEAR = 0xD1 + PROGRAM = 0xD0 + PROGRAM_RESET = 0xCF + GET_PGM_PROCESSOR_INFO = 0xCE + GET_SECTOR_INFO = 0xCD + PROGRAM_PREPARE = 0xCC + PROGRAM_FORMAT = 0xCB + PROGRAM_NEXT = 0xCA + PROGRAM_MAX = 0xC9 + PROGRAM_VERIFY = 0xC8 + +ERROR_CODES = { + 0x00: "Command processor synchronization", + 0x10: "Command was not executed", + 0x11: "Command rejected because DAQ is running", + 0x12: "Command rejected because PGM is running", + 0x20: "Unknown command or not implemented optional command", + 0x21: "Command syntax invalid", + 0x22: "Command syntax valid but command parameter(s) out of range", + 0x23: "The memory location is write protected", + 0x24: "The memory location is not accessible", + 0x25: "Access denied, Seed & Key is required", + 0x26: "Selected page not available", + 0x27: "Selected page mode not available", + 0x28: "Selected segment not valid", + 0x29: "Sequence error", + 0x2A: "DAQ configuration not valid", + 0x30: "Memory overflow error", + 0x31: "Generic error", + 0x32: "The slave internal program verify routine detects an error", +} + +class CONNECT_MODE(IntEnum): + NORMAL = 0x00, + USER_DEFINED = 0x01, + +class GET_ID_REQUEST_TYPE(IntEnum): + ASCII = 0x00, + ASAM_MC2_FILE = 0x01, + ASAM_MC2_PATH = 0x02, + ASAM_MC2_URL = 0x03, + ASAM_MC2_UPLOAD = 0x04, + # 128-255 user defined + +class CommandTimeoutError(Exception): + pass + +class CommandCounterError(Exception): + pass + +class CommandResponseError(Exception): + def __init__(self, message, return_code): + super().__init__() + self.message = message + self.return_code = return_code + + def __str__(self): + return self.message + +class XcpClient(): + def __init__(self, panda, tx_addr: int, rx_addr: int, bus: int=0, timeout: float=0.1, debug=False, pad=True): + self.tx_addr = tx_addr + self.rx_addr = rx_addr + self.can_bus = bus + self.timeout = timeout + self.debug = debug + self._panda = panda + self._byte_order = ">" + self._max_cto = 8 + self._max_dto = 8 + self.pad = pad + + def _send_cto(self, cmd: int, dat: bytes = b"") -> None: + tx_data = (bytes([cmd]) + dat) + + # Some ECUs don't respond if the packets are not padded to 8 bytes + if self.pad: + tx_data = tx_data.ljust(8, b"\x00") + + if self.debug: + print("CAN-CLEAR: TX") + self._panda.can_clear(self.can_bus) + if self.debug: + print("CAN-CLEAR: RX") + self._panda.can_clear(0xFFFF) + if self.debug: + print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(tx_data)}") + self._panda.can_send(self.tx_addr, tx_data, self.can_bus) + + def _recv_dto(self, timeout: float) -> bytes: + start_time = time.time() + while time.time() - start_time < timeout: + msgs = self._panda.can_recv() or [] + if len(msgs) >= 256: + print("CAN RX buffer overflow!!!", file=sys.stderr) + for rx_addr, _, rx_data, rx_bus in msgs: + if rx_bus == self.can_bus and rx_addr == self.rx_addr: + rx_data = bytes(rx_data) # convert bytearray to bytes + if self.debug: + print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}") + + pid = rx_data[0] + if pid == 0xFE: + err = rx_data[1] + err_desc = ERROR_CODES.get(err, "unknown error") + dat = rx_data[2:] + raise CommandResponseError(f"{hex(err)} - {err_desc} {dat}", err) + + return bytes(rx_data[1:]) + time.sleep(0.001) + + raise CommandTimeoutError("timeout waiting for response") + + # commands + def connect(self, connect_mode: CONNECT_MODE=CONNECT_MODE.NORMAL) -> dict: + self._send_cto(COMMAND_CODE.CONNECT, bytes([connect_mode])) + resp = self._recv_dto(self.timeout) + assert len(resp) == 7, f"incorrect data length: {len(resp)}" + self._byte_order = ">" if resp[1] & 0x01 else "<" + self._slave_block_mode = resp[1] & 0x40 != 0 + self._max_cto = resp[2] + self._max_dto = struct.unpack(f"{self._byte_order}H", resp[3:5])[0] + return { + "cal_support": resp[0] & 0x01 != 0, + "daq_support": resp[0] & 0x04 != 0, + "stim_support": resp[0] & 0x08 != 0, + "pgm_support": resp[0] & 0x10 != 0, + "byte_order": self._byte_order, + "address_granularity": 2**((resp[1] & 0x06) >> 1), + "slave_block_mode": self._slave_block_mode, + "optional": resp[1] & 0x80 != 0, + "max_cto": self._max_cto, + "max_dto": self._max_dto, + "protocol_version": resp[5], + "transport_version": resp[6], + } + + def disconnect(self) -> None: + self._send_cto(COMMAND_CODE.DISCONNECT) + resp = self._recv_dto(self.timeout) + assert len(resp) == 0, f"incorrect data length: {len(resp)}" + + def get_id(self, req_id_type: GET_ID_REQUEST_TYPE = GET_ID_REQUEST_TYPE.ASCII) -> dict: + if req_id_type > 255: + raise ValueError("request id type must be less than 255") + self._send_cto(COMMAND_CODE.GET_ID, bytes([req_id_type])) + resp = self._recv_dto(self.timeout) + return { + # mode = 0 means MTA was set + # mode = 1 means data is at end (only CAN-FD has space for this) + "mode": resp[0], + "length": struct.unpack(f"{self._byte_order}I", resp[3:7])[0], + "identifier": resp[7:] if self._max_cto > 8 else None + } + + def get_seed(self, mode: int = 0) -> bytes: + if mode > 255: + raise ValueError("mode must be less than 255") + self._send_cto(COMMAND_CODE.GET_SEED, bytes([0, mode])) + + # TODO: add support for longer seeds spread over multiple blocks + ret = self._recv_dto(self.timeout) + length = ret[0] + return ret[1:length+1] + + def unlock(self, key: bytes) -> bytes: + # TODO: add support for longer keys spread over multiple blocks + self._send_cto(COMMAND_CODE.UNLOCK, bytes([len(key)]) + key) + return self._recv_dto(self.timeout) + + def set_mta(self, addr: int, addr_ext: int = 0) -> bytes: + if addr_ext > 255: + raise ValueError("address extension must be less than 256") + # TODO: this looks broken (missing addr extension) + self._send_cto(COMMAND_CODE.SET_MTA, bytes([0x00, 0x00, addr_ext]) + struct.pack(f"{self._byte_order}I", addr)) + return self._recv_dto(self.timeout) + + def upload(self, size: int) -> bytes: + if size > 255: + raise ValueError("size must be less than 256") + if not self._slave_block_mode and size > self._max_dto - 1: + raise ValueError("block mode not supported") + + self._send_cto(COMMAND_CODE.UPLOAD, bytes([size])) + resp = b"" + while len(resp) < size: + resp += self._recv_dto(self.timeout)[:size - len(resp) + 1] + return resp[:size] # trim off bytes with undefined values + + def short_upload(self, size: int, addr_ext: int, addr: int) -> bytes: + if size > 6: + raise ValueError("size must be less than 7") + if addr_ext > 255: + raise ValueError("address extension must be less than 256") + self._send_cto(COMMAND_CODE.SHORT_UPLOAD, bytes([size, 0x00, addr_ext]) + struct.pack(f"{self._byte_order}I", addr)) + return self._recv_dto(self.timeout)[:size] # trim off bytes with undefined values + + def download(self, data: bytes) -> bytes: + size = len(data) + if size > 255: + raise ValueError("size must be less than 256") + if not self._slave_block_mode and size > self._max_dto - 2: + raise ValueError("block mode not supported") + + self._send_cto(COMMAND_CODE.DOWNLOAD, bytes([size]) + data) + return self._recv_dto(self.timeout)[:size] diff --git a/panda/release/.gitignore b/panda/release/.gitignore new file mode 100644 index 0000000000..c4c4ffc6aa --- /dev/null +++ b/panda/release/.gitignore @@ -0,0 +1 @@ +*.zip diff --git a/panda/release/make_release.sh b/panda/release/make_release.sh new file mode 100755 index 0000000000..5628c4998d --- /dev/null +++ b/panda/release/make_release.sh @@ -0,0 +1,22 @@ +#!/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +export CERT=/home/batman/xx/pandaextra/certs/release + +if [ ! -f "$CERT" ]; then + echo "No release cert found, cannot build release." + echo "You probably aren't looking to do this anyway." + exit +fi + +export RELEASE=1 +export BUILDER=DEV + +cd $DIR/../board +scons -u -c +rm obj/* +scons -u +cd obj +RELEASE_NAME=$(awk '{print $1}' version) +zip -j ../../release/panda-$RELEASE_NAME.zip version panda.bin.signed bootstub.panda.bin panda_h7.bin.signed bootstub.panda_h7.bin diff --git a/panda/requirements.txt b/panda/requirements.txt new file mode 100644 index 0000000000..5ee8636fd5 --- /dev/null +++ b/panda/requirements.txt @@ -0,0 +1,18 @@ +ruff +libusb1 +numpy +hexdump +pycryptodome +tqdm>=4.14.0 +pytest +pytest-mock +pytest-xdist +pytest-timeout +pytest-randomly +parameterized +cffi +pre-commit +scons>=4.4.0 +flaky +spidev +termcolor \ No newline at end of file diff --git a/panda/setup.cfg b/panda/setup.cfg new file mode 100644 index 0000000000..3480374bc2 --- /dev/null +++ b/panda/setup.cfg @@ -0,0 +1,2 @@ +[bdist_wheel] +universal=1 \ No newline at end of file diff --git a/panda/setup.py b/panda/setup.py new file mode 100644 index 0000000000..25ee669236 --- /dev/null +++ b/panda/setup.py @@ -0,0 +1,63 @@ +""" + Panda CAN Controller Dongle + ~~~~~ + + Setup + ````` + + $ pip install . # or python setup.py install +""" + +import codecs +import os +import re +from setuptools import setup + +here = os.path.abspath(os.path.dirname(__file__)) + +def read(*parts): + """Taken from pypa pip setup.py: + intentionally *not* adding an encoding option to open, See: + https://github.com/pypa/virtualenv/issues/201#issuecomment-3145690 + """ + return codecs.open(os.path.join(here, *parts), 'r').read() + + +def find_version(*file_paths): + version_file = read(*file_paths) + version_match = re.search(r"^__version__ = ['\"]([^'\"]*)['\"]", + version_file, re.M) + if version_match: + return version_match.group(1) + raise RuntimeError("Unable to find version string.") + +setup( + name='pandacan', + version=find_version("python", "__init__.py"), + url='https://github.com/commaai/panda', + author='comma.ai', + author_email='', + packages=[ + 'panda', + ], + package_dir={'panda': 'python'}, + platforms='any', + license='MIT', + install_requires=[ + 'libusb1 == 2.0.1', + 'hexdump >= 3.3', + 'pycryptodome >= 3.9.8', + 'tqdm >= 4.14.0', + 'requests' + ], + ext_modules=[], + description="Code powering the comma.ai panda", + long_description='See https://github.com/commaai/panda', + classifiers=[ + 'Development Status :: 2 - Pre-Alpha', + "Natural Language :: English", + "Programming Language :: Python :: 2", + "Programming Language :: Python :: 3", + "Topic :: System :: Hardware", + ], +) diff --git a/selfdrive/manager/__init__.py b/panda/tests/__init__.py similarity index 100% rename from selfdrive/manager/__init__.py rename to panda/tests/__init__.py diff --git a/panda/tests/benchmark.py b/panda/tests/benchmark.py new file mode 100755 index 0000000000..c2b0c85c93 --- /dev/null +++ b/panda/tests/benchmark.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import time +from contextlib import contextmanager + +from panda import Panda, PandaDFU +from panda.tests.hitl.helpers import get_random_can_messages + + +@contextmanager +def print_time(desc): + start = time.perf_counter() + yield + end = time.perf_counter() + print(f"{end - start:.2f}s - {desc}") + + +if __name__ == "__main__": + with print_time("Panda()"): + p = Panda() + + with print_time("PandaDFU.list()"): + PandaDFU.list() + + fxn = [ + 'reset', + 'reconnect', + 'up_to_date', + 'health', + #'flash', + ] + for f in fxn: + with print_time(f"Panda.{f}()"): + getattr(p, f)() + + p.set_can_loopback(True) + + for n in range(6): + msgs = get_random_can_messages(int(10**n)) + with print_time(f"Panda.can_send_many() - {len(msgs)} msgs"): + p.can_send_many(msgs) + + with print_time("Panda.can_recv()"): + m = p.can_recv() diff --git a/panda/tests/black_white_loopback_test.py b/panda/tests/black_white_loopback_test.py new file mode 100755 index 0000000000..8198a30d5f --- /dev/null +++ b/panda/tests/black_white_loopback_test.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 + +# Loopback test between black panda (+ harness and power) and white/grey panda +# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test. +# To be sure, the test should be run with both harness orientations + + +import os +import time +import random +import argparse +from panda import Panda + +def get_test_string(): + return b"test" + os.urandom(10) + +counter = 0 +nonzero_bus_errors = 0 +zero_bus_errors = 0 +content_errors = 0 + +def run_test(sleep_duration): + global counter + + pandas = Panda.list() + print(pandas) + + # make sure two pandas are connected + if len(pandas) != 2: + raise Exception("Connect white/grey and black panda to run this test!") + + # connect + pandas[0] = Panda(pandas[0]) + pandas[1] = Panda(pandas[1]) + + black_panda = None + other_panda = None + + # find out which one is black + if pandas[0].is_black() and not pandas[1].is_black(): + black_panda = pandas[0] + other_panda = pandas[1] + elif not pandas[0].is_black() and pandas[1].is_black(): + black_panda = pandas[1] + other_panda = pandas[0] + else: + raise Exception("Connect white/grey and black panda to run this test!") + + # disable safety modes + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # test health packet + print("black panda health", black_panda.health()) + print("other panda health", other_panda.health()) + + # test black -> other + while True: + test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration) + test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration) + counter += 1 + print("Number of cycles:", counter, "Non-zero bus errors:", nonzero_bus_errors, "Zero bus errors:", zero_bus_errors, "Content errors:", content_errors) + + # Toggle relay + black_panda.set_safety_mode(Panda.SAFETY_SILENT) + time.sleep(1) + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + time.sleep(1) + + +def test_buses(black_panda, other_panda, direction, test_array, sleep_duration): + global nonzero_bus_errors, zero_bus_errors, content_errors + + if direction: + print("***************** TESTING (BLACK --> OTHER) *****************") + else: + print("***************** TESTING (OTHER --> BLACK) *****************") + + for send_bus, obd, recv_buses in test_array: + black_panda.send_heartbeat() + other_panda.send_heartbeat() + print("\ntest can: ", send_bus, " OBD: ", obd) + + # set OBD on black panda + black_panda.set_obd(True if obd else None) + + # clear and flush + if direction: + black_panda.can_clear(send_bus) + else: + other_panda.can_clear(send_bus) + + for recv_bus in recv_buses: + if direction: + other_panda.can_clear(recv_bus) + else: + black_panda.can_clear(recv_bus) + + black_panda.can_recv() + other_panda.can_recv() + + # send the characters + at = random.randint(1, 2000) + st = get_test_string()[0:8] + if direction: + black_panda.can_send(at, st, send_bus) + else: + other_panda.can_send(at, st, send_bus) + time.sleep(0.1) + + # check for receive + if direction: + _ = black_panda.can_recv() # can echo + cans_loop = other_panda.can_recv() + else: + _ = other_panda.can_recv() # can echo + cans_loop = black_panda.can_recv() + + loop_buses = [] + for loop in cans_loop: + if (loop[0] != at) or (loop[2] != st): + content_errors += 1 + + print(" Loop on bus", str(loop[3])) + loop_buses.append(loop[3]) + if len(cans_loop) == 0: + print(" No loop") + assert not os.getenv("NOASSERT") + + # test loop buses + recv_buses.sort() + loop_buses.sort() + if(recv_buses != loop_buses): + if len(loop_buses) == 0: + zero_bus_errors += 1 + else: + nonzero_bus_errors += 1 + assert not os.getenv("NOASSERT") + else: + print(" TEST PASSED") + + time.sleep(sleep_duration) + print("\n") + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", type=int, help="Number of test iterations to run") + parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0) + args = parser.parse_args() + + if args.n is None: + while True: + run_test(sleep_duration=args.sleep) + else: + for _ in range(args.n): + run_test(sleep_duration=args.sleep) diff --git a/panda/tests/black_white_relay_endurance.py b/panda/tests/black_white_relay_endurance.py new file mode 100755 index 0000000000..005277c15a --- /dev/null +++ b/panda/tests/black_white_relay_endurance.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 + +# Loopback test between black panda (+ harness and power) and white/grey panda +# Tests all buses, including OBD CAN, which is on the same bus as CAN0 in this test. +# To be sure, the test should be run with both harness orientations + + +import os +import time +import random +import argparse + +from panda import Panda + +def get_test_string(): + return b"test" + os.urandom(10) + +counter = 0 +nonzero_bus_errors = 0 +zero_bus_errors = 0 +content_errors = 0 + +def run_test(sleep_duration): + global counter + + pandas = Panda.list() + print(pandas) + + # make sure two pandas are connected + if len(pandas) != 2: + raise Exception("Connect white/grey and black panda to run this test!") + + # connect + pandas[0] = Panda(pandas[0]) + pandas[1] = Panda(pandas[1]) + + black_panda = None + other_panda = None + + # find out which one is black + if pandas[0].is_black() and not pandas[1].is_black(): + black_panda = pandas[0] + other_panda = pandas[1] + elif not pandas[0].is_black() and pandas[1].is_black(): + black_panda = pandas[1] + other_panda = pandas[0] + else: + raise Exception("Connect white/grey and black panda to run this test!") + + # disable safety modes + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # test health packet + print("black panda health", black_panda.health()) + print("other panda health", other_panda.health()) + + # test black -> other + start_time = time.time() + temp_start_time = start_time + while True: + test_buses(black_panda, other_panda, True, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (1, True, [0])], sleep_duration) + test_buses(black_panda, other_panda, False, [(0, False, [0]), (1, False, [1]), (2, False, [2]), (0, True, [0, 1])], sleep_duration) + counter += 1 + + runtime = time.time() - start_time + print("Number of cycles:", counter, "Non-zero bus errors:", nonzero_bus_errors, "Zero bus errors:", zero_bus_errors, + "Content errors:", content_errors, "Runtime: ", runtime) + + if (time.time() - temp_start_time) > 3600 * 6: + # Toggle relay + black_panda.set_safety_mode(Panda.SAFETY_SILENT) + time.sleep(1) + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + time.sleep(1) + temp_start_time = time.time() + + +def test_buses(black_panda, other_panda, direction, test_array, sleep_duration): + global nonzero_bus_errors, zero_bus_errors, content_errors + + if direction: + print("***************** TESTING (BLACK --> OTHER) *****************") + else: + print("***************** TESTING (OTHER --> BLACK) *****************") + + for send_bus, obd, recv_buses in test_array: + black_panda.send_heartbeat() + other_panda.send_heartbeat() + print("\ntest can: ", send_bus, " OBD: ", obd) + + # set OBD on black panda + black_panda.set_obd(True if obd else None) + + # clear and flush + if direction: + black_panda.can_clear(send_bus) + else: + other_panda.can_clear(send_bus) + + for recv_bus in recv_buses: + if direction: + other_panda.can_clear(recv_bus) + else: + black_panda.can_clear(recv_bus) + + black_panda.can_recv() + other_panda.can_recv() + + # send the characters + at = random.randint(1, 2000) + st = get_test_string()[0:8] + if direction: + black_panda.can_send(at, st, send_bus) + else: + other_panda.can_send(at, st, send_bus) + time.sleep(0.1) + + # check for receive + if direction: + _ = black_panda.can_recv() # cans echo + cans_loop = other_panda.can_recv() + else: + _ = other_panda.can_recv() # cans echo + cans_loop = black_panda.can_recv() + + loop_buses = [] + for loop in cans_loop: + if (loop[0] != at) or (loop[2] != st): + content_errors += 1 + + print(" Loop on bus", str(loop[3])) + loop_buses.append(loop[3]) + if len(cans_loop) == 0: + print(" No loop") + assert os.getenv("NOASSERT") + + # test loop buses + recv_buses.sort() + loop_buses.sort() + if(recv_buses != loop_buses): + if len(loop_buses) == 0: + zero_bus_errors += 1 + else: + nonzero_bus_errors += 1 + assert os.getenv("NOASSERT") + else: + print(" TEST PASSED") + + time.sleep(sleep_duration) + print("\n") + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", type=int, help="Number of test iterations to run") + parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0) + args = parser.parse_args() + + if args.n is None: + while True: + run_test(sleep_duration=args.sleep) + else: + for _ in range(args.n): + run_test(sleep_duration=args.sleep) diff --git a/panda/tests/black_white_relay_test.py b/panda/tests/black_white_relay_test.py new file mode 100755 index 0000000000..38db06a1a5 --- /dev/null +++ b/panda/tests/black_white_relay_test.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 + +# Relay test with loopback between black panda (+ harness and power) and white/grey panda +# Tests the relay switching multiple times / second by looking at the buses on which loop occurs. + + +import os +import time +import random +import argparse + +from panda import Panda + +def get_test_string(): + return b"test" + os.urandom(10) + +counter = 0 +open_errors = 0 +closed_errors = 0 +content_errors = 0 + +def run_test(sleep_duration): + global counter, open_errors, closed_errors + + pandas = Panda.list() + print(pandas) + + # make sure two pandas are connected + if len(pandas) != 2: + raise Exception("Connect white/grey and black panda to run this test!") + + # connect + pandas[0] = Panda(pandas[0]) + pandas[1] = Panda(pandas[1]) + + # find out which one is black + type0 = pandas[0].get_type() + type1 = pandas[1].get_type() + + black_panda = None + other_panda = None + + if type0 == "\x03" and type1 != "\x03": + black_panda = pandas[0] + other_panda = pandas[1] + elif type0 != "\x03" and type1 == "\x03": + black_panda = pandas[1] + other_panda = pandas[0] + else: + raise Exception("Connect white/grey and black panda to run this test!") + + # disable safety modes + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + other_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # test health packet + print("black panda health", black_panda.health()) + print("other panda health", other_panda.health()) + + # test black -> other + while True: + # Switch on relay + black_panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + time.sleep(0.05) + + if not test_buses(black_panda, other_panda, (0, False, [0])): + open_errors += 1 + raise Exception("Open error") + + # Switch off relay + black_panda.set_safety_mode(Panda.SAFETY_SILENT) + time.sleep(0.05) + + if not test_buses(black_panda, other_panda, (0, False, [0, 2])): + closed_errors += 1 + raise Exception("Close error") + + counter += 1 + print("Number of cycles:", counter, "Open errors:", open_errors, "Closed errors:", closed_errors, "Content errors:", content_errors) + +def test_buses(black_panda, other_panda, test_obj): + global content_errors + send_bus, obd, recv_buses = test_obj + + black_panda.send_heartbeat() + other_panda.send_heartbeat() + + # Set OBD on send panda + other_panda.set_obd(True if obd else None) + + # clear and flush + other_panda.can_clear(send_bus) + + for recv_bus in recv_buses: + black_panda.can_clear(recv_bus) + + black_panda.can_recv() + other_panda.can_recv() + + # send the characters + at = random.randint(1, 2000) + st = get_test_string()[0:8] + other_panda.can_send(at, st, send_bus) + time.sleep(0.05) + + # check for receive + _ = other_panda.can_recv() # can echo + cans_loop = black_panda.can_recv() + + loop_buses = [] + for loop in cans_loop: + if (loop[0] != at) or (loop[2] != st): + content_errors += 1 + loop_buses.append(loop[3]) + + # test loop buses + recv_buses.sort() + loop_buses.sort() + if(recv_buses != loop_buses): + return False + else: + return True + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", type=int, help="Number of test iterations to run") + parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0) + args = parser.parse_args() + + if args.n is None: + while True: + run_test(sleep_duration=args.sleep) + else: + for _ in range(args.n): + run_test(sleep_duration=args.sleep) diff --git a/panda/tests/bulk_write_test.py b/panda/tests/bulk_write_test.py new file mode 100755 index 0000000000..278766a199 --- /dev/null +++ b/panda/tests/bulk_write_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +import os +import time +import threading +from typing import Any + +from panda import Panda + +JUNGLE = "JUNGLE" in os.environ +if JUNGLE: + from panda import PandaJungle + +# The TX buffers on pandas is 0x100 in length. +NUM_MESSAGES_PER_BUS = 10000 + +def flood_tx(panda): + print('Sending!') + msg = b"\xaa" * 4 + packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + panda.can_send_many(packet, timeout=10000) + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + +if __name__ == "__main__": + serials = Panda.list() + if JUNGLE: + sender = Panda() + receiver = PandaJungle() + else: + if len(serials) != 2: + raise Exception("Connect two pandas to perform this test!") + sender = Panda(serials[0]) + receiver = Panda(serials[1]) # type: ignore + receiver.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # Start transmisson + threading.Thread(target=flood_tx, args=(sender,)).start() + + # Receive as much as we can in a few second time period + rx: list[Any] = [] + old_len = 0 + start_time = time.time() + while time.time() - start_time < 3 or len(rx) > old_len: + old_len = len(rx) + print(old_len) + rx.extend(receiver.can_recv()) + print(f"Received {len(rx)} messages") diff --git a/panda/tests/can_printer.py b/panda/tests/can_printer.py new file mode 100755 index 0000000000..15ce89f688 --- /dev/null +++ b/panda/tests/can_printer.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +import os +import time +from collections import defaultdict +import binascii + +from panda import Panda + +# fake +def sec_since_boot(): + return time.time() + +def can_printer(): + p = Panda() + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + start = sec_since_boot() + lp = sec_since_boot() + msgs = defaultdict(list) + canbus = int(os.getenv("CAN", "0")) + while True: + can_recv = p.can_recv() + for address, _, dat, src in can_recv: + if src == canbus: + msgs[address].append(dat) + + if sec_since_boot() - lp > 0.1: + dd = chr(27) + "[2J" + dd += "%5.2f\n" % (sec_since_boot() - start) + for k, v in sorted(zip(list(msgs.keys()), [binascii.hexlify(x[-1]) for x in list(msgs.values())], strict=True)): + dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k, k), len(msgs[k]), v) + print(dd) + lp = sec_since_boot() + +if __name__ == "__main__": + can_printer() diff --git a/panda/tests/canfd/test_canfd.py b/panda/tests/canfd/test_canfd.py new file mode 100755 index 0000000000..873bc796ba --- /dev/null +++ b/panda/tests/canfd/test_canfd.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +import os +import time +import random +from collections import defaultdict +from panda import Panda, calculate_checksum, DLC_TO_LEN +from panda import PandaJungle +from panda.tests.hitl.helpers import time_many_sends + +H7_HW_TYPES = [Panda.HW_TYPE_RED_PANDA, Panda.HW_TYPE_RED_PANDA_V2] +JUNGLE_SERIAL = os.getenv("JUNGLE") +H7_PANDAS_EXCLUDE = [] # type: ignore +if os.getenv("H7_PANDAS_EXCLUDE"): + H7_PANDAS_EXCLUDE = os.getenv("H7_PANDAS_EXCLUDE").strip().split(" ") # type: ignore + +def panda_reset(): + panda_serials = [] + + panda_jungle = PandaJungle(JUNGLE_SERIAL) + panda_jungle.set_can_silent(True) + panda_jungle.set_panda_power(False) + time.sleep(1) + panda_jungle.set_panda_power(True) + time.sleep(4) + + for serial in Panda.list(): + if serial not in H7_PANDAS_EXCLUDE: + with Panda(serial=serial) as p: + if p.get_type() in H7_HW_TYPES: + p.reset() + panda_serials.append(serial) + + print("test pandas", panda_serials) + assert len(panda_serials) == 2, "Two H7 pandas required" + + return panda_serials + +def panda_init(serial, enable_canfd=False, enable_non_iso=False): + p = Panda(serial=serial) + p.set_power_save(False) + for bus in range(3): + p.set_can_speed_kbps(0, 500) + if enable_canfd: + p.set_can_data_speed_kbps(bus, 2000) + if enable_non_iso: + p.set_canfd_non_iso(bus, True) + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + return p + +def test_canfd_throughput(p, p_recv=None): + two_pandas = p_recv is not None + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + if two_pandas: + p_recv.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + # enable output mode + else: + p.set_can_loopback(True) + + tests = [ + [500, 1000, 2000], # speeds + [93, 87, 78], # saturation thresholds + ] + + for i in range(len(tests[0])): + # set bus 0 data speed to speed + p.set_can_data_speed_kbps(0, tests[0][i]) + if p_recv is not None: + p_recv.set_can_data_speed_kbps(0, tests[0][i]) + time.sleep(0.05) + + comp_kbps = time_many_sends(p, 0, p_recv=p_recv, msg_count=400, two_pandas=two_pandas, msg_len=64) + + # bit count from https://en.wikipedia.org/wiki/CAN_bus + saturation_pct = (comp_kbps / tests[0][i]) * 100.0 + assert saturation_pct > tests[1][i] + assert saturation_pct < 100 + +def canfd_test(p_send, p_recv): + for n in range(100): + sent_msgs = defaultdict(set) + to_send = [] + for _ in range(200): + bus = random.randrange(3) + for dlc in range(len(DLC_TO_LEN)): + address = random.randrange(1, 1<<29) + data = bytearray(random.getrandbits(8) for _ in range(DLC_TO_LEN[dlc])) + if len(data) >= 2: + data[0] = calculate_checksum(data[1:] + bytes(str(address), encoding="utf-8")) + to_send.append([address, 0, data, bus]) + sent_msgs[bus].add((address, bytes(data))) + + p_send.can_send_many(to_send, timeout=0) + + start_time = time.monotonic() + while (time.monotonic() - start_time < 1) and any(len(x) > 0 for x in sent_msgs.values()): + incoming = p_recv.can_recv() + for msg in incoming: + address, _, data, bus = msg + if len(data) >= 2: + assert calculate_checksum(data[1:] + bytes(str(address), encoding="utf-8")) == data[0] + k = (address, bytes(data)) + assert k in sent_msgs[bus], f"message {k} was never sent on bus {bus}" + sent_msgs[bus].discard(k) + + for bus in range(3): + assert not len(sent_msgs[bus]), f"loop {n}: bus {bus} missing {len(sent_msgs[bus])} messages" + +def setup_test(enable_non_iso=False): + panda_serials = panda_reset() + + p_send = panda_init(panda_serials[0], enable_canfd=False, enable_non_iso=enable_non_iso) + p_recv = panda_init(panda_serials[1], enable_canfd=True, enable_non_iso=enable_non_iso) + + # Check that sending panda CAN FD and BRS are turned off + for bus in range(3): + health = p_send.can_health(bus) + assert not health["canfd_enabled"] + assert not health["brs_enabled"] + assert health["canfd_non_iso"] == enable_non_iso + + # Receiving panda sends dummy CAN FD message that should enable CAN FD on sender side + for bus in range(3): + p_recv.can_send(0x200, b"dummymessage", bus) + p_recv.can_recv() + p_send.can_recv() + + # Check if all tested buses on sending panda have swithed to CAN FD with BRS + for bus in range(3): + health = p_send.can_health(bus) + assert health["canfd_enabled"] + assert health["brs_enabled"] + assert health["canfd_non_iso"] == enable_non_iso + + return p_send, p_recv + +def main(): + print("[TEST CAN-FD]") + p_send, p_recv = setup_test() + canfd_test(p_send, p_recv) + + print("[TEST CAN-FD non-ISO]") + p_send, p_recv = setup_test(enable_non_iso=True) + canfd_test(p_send, p_recv) + + print("[TEST CAN-FD THROUGHPUT]") + panda_serials = panda_reset() + p_send = panda_init(panda_serials[0], enable_canfd=True) + p_recv = panda_init(panda_serials[1], enable_canfd=True) + test_canfd_throughput(p_send, p_recv) + +if __name__ == "__main__": + main() diff --git a/panda/tests/check_fw_size.py b/panda/tests/check_fw_size.py new file mode 100755 index 0000000000..53681c5a3e --- /dev/null +++ b/panda/tests/check_fw_size.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import subprocess +from collections import defaultdict + + +def check_space(file, mcu): + MCUS = { + "H7": { + ".flash": 1024*1024, # FLASH + ".dtcmram": 128*1024, # DTCMRAM + ".itcmram": 64*1024, # ITCMRAM + ".axisram": 320*1024, # AXI SRAM + ".sram12": 32*1024, # SRAM1(16kb) + SRAM2(16kb) + ".sram4": 16*1024, # SRAM4 + ".backup_sram": 4*1024, # SRAM4 + }, + "F4": { + ".flash": 1024*1024, # FLASH + ".dtcmram": 256*1024, # RAM + ".ram_d1": 64*1024, # RAM2 + }, + } + IGNORE_LIST = [ + ".ARM.attributes", + ".comment", + ".debug_line", + ".debug_info", + ".debug_abbrev", + ".debug_aranges", + ".debug_str", + ".debug_ranges", + ".debug_loc", + ".debug_frame", + ".debug_line_str", + ".debug_rnglists", + ".debug_loclists", + ] + FLASH = [ + ".isr_vector", + ".text", + ".rodata", + ".data" + ] + RAM = [ + ".data", + ".bss", + "._user_heap_stack" # _user_heap_stack considered free? + ] + + result = {} + calcs = defaultdict(int) + + output = str(subprocess.check_output(f"arm-none-eabi-size -x --format=sysv {file}", shell=True), 'utf-8') + + for row in output.split('\n'): + pop = False + line = row.split() + if len(line) == 3 and line[0].startswith('.'): + if line[0] in IGNORE_LIST: + continue + result[line[0]] = [line[1], line[2]] + if line[0] in FLASH: + calcs[".flash"] += int(line[1], 16) + pop = True + if line[0] in RAM: + calcs[".dtcmram"] += int(line[1], 16) + pop = True + if pop: + result.pop(line[0]) + + if len(result): + for line in result: + calcs[line] += int(result[line][0], 16) + + print(f"=======SUMMARY FOR {mcu} FILE {file}=======") + for line in calcs: + if line in MCUS[mcu]: + used_percent = (100 - (MCUS[mcu][line] - calcs[line]) / MCUS[mcu][line] * 100) + print(f"SECTION: {line} size: {MCUS[mcu][line]} USED: {calcs[line]}({used_percent:.2f}%) FREE: {MCUS[mcu][line] - calcs[line]}") + else: + print(line, calcs[line]) + print() + + +if __name__ == "__main__": + # red panda + check_space("../board/obj/bootstub.panda_h7.elf", "H7") + check_space("../board/obj/panda_h7.elf", "H7") + # black panda + check_space("../board/obj/bootstub.panda.elf", "F4") + check_space("../board/obj/panda.elf", "F4") + # jungle v1 + check_space("../board/jungle/obj/bootstub.panda_jungle.elf", "F4") + check_space("../board/jungle/obj/panda_jungle.elf", "F4") + # jungle v2 + check_space("../board/jungle/obj/bootstub.panda_jungle_h7.elf", "H7") + check_space("../board/jungle/obj/panda_jungle_h7.elf", "H7") diff --git a/panda/tests/ci_shell.sh b/panda/tests/ci_shell.sh new file mode 100755 index 0000000000..92c0f96e8a --- /dev/null +++ b/panda/tests/ci_shell.sh @@ -0,0 +1,20 @@ +#!/bin/bash -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +OP_ROOT="$DIR/../../" +PANDA_ROOT="$DIR/../" + +if [ -z "$BUILD" ]; then + docker pull docker.io/commaai/panda:latest +else + docker build --cache-from docker.io/commaai/panda:latest -t docker.io/commaai/panda:latest -f $PANDA_ROOT/Dockerfile $PANDA_ROOT +fi + +docker run \ + -it \ + --rm \ + --volume $OP_ROOT:$OP_ROOT \ + --workdir $PWD \ + --env PYTHONPATH=$OP_ROOT \ + docker.io/commaai/panda:latest \ + /bin/bash diff --git a/panda/tests/debug_console.py b/panda/tests/debug_console.py new file mode 100755 index 0000000000..8755be1498 --- /dev/null +++ b/panda/tests/debug_console.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +import os +import sys +import time +import select +import codecs + +from panda import Panda + +setcolor = ["\033[1;32;40m", "\033[1;31;40m"] +unsetcolor = "\033[00m" + +port_number = int(os.getenv("PORT", "0")) +claim = os.getenv("CLAIM") is not None +no_color = os.getenv("NO_COLOR") is not None +no_reconnect = os.getenv("NO_RECONNECT") is not None + +if __name__ == "__main__": + while True: + try: + serials = Panda.list() + if os.getenv("SERIAL"): + serials = [x for x in serials if x == os.getenv("SERIAL")] + + pandas = [Panda(x, claim=claim) for x in serials] + decoders = [codecs.getincrementaldecoder('utf-8')() for _ in pandas] + + if not len(pandas): + print("no pandas found") + if no_reconnect: + sys.exit(0) + time.sleep(1) + continue + + if os.getenv("BAUD") is not None: + for panda in pandas: + panda.set_uart_baud(port_number, int(os.getenv("BAUD"))) # type: ignore + + while True: + for i, panda in enumerate(pandas): + while True: + ret = panda.serial_read(port_number) + if len(ret) > 0: + decoded = decoders[i].decode(ret) + if no_color: + sys.stdout.write(decoded) + else: + sys.stdout.write(setcolor[i] + decoded + unsetcolor) + sys.stdout.flush() + else: + break + if select.select([sys.stdin], [], [], 0) == ([sys.stdin], [], []): + ln = sys.stdin.readline() + if claim: + panda.serial_write(port_number, ln) + time.sleep(0.01) + except KeyboardInterrupt: + break + except Exception: + print("panda disconnected!") + time.sleep(0.5) diff --git a/panda/tests/development/register_hashmap_spread.py b/panda/tests/development/register_hashmap_spread.py new file mode 100755 index 0000000000..3e20e584aa --- /dev/null +++ b/panda/tests/development/register_hashmap_spread.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +import matplotlib.pyplot as plt # pylint: disable=import-error + +HASHING_PRIME = 23 +REGISTER_MAP_SIZE = 0x3FF +BYTES_PER_REG = 4 + +# From ST32F413 datasheet +REGISTER_ADDRESS_REGIONS = [ + (0x40000000, 0x40007FFF), + (0x40010000, 0x400107FF), + (0x40011000, 0x400123FF), + (0x40012C00, 0x40014BFF), + (0x40015000, 0x400153FF), + (0x40015800, 0x40015BFF), + (0x40016000, 0x400167FF), + (0x40020000, 0x40021FFF), + (0x40023000, 0x400233FF), + (0x40023800, 0x40023FFF), + (0x40026000, 0x400267FF), + (0x50000000, 0x5003FFFF), + (0x50060000, 0x500603FF), + (0x50060800, 0x50060BFF), + (0x50060800, 0x50060BFF), + (0xE0000000, 0xE00FFFFF) +] + +def _hash(reg_addr): + return (((reg_addr >> 16) ^ ((((reg_addr + 1) & 0xFFFF) * HASHING_PRIME) & 0xFFFF)) & REGISTER_MAP_SIZE) + +# Calculate hash for each address +hashes = [] +double_hashes = [] +for (start_addr, stop_addr) in REGISTER_ADDRESS_REGIONS: + for addr in range(start_addr, stop_addr + 1, BYTES_PER_REG): + h = _hash(addr) + hashes.append(h) + double_hashes.append(_hash(h)) + +# Make histograms +plt.subplot(2, 1, 1) +plt.hist(hashes, bins=REGISTER_MAP_SIZE) +plt.title("Number of collisions per _hash") +plt.xlabel("Address") + +plt.subplot(2, 1, 2) +plt.hist(double_hashes, bins=REGISTER_MAP_SIZE) +plt.title("Number of collisions per double _hash") +plt.xlabel("Address") +plt.show() diff --git a/panda/tests/echo.py b/panda/tests/echo.py new file mode 100755 index 0000000000..90bf4a8c76 --- /dev/null +++ b/panda/tests/echo.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 +from panda import Panda + +# This script is intended to be used in conjunction with the echo_loopback_test.py test script from panda jungle. +# It sends a reversed response back for every message received containing b"test". +if __name__ == "__main__": + p = Panda() + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_power_save(False) + + while True: + incoming = p.can_recv() + for message in incoming: + address, notused, data, bus = message + if b'test' in data: + p.can_send(address, data[::-1], bus) diff --git a/panda/tests/elm_car_simulator.py b/panda/tests/elm_car_simulator.py new file mode 100755 index 0000000000..56e825f5dd --- /dev/null +++ b/panda/tests/elm_car_simulator.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python3 + +"""Used to Reverse/Test ELM protocol auto detect and OBD message response without a car.""" + +import os +import sys +import struct +import binascii +import time +import threading +from collections import deque + +from panda import Panda + +def lin_checksum(dat): + return sum(dat) % 0x100 + +class ELMCarSimulator(): + def __init__(self, sn, silent=False, can_kbaud=500, + can=True, can11b=True, can29b=True, + lin=True): + self.__p = Panda(sn if sn else Panda.list()[0]) + self.__on = True + self.__stop = False + self.__silent = silent + + self.__lin_timer = None + self.__lin_active = False + self.__lin_enable = lin + self.__lin_monitor_thread = threading.Thread(target=self.__lin_monitor) + + self.__can_multipart_data = None + self.__can_kbaud = can_kbaud + self.__can_extra_noise_msgs = deque() + self.__can_enable = can + self.__can11b = can11b + self.__can29b = can29b + self.__can_monitor_thread = threading.Thread(target=self.__can_monitor) + + @property + def panda(self): + return self.__p + + def stop(self): + if self.__lin_timer: + self.__lin_timer.cancel() + self.__lin_timeout_handler() + + self.__stop = True + + def join(self): + if self.__lin_monitor_thread.is_alive(): + self.__lin_monitor_thread.join() + if self.__can_monitor_thread.is_alive(): + self.__can_monitor_thread.join() + if self.__p: + print("closing handle") + self.__p.close() + + def set_enable(self, on): + self.__on = on + + def start(self): + self.panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + if self.__lin_enable: + self.__lin_monitor_thread.start() + if self.__can_enable: + self.__can_monitor_thread.start() + + ######################### + # CAN related functions # + ######################### + + def __can_monitor(self): + print("STARTING CAN THREAD") + self.panda.set_can_speed_kbps(0, self.__can_kbaud) + self.panda.can_recv() # Toss whatever was already there + + while not self.__stop: + for address, ts, data, src in self.panda.can_recv(): + if self.__on and src == 0 and len(data) == 8 and data[0] >= 2: + if not self.__silent: + print("Processing CAN message", src, hex(address), binascii.hexlify(data)) + self.__can_process_msg(data[1], data[2], address, ts, data, src) + elif not self.__silent: + print("Rejecting CAN message", src, hex(address), binascii.hexlify(data)) + + def can_mode_11b(self): + self.__can11b = True + self.__can29b = False + + def can_mode_29b(self): + self.__can11b = False + self.__can29b = True + + def can_mode_11b_29b(self): + self.__can11b = True + self.__can29b = True + + def change_can_baud(self, kbaud): + self.__can_kbaud = kbaud + self.panda.set_can_speed_kbps(0, self.__can_kbaud) + + def can_add_extra_noise(self, noise_msg, addr=None): + self.__can_extra_noise_msgs.append((addr, noise_msg)) + + def _can_send(self, addr, msg): + if not self.__silent: + print(" CAN Reply (%x)" % addr, binascii.hexlify(msg)) + self.panda.can_send(addr, msg + b'\x00' * (8 - len(msg)), 0) + if self.__can_extra_noise_msgs: + noise = self.__can_extra_noise_msgs.popleft() + self.panda.can_send(noise[0] if noise[0] is not None else addr, + noise[1] + b'\x00' * (8 - len(noise[1])), 0) + + def _can_addr_matches(self, addr): + if self.__can11b and (addr == 0x7DF or (addr & 0x7F8) == 0x7E0): + return True + if self.__can29b and (addr == 0x18db33f1 or (addr & 0x1FFF00FF) == 0x18da00f1): + return True + return False + + def __can_process_msg(self, mode, pid, address, ts, data, src): + if not self.__silent: + print("CAN MSG", binascii.hexlify(data[1:1 + data[0]]), + "Addr:", hex(address), "Mode:", hex(mode)[2:].zfill(2), + "PID:", hex(pid)[2:].zfill(2), "canLen:", len(data), + binascii.hexlify(data)) + + if self._can_addr_matches(address) and len(data) == 8: + outmsg = None + if data[:3] == b'\x30\x00\x00' and len(self.__can_multipart_data): + if not self.__silent: + print("Request for more data") + outaddr = 0x7E8 if address == 0x7DF or address == 0x7E0 else 0x18DAF110 + msgnum = 1 + while(self.__can_multipart_data): + datalen = min(7, len(self.__can_multipart_data)) + msgpiece = struct.pack("B", 0x20 | msgnum) + self.__can_multipart_data[:datalen] + self._can_send(outaddr, msgpiece) + self.__can_multipart_data = self.__can_multipart_data[7:] + msgnum = (msgnum + 1) % 0x10 + time.sleep(0.01) + + else: + outmsg = self._process_obd(mode, pid) + + if outmsg: + outaddr = 0x7E8 if address == 0x7DF or address == 0x7E0 else 0x18DAF110 + + if len(outmsg) <= 5: + self._can_send(outaddr, + struct.pack("BBB", len(outmsg) + 2, 0x40 | data[1], pid) + outmsg) + else: + first_msg_len = min(3, len(outmsg) % 7) + payload_len = len(outmsg) + 3 + msgpiece = struct.pack("BBBBB", 0x10 | ((payload_len >> 8) & 0xF), + payload_len & 0xFF, + 0x40 | data[1], pid, 1) + outmsg[:first_msg_len] + self._can_send(outaddr, msgpiece) + self.__can_multipart_data = outmsg[first_msg_len:] + + ######################### + # General OBD functions # + ######################### + + def _process_obd(self, mode, pid): + if mode == 0x01: # Mode: Show current data + if pid == 0x00: # List supported things + return b"\xff\xff\xff\xfe" # b"\xBE\x1F\xB8\x10" #Bitfield, random features + elif pid == 0x01: # Monitor Status since DTC cleared + return b"\x00\x00\x00\x00" # Bitfield, random features + elif pid == 0x04: # Calculated engine load + return b"\x2f" + elif pid == 0x05: # Engine coolant temperature + return b"\x3c" + elif pid == 0x0B: # Intake manifold absolute pressure + return b"\x90" + elif pid == 0x0C: # Engine RPM + return b"\x1A\xF8" + elif pid == 0x0D: # Vehicle Speed + return b"\x53" + elif pid == 0x10: # MAF air flow rate + return b"\x01\xA0" + elif pid == 0x11: # Throttle Position + return b"\x90" + elif pid == 0x33: # Absolute Barometric Pressure + return b"\x90" + elif mode == 0x09: # Mode: Request vehicle information + if pid == 0x02: # Show VIN + return b"1D4GP00R55B123456" + if pid == 0xFC: # test long multi message. Ligned up for LIN responses + return b''.join(struct.pack(">BBH", 0xAA, 0xAA, num + 1) for num in range(80)) + if pid == 0xFD: # test long multi message + parts = (b'\xAA\xAA\xAA' + struct.pack(">I", num) for num in range(80)) + return b'\xAA\xAA\xAA' + b''.join(parts) + if pid == 0xFE: # test very long multi message + parts = (b'\xAA\xAA\xAA' + struct.pack(">I", num) for num in range(584)) + return b'\xAA\xAA\xAA' + b''.join(parts) + b'\xAA' + if pid == 0xFF: + return b'\xAA\x00\x00' + \ + b"".join((b'\xAA' * 5) + struct.pack(">H", num + 1) for num in range(584)) + #return b"\xAA"*100#(0xFFF-3) + + +if __name__ == "__main__": + serial = os.getenv("SERIAL") if os.getenv("SERIAL") else None + kbaud = int(os.getenv("CANKBAUD")) if os.getenv("CANKBAUD") else 500 # type: ignore + bitwidth = int(os.getenv("CANBITWIDTH")) if os.getenv("CANBITWIDTH") else 0 # type: ignore + canenable = bool(int(os.getenv("CANENABLE"))) if os.getenv("CANENABLE") else True # type: ignore + linenable = bool(int(os.getenv("LINENABLE"))) if os.getenv("LINENABLE") else True # type: ignore + sim = ELMCarSimulator(serial, can_kbaud=kbaud, can=canenable, lin=linenable) + if(bitwidth == 0): + sim.can_mode_11b_29b() + if(bitwidth == 11): + sim.can_mode_11b() + if(bitwidth == 29): + sim.can_mode_29b() + + import signal + + def signal_handler(signal, frame): + print('\nShutting down simulator') + sim.stop() + sim.join() + sys.exit(0) + + signal.signal(signal.SIGINT, signal_handler) + + sim.start() + + signal.pause() diff --git a/panda/tests/elm_throughput.py b/panda/tests/elm_throughput.py new file mode 100755 index 0000000000..983d4a1416 --- /dev/null +++ b/panda/tests/elm_throughput.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 + +import socket +import threading +import select + +class Reader(threading.Thread): + def __init__(self, s, *args, **kwargs): + super().__init__(*args, **kwargs) + self._s = s + self.__stop = False + + def stop(self): + self.__stop = True + + def run(self): + while not self.__stop: + s.recv(1000) + +def read_or_fail(s): + ready = select.select([s], [], [], 4) + assert ready[0], "Socket did not receive data within the timeout duration." + return s.recv(1000) + +def send_msg(s, msg): + s.send(msg) + res = b'' + while not res.endswith(">"): + res += read_or_fail(s) + return res + +if __name__ == "__main__": + s = socket.create_connection(("192.168.0.10", 35000)) + send_msg(s, b"ATZ\r") + send_msg(s, b"ATL1\r") + print(send_msg(s, b"ATE0\r")) + print(send_msg(s, b"ATS0\r")) + print(send_msg(s, b"ATSP6\r")) + + print("\nLOOP\n") + + while True: + print(send_msg(s, b"0100\r")) + print(send_msg(s, b"010d\r")) diff --git a/panda/tests/fan/fan_test.py b/panda/tests/fan/fan_test.py new file mode 100755 index 0000000000..36a11715b3 --- /dev/null +++ b/panda/tests/fan/fan_test.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python +import time + +from panda import Panda + +if __name__ == "__main__": + p = Panda() + power = 0 + while True: + p.set_fan_power(power) + time.sleep(5) + print("Power: ", power, "RPM:", str(p.get_fan_rpm()), "Expected:", int(6500 * power / 100)) + power += 10 + power %= 110 diff --git a/panda/tests/fan/fan_tuning.py b/panda/tests/fan/fan_tuning.py new file mode 100755 index 0000000000..2bdfab79a2 --- /dev/null +++ b/panda/tests/fan/fan_tuning.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +import json +import time +import threading + +from panda import Panda + +def drain_serial(p): + ret = [] + while True: + d = p.serial_read(0) + if len(d) == 0: + break + ret.append(d) + return ret + + +fan_cmd = 0. + +def logger(event): + # requires a build with DEBUG_FAN + with Panda(claim=False) as p, open('/tmp/fan_log', 'w') as f: + power = None + target_rpm = None + stall_count = None + rpm_fast = None + t = time.monotonic() + + drain_serial(p) + while not event.is_set(): + p.set_fan_power(fan_cmd) + + for l in drain_serial(p)[::-1]: + ns = l.decode('utf8').strip().split(' ') + if len(ns) == 4: + target_rpm, rpm_fast, power, stall_count = (int(n, 16) for n in ns) + break + + dat = { + 't': time.monotonic() - t, + 'cmd_power': fan_cmd, + 'pwm_power': power, + 'target_rpm': target_rpm, + 'rpm_fast': rpm_fast, + 'rpm': p.get_fan_rpm(), + 'stall_counter': stall_count, + 'total_stall_count': p.health()['fan_stall_count'], + } + f.write(json.dumps(dat) + '\n') + time.sleep(1/16.) + p.set_fan_power(0) + +def get_overshoot_rpm(p, power): + global fan_cmd + + # make sure the fan is stopped completely + fan_cmd = 0. + while p.get_fan_rpm() > 100: + time.sleep(0.1) + time.sleep(3) + + # set it to 30% power to mimic going onroad + fan_cmd = power + max_rpm = 0 + max_power = 0 + for _ in range(70): + max_rpm = max(max_rpm, p.get_fan_rpm()) + max_power = max(max_power, p.health()['fan_power']) + time.sleep(0.1) + + # tolerate 10% overshoot + expected_rpm = Panda.MAX_FAN_RPMs[bytes(p.get_type())] * power / 100 + overshoot = (max_rpm / expected_rpm) - 1 + + return overshoot, max_rpm, max_power + + +if __name__ == "__main__": + event = threading.Event() + threading.Thread(target=logger, args=(event, )).start() + + try: + p = Panda() + for power in range(10, 101, 10): + overshoot, max_rpm, max_power = get_overshoot_rpm(p, power) + print(f"Fan power {power}%: overshoot {overshoot:.2%}, Max RPM {max_rpm}, Max power {max_power}%") + finally: + event.set() diff --git a/panda/tests/get_version.py b/panda/tests/get_version.py new file mode 100755 index 0000000000..a0138122ba --- /dev/null +++ b/panda/tests/get_version.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 +from panda import Panda + +if __name__ == "__main__": + for p in Panda.list(): + pp = Panda(p) + print(f"{pp.get_serial()[0]}: {pp.get_version()}") diff --git a/panda/tests/health_test.py b/panda/tests/health_test.py new file mode 100755 index 0000000000..1195c2d7f2 --- /dev/null +++ b/panda/tests/health_test.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 +import time +from panda import Panda + +if __name__ == "__main__": + i = 0 + pi = 0 + + panda = Panda() + while True: + st = time.monotonic() + while time.monotonic() - st < 1: + panda.health() + i += 1 + print(i, panda.health(), "\n") + print(f"Speed: {i - pi}Hz") + pi = i + diff --git a/panda/tests/hitl/1_program.py b/panda/tests/hitl/1_program.py new file mode 100644 index 0000000000..6a5087fa96 --- /dev/null +++ b/panda/tests/hitl/1_program.py @@ -0,0 +1,103 @@ +import os +import time +import pytest + +from panda import Panda, PandaDFU, McuType, BASEDIR + + +def check_signature(p): + assert not p.bootstub, "Flashed firmware not booting. Stuck in bootstub." + assert p.up_to_date() + + +def test_dfu(p): + app_mcu_type = p.get_mcu_type() + dfu_serial = p.get_dfu_serial() + + p.reset(enter_bootstub=True) + p.reset(enter_bootloader=True) + assert Panda.wait_for_dfu(dfu_serial, timeout=19), "failed to enter DFU" + + dfu = PandaDFU(dfu_serial) + assert dfu.get_mcu_type() == app_mcu_type + + assert dfu_serial in PandaDFU.list() + + dfu._handle.clear_status() + dfu.reset() + p.reconnect() + +# TODO: make more comprehensive bootstub tests and run on a few production ones + current +# TODO: also test release-signed app +@pytest.mark.timeout(30) +def test_known_bootstub(p): + """ + Test that compiled app can work with known production bootstub + """ + known_bootstubs = { + # covers the two cases listed in Panda.connect + McuType.F4: [ + # case A - no bcdDevice or panda type, has to assume F4 + "bootstub_f4_first_dos_production.panda.bin", + + # case B - just bcdDevice + "bootstub_f4_only_bcd.panda.bin", + ], + McuType.H7: ["bootstub.panda_h7.bin"], + } + + for kb in known_bootstubs[p.get_mcu_type()]: + app_ids = (p.get_mcu_type(), p.get_usb_serial()) + assert None not in app_ids + + p.reset(enter_bootstub=True) + p.reset(enter_bootloader=True) + + dfu_serial = p.get_dfu_serial() + assert Panda.wait_for_dfu(dfu_serial, timeout=30) + + dfu = PandaDFU(dfu_serial) + with open(os.path.join(BASEDIR, "tests/hitl/known_bootstub", kb), "rb") as f: + code = f.read() + + dfu.program_bootstub(code) + dfu.reset() + + p.connect(claim=False, wait=True) + + # check for MCU or serial mismatch + with Panda(p._serial, claim=False) as np: + bootstub_ids = (np.get_mcu_type(), np.get_usb_serial()) + assert app_ids == bootstub_ids + + # ensure we can flash app and it jumps to app + p.flash() + check_signature(p) + assert not p.bootstub + +@pytest.mark.timeout(25) +def test_recover(p): + assert p.recover(timeout=30) + check_signature(p) + +@pytest.mark.timeout(25) +def test_flash(p): + # test flash from bootstub + serial = p._serial + assert serial is not None + p.reset(enter_bootstub=True) + p.close() + time.sleep(2) + + with Panda(serial) as np: + assert np.bootstub + assert np._serial == serial + np.flash() + + p.reconnect() + p.reset() + check_signature(p) + + # test flash from app + p.flash() + check_signature(p) diff --git a/panda/tests/hitl/2_health.py b/panda/tests/hitl/2_health.py new file mode 100644 index 0000000000..12c75f561d --- /dev/null +++ b/panda/tests/hitl/2_health.py @@ -0,0 +1,65 @@ +import time +import pytest + +from panda import Panda + + +@pytest.mark.skip_panda_types((Panda.HW_TYPE_DOS, )) +def test_voltage(p): + for _ in range(10): + voltage = p.health()['voltage'] + assert ((voltage > 11000) and (voltage < 13000)) + time.sleep(0.1) + +def test_hw_type(p): + """ + hw type should be same in bootstub as application + """ + + hw_type = p.get_type() + mcu_type = p.get_mcu_type() + assert mcu_type is not None + + app_uid = p.get_uid() + usb_serial = p.get_usb_serial() + assert app_uid == usb_serial + + p.reset(enter_bootstub=True, reconnect=True) + p.close() + time.sleep(3) + with Panda(p.get_usb_serial()) as pp: + assert pp.bootstub + assert pp.get_type() == hw_type, "Bootstub and app hw type mismatch" + assert pp.get_mcu_type() == mcu_type, "Bootstub and app MCU type mismatch" + assert pp.get_uid() == app_uid + +def test_heartbeat(p, panda_jungle): + panda_jungle.set_ignition(True) + # TODO: add more cases here once the tests aren't super slow + p.set_safety_mode(mode=Panda.SAFETY_HYUNDAI, param=Panda.FLAG_HYUNDAI_LONG) + p.send_heartbeat() + assert p.health()['safety_mode'] == Panda.SAFETY_HYUNDAI + assert p.health()['safety_param'] == Panda.FLAG_HYUNDAI_LONG + + # shouldn't do anything once we're in a car safety mode + p.set_heartbeat_disabled() + + time.sleep(6.) + + h = p.health() + assert h['heartbeat_lost'] + assert h['safety_mode'] == Panda.SAFETY_SILENT + assert h['safety_param'] == 0 + assert h['controls_allowed'] == 0 + +def test_microsecond_timer(p): + start_time = p.get_microsecond_timer() + time.sleep(1) + end_time = p.get_microsecond_timer() + + # account for uint32 overflow + if end_time < start_time: + end_time += 2**32 + + time_diff = (end_time - start_time) / 1e6 + assert 0.98 < time_diff < 1.02, f"Timer not running at the correct speed! (got {time_diff:.2f}s instead of 1.0s)" diff --git a/panda/tests/hitl/3_usb_to_can.py b/panda/tests/hitl/3_usb_to_can.py new file mode 100644 index 0000000000..c0a9035fe4 --- /dev/null +++ b/panda/tests/hitl/3_usb_to_can.py @@ -0,0 +1,86 @@ +import time +from flaky import flaky + +from panda import Panda +from panda.tests.hitl.helpers import time_many_sends + +def test_can_loopback(p): + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_can_loopback(True) + + for bus in (0, 1, 2): + # set bus 0 speed to 5000 + p.set_can_speed_kbps(bus, 500) + + # send a message on bus 0 + p.can_send(0x1aa, b"message", bus) + + # confirm receive both on loopback and send receipt + time.sleep(0.05) + r = p.can_recv() + sr = [x for x in r if x[3] == 0x80 | bus] + lb = [x for x in r if x[3] == bus] + assert len(sr) == 1 + assert len(lb) == 1 + + # confirm data is correct + assert 0x1aa == sr[0][0] == lb[0][0] + assert b"message" == sr[0][2] == lb[0][2] + +def test_reliability(p): + MSG_COUNT = 100 + + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_can_loopback(True) + p.set_can_speed_kbps(0, 1000) + + addrs = list(range(100, 100 + MSG_COUNT)) + ts = [(j, 0, b"\xaa" * 8, 0) for j in addrs] + + for _ in range(100): + st = time.monotonic() + + p.can_send_many(ts) + + r = [] + while len(r) < 200 and (time.monotonic() - st) < 0.5: + r.extend(p.can_recv()) + + sent_echo = [x for x in r if x[3] == 0x80] + loopback_resp = [x for x in r if x[3] == 0] + + assert sorted([x[0] for x in loopback_resp]) == addrs + assert sorted([x[0] for x in sent_echo]) == addrs + assert len(r) == 200 + + # take sub 20ms + et = (time.monotonic() - st) * 1000.0 + assert et < 20 + +@flaky(max_runs=6, min_passes=1) +def test_throughput(p): + # enable output mode + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # enable CAN loopback mode + p.set_can_loopback(True) + + for speed in [10, 20, 50, 100, 125, 250, 500, 1000]: + # set bus 0 speed to speed + p.set_can_speed_kbps(0, speed) + time.sleep(0.05) + + comp_kbps = time_many_sends(p, 0) + + # bit count from https://en.wikipedia.org/wiki/CAN_bus + saturation_pct = (comp_kbps / speed) * 100.0 + assert saturation_pct > 80 + assert saturation_pct < 100 + + print("loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) + +# this will fail if you have hardware serial connected +def test_serial_debug(p): + _ = p.serial_read(Panda.SERIAL_DEBUG) # junk + p.call_control_api(0x01) + assert p.serial_read(Panda.SERIAL_DEBUG).startswith(b"NO HANDLER") diff --git a/panda/tests/hitl/4_can_loopback.py b/panda/tests/hitl/4_can_loopback.py new file mode 100644 index 0000000000..a7e1aea1ee --- /dev/null +++ b/panda/tests/hitl/4_can_loopback.py @@ -0,0 +1,202 @@ +import os +import time +import pytest +import random +import threading +from flaky import flaky +from collections import defaultdict + +from panda import Panda +from panda.tests.hitl.conftest import PandaGroup +from panda.tests.hitl.helpers import time_many_sends, get_random_can_messages, clear_can_buffers + +@flaky(max_runs=3, min_passes=1) +@pytest.mark.timeout(35) +def test_send_recv(p, panda_jungle): + def test(p_send, p_recv): + for bus in (0, 1, 2): + for speed in (10, 20, 50, 100, 125, 250, 500, 1000): + clear_can_buffers(p_send, speed) + clear_can_buffers(p_recv, speed) + + comp_kbps = time_many_sends(p_send, bus, p_recv, two_pandas=True) + + saturation_pct = (comp_kbps / speed) * 100.0 + assert 80 < saturation_pct < 100 + + print(f"two pandas bus {bus}, 100 messages at speed {speed:4d}, comp speed is {comp_kbps:7.2f}, {saturation_pct:6.2f}%") + + # Run tests in both directions + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + test(p, panda_jungle) + test(panda_jungle, p) + + +@flaky(max_runs=6, min_passes=1) +@pytest.mark.timeout(30) +def test_latency(p, panda_jungle): + def test(p_send, p_recv): + for bus in (0, 1, 2): + for speed in (10, 20, 50, 100, 125, 250, 500, 1000): + clear_can_buffers(p_send, speed) + clear_can_buffers(p_recv, speed) + + latencies = [] + comp_kbps_list = [] + saturation_pcts = [] + + num_messages = 100 + + for _ in range(num_messages): + st = time.monotonic() + p_send.can_send(0x1ab, b"message", bus) + r = [] + while len(r) < 1 and (time.monotonic() - st) < 5: + r = p_recv.can_recv() + et = time.monotonic() + r_echo = [] + while len(r_echo) < 1 and (time.monotonic() - st) < 10: + r_echo = p_send.can_recv() + + if len(r) == 0 or len(r_echo) == 0: + print(f"r: {r}, r_echo: {r_echo}") + + assert len(r) == 1 + assert len(r_echo) == 1 + + et = (et - st) * 1000.0 + comp_kbps = (1 + 11 + 1 + 1 + 1 + 4 + 8 * 8 + 15 + 1 + 1 + 1 + 7) / et + latency = et - ((1 + 11 + 1 + 1 + 1 + 4 + 8 * 8 + 15 + 1 + 1 + 1 + 7) / speed) + + assert latency < 5.0 + + saturation_pct = (comp_kbps / speed) * 100.0 + latencies.append(latency) + comp_kbps_list.append(comp_kbps) + saturation_pcts.append(saturation_pct) + + average_latency = sum(latencies) / num_messages + assert average_latency < 1.0 + average_comp_kbps = sum(comp_kbps_list) / num_messages + average_saturation_pct = sum(saturation_pcts) / num_messages + + print("two pandas bus {}, {} message average at speed {:4d}, latency is {:5.3f}ms, comp speed is {:7.2f}, percent {:6.2f}" + .format(bus, num_messages, speed, average_latency, average_comp_kbps, average_saturation_pct)) + + # Run tests in both directions + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + test(p, panda_jungle) + test(panda_jungle, p) + + +@pytest.mark.panda_expect_can_error +@pytest.mark.test_panda_types(PandaGroup.GEN2) +def test_gen2_loopback(p, panda_jungle): + def test(p_send, p_recv, address=None): + for bus in range(4): + obd = False + if bus == 3: + obd = True + bus = 1 + + # Clear buses + clear_can_buffers(p_send) + clear_can_buffers(p_recv) + + # Send a random string + addr = address if address else random.randint(1, 2000) + string = b"test" + os.urandom(4) + p_send.set_obd(obd) + p_recv.set_obd(obd) + time.sleep(0.2) + p_send.can_send(addr, string, bus) + time.sleep(0.2) + + content = p_recv.can_recv() + + # Check amount of messages + assert len(content) == 1 + + # Check content + assert content[0][0] == addr and content[0][2] == string + + # Check bus + assert content[0][3] == bus + + print("Bus:", bus, "address:", addr, "OBD:", obd, "OK") + + # Run tests in both directions + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + test(p, panda_jungle) + test(panda_jungle, p) + + # Test extended frame address with ELM327 mode + p.set_safety_mode(Panda.SAFETY_ELM327) + test(p, panda_jungle, 0x18DB33F1) + test(panda_jungle, p, 0x18DB33F1) + + # TODO: why it's not being reset by fixtures reinit? + p.set_obd(False) + panda_jungle.set_obd(False) + +def test_bulk_write(p, panda_jungle): + # The TX buffers on pandas is 0x100 in length. + NUM_MESSAGES_PER_BUS = 10000 + + def flood_tx(panda): + print('Sending!') + msg = b"\xaa" * 8 + packet = [] + # start with many messages on a single bus (higher contention for single TX ring buffer) + packet += [[0xaa, None, msg, 0]] * NUM_MESSAGES_PER_BUS + # end with many messages on multiple buses + packet += [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + + # Disable timeout + panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + panda.can_send_many(packet, timeout=0) + print(f"Done sending {4 * NUM_MESSAGES_PER_BUS} messages!", time.monotonic()) + print(panda.health()) + + # Start transmisson + threading.Thread(target=flood_tx, args=(p,)).start() + + # Receive as much as we can in a few second time period + rx = [] + old_len = 0 + start_time = time.monotonic() + while time.monotonic() - start_time < 5 or len(rx) > old_len: + old_len = len(rx) + rx.extend(panda_jungle.can_recv()) + print(f"Received {len(rx)} messages", time.monotonic()) + + # All messages should have been received + if len(rx) != 4 * NUM_MESSAGES_PER_BUS: + raise Exception("Did not receive all messages!") + +def test_message_integrity(p): + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_can_loopback(True) + for i in range(250): + sent_msgs = defaultdict(set) + for _ in range(random.randrange(10)): + to_send = get_random_can_messages(random.randrange(100)) + for m in to_send: + sent_msgs[m[3]].add((m[0], m[2])) + p.can_send_many(to_send, timeout=0) + + start_time = time.monotonic() + while time.monotonic() - start_time < 2 and any(len(sent_msgs[bus]) for bus in range(3)): + recvd = p.can_recv() + for msg in recvd: + if msg[3] >= 128: + k = (msg[0], bytes(msg[2])) + bus = msg[3]-128 + assert k in sent_msgs[bus], f"message {k} was never sent on bus {bus}" + sent_msgs[msg[3]-128].discard(k) + + # if a set isn't empty, messages got dropped + for bus in range(3): + assert not len(sent_msgs[bus]), f"loop {i}: bus {bus} missing {len(sent_msgs[bus])} messages" + + print("Got all messages intact") diff --git a/panda/tests/hitl/5_spi.py b/panda/tests/hitl/5_spi.py new file mode 100644 index 0000000000..f9a4e28d4e --- /dev/null +++ b/panda/tests/hitl/5_spi.py @@ -0,0 +1,103 @@ +import binascii +import pytest +import random +from unittest.mock import patch + +from panda import Panda, PandaDFU +from panda.python.spi import SpiDevice, PandaProtocolMismatch, PandaSpiNackResponse + +pytestmark = [ + pytest.mark.test_panda_types((Panda.HW_TYPE_TRES, )) +] + +@pytest.mark.skip("doesn't work, bootloader seems to ignore commands once it sees junk") +def test_dfu_with_spam(p): + dfu_serial = p.get_dfu_serial() + + # enter DFU + p.reset(enter_bootstub=True) + p.reset(enter_bootloader=True) + assert Panda.wait_for_dfu(dfu_serial, timeout=19), "failed to enter DFU" + + # send junk + d = SpiDevice() + for _ in range(9): + with d.acquire() as spi: + dat = [random.randint(-1, 255) for _ in range(random.randint(1, 100))] + spi.xfer(dat) + + # should still show up + assert dfu_serial in PandaDFU.list() + +class TestSpi: + def _ping(self, mocker, panda): + # should work with no retries + spy = mocker.spy(panda._handle, '_wait_for_ack') + panda.health() + assert spy.call_count == 2 + mocker.stop(spy) + + def test_protocol_version_check(self, p): + for bootstub in (False, True): + p.reset(enter_bootstub=bootstub) + with patch('panda.python.spi.PandaSpiHandle.PROTOCOL_VERSION', return_value="abc"): + # list should still work with wrong version + assert p._serial in Panda.list() + + # connect but raise protocol error + with pytest.raises(PandaProtocolMismatch): + Panda(p._serial) + + def test_protocol_version_data(self, p): + for bootstub in (False, True): + p.reset(enter_bootstub=bootstub) + v = p._handle.get_protocol_version() + + uid = binascii.hexlify(v[:12]).decode() + assert uid == p.get_uid() + + hwtype = v[12] + assert hwtype == ord(p.get_type()) + + bstub = v[13] + assert bstub == (0xEE if bootstub else 0xCC) + + def test_all_comm_types(self, mocker, p): + spy = mocker.spy(p._handle, '_wait_for_ack') + + # controlRead + controlWrite + p.health() + p.can_clear(0) + assert spy.call_count == 2*2 + + # bulkRead + bulkWrite + p.can_recv() + p.can_send(0x123, b"somedata", 0) + assert spy.call_count == 2*4 + + def test_bad_header(self, mocker, p): + with patch('panda.python.spi.SYNC', return_value=0): + with pytest.raises(PandaSpiNackResponse): + p._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, p.HEALTH_STRUCT.size, timeout=50) + self._ping(mocker, p) + + def test_bad_checksum(self, mocker, p): + cnt = p.health()['spi_checksum_error_count'] + with patch('panda.python.spi.PandaSpiHandle._calc_checksum', return_value=0): + with pytest.raises(PandaSpiNackResponse): + p._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, p.HEALTH_STRUCT.size, timeout=50) + self._ping(mocker, p) + assert (p.health()['spi_checksum_error_count'] - cnt) > 0 + + def test_non_existent_endpoint(self, mocker, p): + for _ in range(10): + ep = random.randint(4, 20) + with pytest.raises(PandaSpiNackResponse): + p._handle.bulkRead(ep, random.randint(1, 1000), timeout=50) + + self._ping(mocker, p) + + with pytest.raises(PandaSpiNackResponse): + p._handle.bulkWrite(ep, b"abc", timeout=50) + + self._ping(mocker, p) diff --git a/panda/tests/hitl/6_safety.py b/panda/tests/hitl/6_safety.py new file mode 100644 index 0000000000..2237f53036 --- /dev/null +++ b/panda/tests/hitl/6_safety.py @@ -0,0 +1,29 @@ +import time + +from panda import Panda + + +def test_safety_nooutput(p): + p.set_safety_mode(Panda.SAFETY_SILENT) + p.set_can_loopback(True) + + # send a message on bus 0 + p.can_send(0x1aa, b"message", 0) + + # confirm receive nothing + time.sleep(0.05) + r = p.can_recv() + # bus 192 is messages blocked by TX safety hook on bus 0 + assert len([x for x in r if x[3] != 192]) == 0 + assert len([x for x in r if x[3] == 192]) == 1 + + +def test_canfd_safety_modes(p): + # works on all pandas + p.set_safety_mode(Panda.SAFETY_TOYOTA) + assert p.health()['safety_mode'] == Panda.SAFETY_TOYOTA + + # shouldn't be able to set a CAN-FD safety mode on non CAN-FD panda + p.set_safety_mode(Panda.SAFETY_HYUNDAI_CANFD) + expected_mode = Panda.SAFETY_HYUNDAI_CANFD if p.get_type() in Panda.H7_DEVICES else Panda.SAFETY_SILENT + assert p.health()['safety_mode'] == expected_mode diff --git a/panda/tests/hitl/7_internal.py b/panda/tests/hitl/7_internal.py new file mode 100644 index 0000000000..eb8577fb55 --- /dev/null +++ b/panda/tests/hitl/7_internal.py @@ -0,0 +1,68 @@ +import time +import pytest + +from panda import Panda + +pytestmark = [ + pytest.mark.skip_panda_types(Panda.HW_TYPE_UNO), + pytest.mark.test_panda_types(Panda.INTERNAL_DEVICES) +] + +@pytest.mark.timeout(2*60) +def test_fan_controller(p): + start_health = p.health() + + for power in (30, 50, 80, 100): + p.set_fan_power(0) + while p.get_fan_rpm() > 0: + time.sleep(0.1) + + # wait until fan spins up (and recovers if needed), + # then wait a bit more for the RPM to converge + p.set_fan_power(power) + for _ in range(20): + time.sleep(1) + if p.get_fan_rpm() > 1000: + break + time.sleep(5) + + expected_rpm = Panda.MAX_FAN_RPMs[bytes(p.get_type())] * power / 100 + assert 0.9 * expected_rpm <= p.get_fan_rpm() <= 1.1 * expected_rpm + + # Ensure the stall detection is tested on dos + if p.get_type() == Panda.HW_TYPE_DOS: + stalls = p.health()['fan_stall_count'] - start_health['fan_stall_count'] + assert stalls >= 2 + print("stall count", stalls) + else: + assert p.health()['fan_stall_count'] == 0 + +def test_fan_cooldown(p): + # if the fan cooldown doesn't work, we get high frequency noise on the tach line + # while the rotor spins down. this makes sure it never goes beyond the expected max RPM + p.set_fan_power(100) + time.sleep(3) + p.set_fan_power(0) + for _ in range(5): + assert p.get_fan_rpm() <= 7000 + time.sleep(0.5) + +def test_fan_overshoot(p): + if p.get_type() == Panda.HW_TYPE_DOS: + pytest.skip("panda's fan controller overshoots on the comma three fans that need stall recovery") + + # make sure it's stopped completely + p.set_fan_power(0) + while p.get_fan_rpm() > 0: + time.sleep(0.1) + + # set it to 30% power to mimic going onroad + p.set_fan_power(30) + max_rpm = 0 + for _ in range(50): + max_rpm = max(max_rpm, p.get_fan_rpm()) + time.sleep(0.1) + + # tolerate 10% overshoot + expected_rpm = Panda.MAX_FAN_RPMs[bytes(p.get_type())] * 30 / 100 + assert max_rpm <= 1.1 * expected_rpm, f"Fan overshoot: {(max_rpm / expected_rpm * 100) - 100:.1f}%" diff --git a/panda/tests/hitl/8_misc.py b/panda/tests/hitl/8_misc.py new file mode 100644 index 0000000000..3c47ed492b --- /dev/null +++ b/panda/tests/hitl/8_misc.py @@ -0,0 +1,13 @@ +import time + +from panda import Panda + +def test_boot_time(p): + # boot time should be instant + st = time.monotonic() + p.reset(reconnect=False) + assert Panda.wait_for_panda(p.get_usb_serial(), timeout=3.0) + + # USB enumeration is slow, so SPI is faster + assert time.monotonic() - st < (1.0 if p.spi else 5.0) + diff --git a/panda/tests/hitl/9_harness.py b/panda/tests/hitl/9_harness.py new file mode 100644 index 0000000000..5689892b8a --- /dev/null +++ b/panda/tests/hitl/9_harness.py @@ -0,0 +1,77 @@ +import time +import pytest +import itertools + +from panda import Panda +from panda.tests.hitl.conftest import PandaGroup + +# TODO: test relay + +@pytest.mark.panda_expect_can_error +@pytest.mark.test_panda_types(PandaGroup.GEN2) +def test_harness_status(p, panda_jungle): + # map from jungle orientations to panda orientations + orientation_map = { + Panda.HARNESS_STATUS_NC: Panda.HARNESS_STATUS_NC, + } + + # this shouldn't be parameterized since we don't want the panda to be reset + # between the tests. + for ignition, orientation in itertools.product([True, False], [Panda.HARNESS_STATUS_NC, Panda.HARNESS_STATUS_NORMAL, Panda.HARNESS_STATUS_FLIPPED]): + print() + p.set_safety_mode(Panda.SAFETY_ELM327) + panda_jungle.set_harness_orientation(orientation) + panda_jungle.set_ignition(ignition) + + # wait for orientation detection + time.sleep(0.25) + + health = p.health() + detected_orientation = health['car_harness_status'] + print(f"orientation set: {orientation} detected: {detected_orientation}") + + if detected_orientation not in orientation_map: + assert detected_orientation != Panda.HARNESS_STATUS_NC + other = {Panda.HARNESS_STATUS_NORMAL: Panda.HARNESS_STATUS_FLIPPED, Panda.HARNESS_STATUS_FLIPPED: Panda.HARNESS_STATUS_NORMAL} + orientation_map.update({ + orientation: detected_orientation, + other[orientation]: other[detected_orientation], + }) + + # Orientation + assert orientation_map[detected_orientation] == orientation + + # Line ignition + assert health['ignition_line'] == (False if orientation == Panda.HARNESS_STATUS_NC else ignition) + + # CAN traffic + if orientation != Panda.HARNESS_STATUS_NC: + for bus in range(3): + panda_jungle.can_send(0x123, f"{bus}".encode(), bus) + time.sleep(0.5) + + msgs = p.can_recv() + buses = {int(dat): bus for _, _, dat, bus in msgs if bus <= 3} + print(msgs) + + # jungle doesn't actually switch buses when switching orientation + flipped = orientation == Panda.HARNESS_STATUS_FLIPPED + assert buses[0] == (2 if flipped else 0) + assert buses[2] == (0 if flipped else 2) + + # SBU voltages + supply_voltage_mV = 1800 if p.get_type() in [Panda.HW_TYPE_TRES, ] else 3300 + + if orientation == Panda.HARNESS_STATUS_NC: + assert health['sbu1_voltage_mV'] > 0.9 * supply_voltage_mV + assert health['sbu2_voltage_mV'] > 0.9 * supply_voltage_mV + else: + relay_line = 'sbu1_voltage_mV' if (detected_orientation == Panda.HARNESS_STATUS_FLIPPED) else 'sbu2_voltage_mV' + ignition_line = 'sbu2_voltage_mV' if (detected_orientation == Panda.HARNESS_STATUS_FLIPPED) else 'sbu1_voltage_mV' + + assert health[relay_line] < 0.1 * supply_voltage_mV + assert health[ignition_line] > health[relay_line] + if ignition: + assert health[ignition_line] < 0.3 * supply_voltage_mV + else: + assert health[ignition_line] > 0.9 * supply_voltage_mV diff --git a/selfdrive/manager/test/__init__.py b/panda/tests/hitl/__init__.py similarity index 100% rename from selfdrive/manager/test/__init__.py rename to panda/tests/hitl/__init__.py diff --git a/panda/tests/hitl/conftest.py b/panda/tests/hitl/conftest.py new file mode 100644 index 0000000000..1c4db1e921 --- /dev/null +++ b/panda/tests/hitl/conftest.py @@ -0,0 +1,222 @@ +import os +import pytest +import concurrent.futures + +from panda import Panda, PandaDFU, PandaJungle +from panda.tests.hitl.helpers import clear_can_buffers + +# needed to get output when using xdist +if "DEBUG" in os.environ: + import sys + sys.stdout = sys.stderr + +SPEED_NORMAL = 500 +BUS_SPEEDS = [(0, SPEED_NORMAL), (1, SPEED_NORMAL), (2, SPEED_NORMAL)] + + +JUNGLE_SERIAL = os.getenv("PANDAS_JUNGLE") +NO_JUNGLE = os.environ.get("NO_JUNGLE", "0") == "1" +PANDAS_EXCLUDE = os.getenv("PANDAS_EXCLUDE", "").strip().split(" ") +HW_TYPES = os.environ.get("HW_TYPES", None) + +PARALLEL = "PARALLEL" in os.environ +NON_PARALLEL = "NON_PARALLEL" in os.environ +if PARALLEL: + NO_JUNGLE = True + +class PandaGroup: + H7 = (Panda.HW_TYPE_RED_PANDA, Panda.HW_TYPE_RED_PANDA_V2, Panda.HW_TYPE_TRES) + GEN2 = (Panda.HW_TYPE_BLACK_PANDA, Panda.HW_TYPE_UNO, Panda.HW_TYPE_DOS) + H7 + TESTED = (Panda.HW_TYPE_WHITE_PANDA, Panda.HW_TYPE_BLACK_PANDA, Panda.HW_TYPE_RED_PANDA, Panda.HW_TYPE_RED_PANDA_V2, Panda.HW_TYPE_UNO) + +if HW_TYPES is not None: + PandaGroup.TESTED = [bytes([int(x), ]) for x in HW_TYPES.strip().split(",")] # type: ignore + + +# Find all pandas connected +_all_pandas = {} +_panda_jungle = None +def init_all_pandas(): + if not NO_JUNGLE: + global _panda_jungle + _panda_jungle = PandaJungle(JUNGLE_SERIAL) + _panda_jungle.set_panda_power(True) + + for serial in Panda.list(): + if serial not in PANDAS_EXCLUDE: + with Panda(serial=serial, claim=False) as p: + ptype = bytes(p.get_type()) + if ptype in PandaGroup.TESTED: + _all_pandas[serial] = ptype + + # ensure we have all tested panda types + missing_types = set(PandaGroup.TESTED) - set(_all_pandas.values()) + assert len(missing_types) == 0, f"Missing panda types: {missing_types}" + + print(f"{len(_all_pandas)} total pandas") +init_all_pandas() +_all_panda_serials = sorted(_all_pandas.keys()) + + +def init_jungle(): + if _panda_jungle is None: + return + clear_can_buffers(_panda_jungle) + _panda_jungle.set_panda_power(True) + _panda_jungle.set_can_loopback(False) + _panda_jungle.set_obd(False) + _panda_jungle.set_harness_orientation(PandaJungle.HARNESS_ORIENTATION_1) + for bus, speed in BUS_SPEEDS: + _panda_jungle.set_can_speed_kbps(bus, speed) + + # ensure FW hasn't changed + assert _panda_jungle.up_to_date() + + +def pytest_configure(config): + config.addinivalue_line( + "markers", "test_panda_types(name): whitelist a test for specific panda types" + ) + config.addinivalue_line( + "markers", "skip_panda_types(name): blacklist panda types from a test" + ) + config.addinivalue_line( + "markers", "panda_expect_can_error: mark test to ignore CAN health errors" + ) + +@pytest.hookimpl(tryfirst=True) +def pytest_collection_modifyitems(items): + for item in items: + if item.get_closest_marker('timeout') is None: + item.add_marker(pytest.mark.timeout(60)) + + # xdist grouping by panda + serial = item.name.split("serial=")[1].split(",")[0] + assert len(serial) == 24 + item.add_marker(pytest.mark.xdist_group(serial)) + + needs_jungle = "panda_jungle" in item.fixturenames + if PARALLEL and needs_jungle: + item.add_marker(pytest.mark.skip(reason="no jungle tests in PARALLEL mode")) + elif NON_PARALLEL and not needs_jungle: + item.add_marker(pytest.mark.skip(reason="only running jungle tests")) + +def pytest_make_parametrize_id(config, val, argname): + if val in _all_pandas: + # TODO: get nice string instead of int + hw_type = _all_pandas[val][0] + return f"serial={val}, hw_type={hw_type}" + return None + + +@pytest.fixture(name='panda_jungle', scope='function') +def fixture_panda_jungle(request): + init_jungle() + return _panda_jungle + +@pytest.fixture(name='p', scope='function') +def func_fixture_panda(request, module_panda): + p = module_panda + + # Check if test is applicable to this panda + mark = request.node.get_closest_marker('test_panda_types') + if mark: + assert len(mark.args) > 0, "Missing panda types argument in mark" + test_types = mark.args[0] + if _all_pandas[p.get_usb_serial()] not in test_types: + pytest.skip(f"Not applicable, {test_types} pandas only") + + mark = request.node.get_closest_marker('skip_panda_types') + if mark: + assert len(mark.args) > 0, "Missing panda types argument in mark" + skip_types = mark.args[0] + if _all_pandas[p.get_usb_serial()] in skip_types: + pytest.skip(f"Not applicable to {skip_types}") + + # this is 2+ seconds on USB pandas due to slow + # enumeration on the host side + p.reset() + + # ensure FW hasn't changed + assert p.up_to_date() + + # Run test + yield p + + # Teardown + + # reconnect + if p.get_dfu_serial() in PandaDFU.list(): + PandaDFU(p.get_dfu_serial()).reset() + p.reconnect() + if not p.connected: + p.reconnect() + if p.bootstub: + p.reset() + + assert not p.bootstub + + # TODO: would be nice to make these common checks in the teardown + # show up as failed tests instead of "errors" + + # Check for faults + assert p.health()['faults'] == 0 + assert p.health()['fault_status'] == 0 + + # Check for SPI errors + #assert p.health()['spi_checksum_error_count'] == 0 + + # Check health of each CAN core after test, normal to fail for test_gen2_loopback on OBD bus, so skipping + mark = request.node.get_closest_marker('panda_expect_can_error') + expect_can_error = mark is not None + if not expect_can_error: + for i in range(3): + can_health = p.can_health(i) + assert can_health['bus_off_cnt'] == 0 + assert can_health['receive_error_cnt'] < 127 + assert can_health['transmit_error_cnt'] < 255 + assert can_health['error_passive'] == 0 + assert can_health['error_warning'] == 0 + assert can_health['total_rx_lost_cnt'] == 0 + assert can_health['total_tx_lost_cnt'] == 0 + assert can_health['total_error_cnt'] == 0 + assert can_health['total_tx_checksum_error_cnt'] == 0 + +@pytest.fixture(name='module_panda', params=_all_panda_serials, scope='module') +def fixture_panda_setup(request): + """ + Clean up all pandas + jungle and return the panda under test. + """ + panda_serial = request.param + + # Initialize jungle + init_jungle() + + # Connect to pandas + def cnnct(s): + if s == panda_serial: + p = Panda(serial=s) + p.reset(reconnect=True) + + p.set_can_loopback(False) + p.set_power_save(False) + for bus, speed in BUS_SPEEDS: + p.set_can_speed_kbps(bus, speed) + clear_can_buffers(p) + p.set_power_save(False) + return p + elif not PARALLEL: + with Panda(serial=s) as p: + p.reset(reconnect=False) + return None + + with concurrent.futures.ThreadPoolExecutor() as exc: + ps = list(exc.map(cnnct, _all_panda_serials, timeout=20)) + pandas = [p for p in ps if p is not None] + + # run test + yield pandas[0] + + # Teardown + for p in pandas: + p.close() diff --git a/panda/tests/hitl/helpers.py b/panda/tests/hitl/helpers.py new file mode 100644 index 0000000000..4eee437311 --- /dev/null +++ b/panda/tests/hitl/helpers.py @@ -0,0 +1,71 @@ +import time +import random + + +def get_random_can_messages(n): + m = [] + for _ in range(n): + bus = random.randrange(3) + addr = random.randrange(1 << 29) + dat = bytes([random.getrandbits(8) for _ in range(random.randrange(1, 9))]) + m.append([addr, None, dat, bus]) + return m + + +def time_many_sends(p, bus, p_recv=None, msg_count=100, two_pandas=False, msg_len=8): + if p_recv is None: + p_recv = p + if p == p_recv and two_pandas: + raise ValueError("Cannot have two pandas that are the same panda") + + msg_id = random.randint(0x100, 0x200) + to_send = [(msg_id, 0, b"\xaa" * msg_len, bus)] * msg_count + + start_time = time.monotonic() + p.can_send_many(to_send) + r = [] + r_echo = [] + r_len_expected = msg_count if two_pandas else msg_count * 2 + r_echo_len_exected = msg_count if two_pandas else 0 + + while len(r) < r_len_expected and (time.monotonic() - start_time) < 5: + r.extend(p_recv.can_recv()) + end_time = time.monotonic() + if two_pandas: + while len(r_echo) < r_echo_len_exected and (time.monotonic() - start_time) < 10: + r_echo.extend(p.can_recv()) + + sent_echo = [x for x in r if x[3] == 0x80 | bus and x[0] == msg_id] + sent_echo.extend([x for x in r_echo if x[3] == 0x80 | bus and x[0] == msg_id]) + resp = [x for x in r if x[3] == bus and x[0] == msg_id] + + leftovers = [x for x in r if (x[3] != 0x80 | bus and x[3] != bus) or x[0] != msg_id] + assert len(leftovers) == 0 + + assert len(resp) == msg_count + assert len(sent_echo) == msg_count + + end_time = (end_time - start_time) * 1000.0 + comp_kbps = (1 + 11 + 1 + 1 + 1 + 4 + (msg_len * 8) + 15 + 1 + 1 + 1 + 7) * msg_count / end_time + + return comp_kbps + + +def clear_can_buffers(panda, speed: int | None = None): + if speed is not None: + for bus in range(3): + panda.set_can_speed_kbps(bus, speed) + + # clear tx buffers + for i in range(4): + panda.can_clear(i) + + # clear rx buffers + panda.can_clear(0xFFFF) + r = [1] + st = time.monotonic() + while len(r) > 0: + r = panda.can_recv() + time.sleep(0.05) + if (time.monotonic() - st) > 10: + raise Exception("Unable to clear can buffers for panda ", panda.get_serial()) diff --git a/panda/tests/hitl/known_bootstub/bootstub.panda_h7.bin b/panda/tests/hitl/known_bootstub/bootstub.panda_h7.bin new file mode 100755 index 0000000000..a6d856bd2c Binary files /dev/null and b/panda/tests/hitl/known_bootstub/bootstub.panda_h7.bin differ diff --git a/panda/tests/hitl/known_bootstub/bootstub_f4_first_dos_production.panda.bin b/panda/tests/hitl/known_bootstub/bootstub_f4_first_dos_production.panda.bin new file mode 100644 index 0000000000..786acf639e Binary files /dev/null and b/panda/tests/hitl/known_bootstub/bootstub_f4_first_dos_production.panda.bin differ diff --git a/panda/tests/hitl/known_bootstub/bootstub_f4_only_bcd.panda.bin b/panda/tests/hitl/known_bootstub/bootstub_f4_only_bcd.panda.bin new file mode 100755 index 0000000000..000fd26fe4 Binary files /dev/null and b/panda/tests/hitl/known_bootstub/bootstub_f4_only_bcd.panda.bin differ diff --git a/panda/tests/hitl/reset_jungles.py b/panda/tests/hitl/reset_jungles.py new file mode 100755 index 0000000000..fb946731c9 --- /dev/null +++ b/panda/tests/hitl/reset_jungles.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +import concurrent.futures + +from panda import PandaJungle, PandaJungleDFU, McuType +from panda.tests.libs.resetter import Resetter + +SERIALS = {'180019001451313236343430', '1d0017000c50435635333720'} + +def recover(s): + with PandaJungleDFU(s) as pd: + pd.recover() + +def flash(s): + with PandaJungle(s) as p: + p.flash() + return p.get_mcu_type() + +# Reset + flash all CI hardware to get it into a consistent state +# * port 1: jungles-under-test +# * port 2: USB hubs +# * port 3: HITL pandas and their jungles +if __name__ == "__main__": + with Resetter() as r: + # everything off + for i in range(1, 4): + r.enable_power(i, 0) + r.cycle_power(ports=[1, 2], dfu=True) + + dfu_serials = PandaJungleDFU.list() + print(len(dfu_serials), len(SERIALS)) + assert len(dfu_serials) == len(SERIALS) + + with concurrent.futures.ProcessPoolExecutor(max_workers=len(dfu_serials)) as exc: + list(exc.map(recover, dfu_serials, timeout=30)) + + # power cycle for H7 bootloader bug + r.cycle_power(ports=[1, 2]) + + serials = PandaJungle.list() + assert set(PandaJungle.list()) >= SERIALS + mcu_types = list(exc.map(flash, SERIALS, timeout=20)) + assert set(mcu_types) == {McuType.F4, McuType.H7} diff --git a/panda/tests/hitl/run_parallel_tests.sh b/panda/tests/hitl/run_parallel_tests.sh new file mode 100755 index 0000000000..b6b79d99f6 --- /dev/null +++ b/panda/tests/hitl/run_parallel_tests.sh @@ -0,0 +1,8 @@ +#!/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +# n = number of pandas tested +PARALLEL=1 pytest --durations=0 *.py -n 5 --dist loadgroup -x diff --git a/panda/tests/hitl/run_serial_tests.sh b/panda/tests/hitl/run_serial_tests.sh new file mode 100755 index 0000000000..31270f044c --- /dev/null +++ b/panda/tests/hitl/run_serial_tests.sh @@ -0,0 +1,7 @@ +#!/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +NON_PARALLEL=1 pytest --durations=0 *.py -x diff --git a/panda/tests/ir_test.py b/panda/tests/ir_test.py new file mode 100755 index 0000000000..e41decf255 --- /dev/null +++ b/panda/tests/ir_test.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +import time + +from panda import Panda + +power = 0 +if __name__ == "__main__": + p = Panda() + while True: + p.set_ir_power(power) + print("Power: ", str(power)) + time.sleep(1) + power += 10 + power %= 100 diff --git a/panda/tests/libpanda/SConscript b/panda/tests/libpanda/SConscript new file mode 100644 index 0000000000..cc08907308 --- /dev/null +++ b/panda/tests/libpanda/SConscript @@ -0,0 +1,42 @@ +import platform + +CC = 'gcc' +system = platform.system() +if system == 'Darwin': + # gcc installed by homebrew has version suffix (e.g. gcc-12) in order to be + # distinguishable from system one - which acts as a symlink to clang + CC += '-13' + +env = Environment( + CC=CC, + CFLAGS=[ + '-nostdlib', + '-fno-builtin', + '-std=gnu11', + '-Wfatal-errors', + '-Wno-pointer-to-int-cast', + ], + CPPPATH=[".", "../../board/"], +) +if system == "Darwin": + env.PrependENVPath('PATH', '/opt/homebrew/bin') + +if GetOption('ubsan'): + flags = [ + "-fsanitize=undefined", + "-fno-sanitize-recover=undefined", + ] + env['CFLAGS'] += flags + env['LINKFLAGS'] += flags + +panda = env.SharedObject("panda.os", "panda.c") +libpanda = env.SharedLibrary("libpanda.so", [panda]) + +if GetOption('coverage'): + env.Append( + CFLAGS=["-fprofile-arcs", "-ftest-coverage", "-fprofile-abs-path",], + LIBS=["gcov"], + ) + # GCC note file is generated by compiler, ensure we build it, and allow scons to clean it up + AlwaysBuild(panda) + env.SideEffect("panda.gcno", panda) diff --git a/panda/tests/libpanda/libpanda_py.py b/panda/tests/libpanda/libpanda_py.py new file mode 100644 index 0000000000..6f888ac60e --- /dev/null +++ b/panda/tests/libpanda/libpanda_py.py @@ -0,0 +1,96 @@ +import os +from cffi import FFI +from typing import Any, Protocol + +from panda import LEN_TO_DLC +from panda.tests.libpanda.safety_helpers import PandaSafety, setup_safety_helpers + +libpanda_dir = os.path.dirname(os.path.abspath(__file__)) +libpanda_fn = os.path.join(libpanda_dir, "libpanda.so") + +ffi = FFI() + +ffi.cdef(""" +typedef struct { + unsigned char reserved : 1; + unsigned char bus : 3; + unsigned char data_len_code : 4; + unsigned char rejected : 1; + unsigned char returned : 1; + unsigned char extended : 1; + unsigned int addr : 29; + unsigned char checksum; + unsigned char data[64]; +} CANPacket_t; +""", packed=True) + +ffi.cdef(""" +bool safety_rx_hook(CANPacket_t *to_send); +bool safety_tx_hook(CANPacket_t *to_push); +int safety_fwd_hook(int bus_num, int addr); +int set_safety_hooks(uint16_t mode, uint16_t param); +""") + +ffi.cdef(""" +typedef struct { + volatile uint32_t w_ptr; + volatile uint32_t r_ptr; + uint32_t fifo_size; + CANPacket_t *elems; +} can_ring; + +extern can_ring *rx_q; +extern can_ring *tx1_q; +extern can_ring *tx2_q; +extern can_ring *tx3_q; + +bool can_pop(can_ring *q, CANPacket_t *elem); +bool can_push(can_ring *q, CANPacket_t *elem); +void can_set_checksum(CANPacket_t *packet); +int comms_can_read(uint8_t *data, uint32_t max_len); +void comms_can_write(uint8_t *data, uint32_t len); +void comms_can_reset(void); +uint32_t can_slots_empty(can_ring *q); +""") + +setup_safety_helpers(ffi) + +class CANPacket: + reserved: int + bus: int + data_len_code: int + rejected: int + returned: int + extended: int + addr: int + data: list[int] + +class Panda(PandaSafety, Protocol): + # CAN + tx1_q: Any + tx2_q: Any + tx3_q: Any + def can_set_checksum(self, p: CANPacket) -> None: ... + + # safety + def safety_rx_hook(self, to_send: CANPacket) -> int: ... + def safety_tx_hook(self, to_push: CANPacket) -> int: ... + def safety_fwd_hook(self, bus_num: int, addr: int) -> int: ... + def set_safety_hooks(self, mode: int, param: int) -> int: ... + + +libpanda: Panda = ffi.dlopen(libpanda_fn) + + +# helpers + +def make_CANPacket(addr: int, bus: int, dat): + ret = ffi.new('CANPacket_t *') + ret[0].extended = 1 if addr >= 0x800 else 0 + ret[0].addr = addr + ret[0].data_len_code = LEN_TO_DLC[len(dat)] + ret[0].bus = bus + ret[0].data = bytes(dat) + libpanda.can_set_checksum(ret) + + return ret diff --git a/panda/tests/libpanda/panda.c b/panda/tests/libpanda/panda.c new file mode 100644 index 0000000000..f34c028048 --- /dev/null +++ b/panda/tests/libpanda/panda.c @@ -0,0 +1,31 @@ +#include "fake_stm.h" +#include "config.h" +#include "can_definitions.h" + +bool can_init(uint8_t can_number) { return true; } +void process_can(uint8_t can_number) { } +//int safety_tx_hook(CANPacket_t *to_send) { return 1; } + +typedef struct harness_configuration harness_configuration; +void refresh_can_tx_slots_available(void); +void can_tx_comms_resume_usb(void) { }; +void can_tx_comms_resume_spi(void) { }; + +#include "health.h" +#include "faults.h" +#include "libc.h" +#include "boards/board_declarations.h" +#include "safety.h" +#include "main_declarations.h" +#include "drivers/can_common.h" + +can_ring *rx_q = &can_rx_q; +can_ring *tx1_q = &can_tx1_q; +can_ring *tx2_q = &can_tx2_q; +can_ring *tx3_q = &can_tx3_q; + +#include "comms_definitions.h" +#include "can_comms.h" + +// libpanda stuff +#include "safety_helpers.h" diff --git a/panda/tests/libpanda/safety_helpers.h b/panda/tests/libpanda/safety_helpers.h new file mode 100644 index 0000000000..36887c8963 --- /dev/null +++ b/panda/tests/libpanda/safety_helpers.h @@ -0,0 +1,191 @@ +void safety_tick_current_safety_config() { + safety_tick(¤t_safety_config); +} + +bool safety_config_valid() { + if (current_safety_config.rx_checks_len <= 0) { + printf("missing RX checks\n"); + return false; + } + + for (int i = 0; i < current_safety_config.rx_checks_len; i++) { + const RxCheck addr = current_safety_config.rx_checks[i]; + bool valid = addr.status.msg_seen && !addr.status.lagging && addr.status.valid_checksum && (addr.status.wrong_counters < MAX_WRONG_COUNTERS) && addr.status.valid_quality_flag; + if (!valid) { + // printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.status.msg_seen, addr.status.lagging, addr.status.valid_checksum, addr.status.wrong_counters, addr.status.valid_quality_flag); + return false; + } + } + return true; +} + +void set_controls_allowed(bool c){ + controls_allowed = c; +} + +void set_alternative_experience(int mode){ + alternative_experience = mode; +} + +void set_relay_malfunction(bool c){ + relay_malfunction = c; +} + +bool get_controls_allowed(void){ + return controls_allowed; +} + +int get_alternative_experience(void){ + return alternative_experience; +} + +bool get_relay_malfunction(void){ + return relay_malfunction; +} + +bool get_gas_pressed_prev(void){ + return gas_pressed_prev; +} + +bool get_brake_pressed_prev(void){ + return brake_pressed_prev; +} + +bool get_regen_braking_prev(void){ + return regen_braking_prev; +} + +bool get_cruise_engaged_prev(void){ + return cruise_engaged_prev; +} + +void set_cruise_engaged_prev(bool engaged){ + cruise_engaged_prev = engaged; +} + +bool get_vehicle_moving(void){ + return vehicle_moving; +} + +bool get_acc_main_on(void){ + return acc_main_on; +} + +int get_vehicle_speed_min(void){ + return vehicle_speed.min; +} + +int get_vehicle_speed_max(void){ + return vehicle_speed.max; +} + +int get_vehicle_speed_last(void){ + return vehicle_speed.values[0]; +} + +int get_current_safety_mode(void){ + return current_safety_mode; +} + +int get_current_safety_param(void){ + return current_safety_param; +} + +int get_hw_type(void){ + return hw_type; +} + +void set_timer(uint32_t t){ + timer.CNT = t; +} + +void set_torque_meas(int min, int max){ + torque_meas.min = min; + torque_meas.max = max; +} + +int get_torque_meas_min(void){ + return torque_meas.min; +} + +int get_torque_meas_max(void){ + return torque_meas.max; +} + +void set_torque_driver(int min, int max){ + torque_driver.min = min; + torque_driver.max = max; +} + +int get_torque_driver_min(void){ + return torque_driver.min; +} + +int get_torque_driver_max(void){ + return torque_driver.max; +} + +void set_rt_torque_last(int t){ + rt_torque_last = t; +} + +void set_desired_torque_last(int t){ + desired_torque_last = t; +} + +void set_desired_angle_last(int t){ + desired_angle_last = t; +} + +int get_desired_angle_last(void){ + return desired_angle_last; +} + +void set_angle_meas(int min, int max){ + angle_meas.min = min; + angle_meas.max = max; +} + +int get_angle_meas_min(void){ + return angle_meas.min; +} + +int get_angle_meas_max(void){ + return angle_meas.max; +} + + +// ***** car specific helpers ***** + +void set_honda_alt_brake_msg(bool c){ + honda_alt_brake_msg = c; +} + +void set_honda_bosch_long(bool c){ + honda_bosch_long = c; +} + +int get_honda_hw(void) { + return honda_hw; +} + +void set_honda_fwd_brake(bool c){ + honda_fwd_brake = c; +} + +bool get_honda_fwd_brake(void){ + return honda_fwd_brake; +} + +void init_tests(void){ + // get HW_TYPE from env variable set in test.sh + if (getenv("HW_TYPE")) { + hw_type = atoi(getenv("HW_TYPE")); + } + safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic + alternative_experience = 0; + set_timer(0); + ts_steer_req_mismatch_last = 0; + valid_steer_req_count = 0; + invalid_steer_req_count = 0; +} diff --git a/panda/tests/libpanda/safety_helpers.py b/panda/tests/libpanda/safety_helpers.py new file mode 100644 index 0000000000..ea41264ae0 --- /dev/null +++ b/panda/tests/libpanda/safety_helpers.py @@ -0,0 +1,104 @@ +# panda safety helpers, from safety_helpers.c +from typing import Protocol + +def setup_safety_helpers(ffi): + ffi.cdef(""" + void set_controls_allowed(bool c); + bool get_controls_allowed(void); + bool get_longitudinal_allowed(void); + void set_alternative_experience(int mode); + int get_alternative_experience(void); + void set_relay_malfunction(bool c); + bool get_relay_malfunction(void); + bool get_gas_pressed_prev(void); + bool get_brake_pressed_prev(void); + bool get_regen_braking_prev(void); + bool get_acc_main_on(void); + int get_vehicle_speed_min(void); + int get_vehicle_speed_max(void); + int get_vehicle_speed_last(void); + int get_current_safety_mode(void); + int get_current_safety_param(void); + + void set_torque_meas(int min, int max); + int get_torque_meas_min(void); + int get_torque_meas_max(void); + void set_torque_driver(int min, int max); + int get_torque_driver_min(void); + int get_torque_driver_max(void); + void set_desired_torque_last(int t); + void set_rt_torque_last(int t); + void set_desired_angle_last(int t); + int get_desired_angle_last(); + void set_angle_meas(int min, int max); + int get_angle_meas_min(void); + int get_angle_meas_max(void); + + bool get_cruise_engaged_prev(void); + void set_cruise_engaged_prev(bool engaged); + bool get_vehicle_moving(void); + int get_hw_type(void); + void set_timer(uint32_t t); + + void safety_tick_current_safety_config(); + bool safety_config_valid(); + + void init_tests(void); + + void set_honda_fwd_brake(bool c); + bool get_honda_fwd_brake(void); + void set_honda_alt_brake_msg(bool c); + void set_honda_bosch_long(bool c); + int get_honda_hw(void); + """) + +class PandaSafety(Protocol): + def set_controls_allowed(self, c: bool) -> None: ... + def get_controls_allowed(self) -> bool: ... + def get_longitudinal_allowed(self) -> bool: ... + def set_alternative_experience(self, mode: int) -> None: ... + def get_alternative_experience(self) -> int: ... + def set_relay_malfunction(self, c: bool) -> None: ... + def get_relay_malfunction(self) -> bool: ... + def get_gas_pressed_prev(self) -> bool: ... + def get_brake_pressed_prev(self) -> bool: ... + def get_regen_braking_prev(self) -> bool: ... + def get_acc_main_on(self) -> bool: ... + def get_vehicle_speed_min(self) -> int: ... + def get_vehicle_speed_max(self) -> int: ... + def get_vehicle_speed_last(self) -> int: ... + def get_current_safety_mode(self) -> int: ... + def get_current_safety_param(self) -> int: ... + + def set_torque_meas(self, min: int, max: int) -> None: ... # noqa: A002 + def get_torque_meas_min(self) -> int: ... + def get_torque_meas_max(self) -> int: ... + def set_torque_driver(self, min: int, max: int) -> None: ... # noqa: A002 + def get_torque_driver_min(self) -> int: ... + def get_torque_driver_max(self) -> int: ... + def set_desired_torque_last(self, t: int) -> None: ... + def set_rt_torque_last(self, t: int) -> None: ... + def set_desired_angle_last(self, t: int) -> None: ... + def get_desired_angle_last(self) -> int: ... + def set_angle_meas(self, min: int, max: int) -> None: ... # noqa: A002 + def get_angle_meas_min(self) -> int: ... + def get_angle_meas_max(self) -> int: ... + + def get_cruise_engaged_prev(self) -> bool: ... + def set_cruise_engaged_prev(self, enabled: bool) -> None: ... + def get_vehicle_moving(self) -> bool: ... + def get_hw_type(self) -> int: ... + def set_timer(self, t: int) -> None: ... + + def safety_tick_current_safety_config(self) -> None: ... + def safety_config_valid(self) -> bool: ... + + def init_tests(self) -> None: ... + + def set_honda_fwd_brake(self, c: bool) -> None: ... + def get_honda_fwd_brake(self) -> bool: ... + def set_honda_alt_brake_msg(self, c: bool) -> None: ... + def set_honda_bosch_long(self, c: bool) -> None: ... + def get_honda_hw(self) -> int: ... + + diff --git a/panda/tests/libs/resetter.py b/panda/tests/libs/resetter.py new file mode 100644 index 0000000000..3868bde674 --- /dev/null +++ b/panda/tests/libs/resetter.py @@ -0,0 +1,57 @@ +import time +import usb1 + + +class Resetter(): + def __init__(self): + self._handle = None + self.connect() + + def __enter__(self): + return self + + def __exit__(self, *args): + self.close() + + def close(self): + self._handle.close() + self._context.close() + self._handle = None + + def connect(self): + if self._handle: + self.close() + + self._handle = None + + self._context = usb1.USBContext() + self._context.open() + for device in self._context.getDeviceList(skip_on_error=True): + if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddc0: + try: + self._handle = device.open() + self._handle.claimInterface(0) + break + except Exception as e: + print(e) + assert self._handle + + def enable_power(self, port, enabled): + self._handle.controlWrite((usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE), 0xff, port, enabled, b'') + + def enable_boot(self, enabled): + self._handle.controlWrite((usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE), 0xff, 0, enabled, b'') + + def cycle_power(self, delay=5, dfu=False, ports=None): + if ports is None: + ports = [1, 2, 3] + + self.enable_boot(dfu) + for port in ports: + self.enable_power(port, False) + time.sleep(0.5) + + for port in ports: + self.enable_power(port, True) + time.sleep(delay) + self.enable_boot(False) diff --git a/panda/tests/loopback_test.py b/panda/tests/loopback_test.py new file mode 100755 index 0000000000..a95c2ea228 --- /dev/null +++ b/panda/tests/loopback_test.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +import os +import time +import random +import argparse +from itertools import permutations + +from panda import Panda + +def get_test_string(): + return b"test" + os.urandom(10) + +def run_test(sleep_duration): + pandas = Panda.list() + print(pandas) + + if len(pandas) < 2: + raise Exception("Minimum two pandas are needed for test") + + run_test_w_pandas(pandas, sleep_duration) + +def run_test_w_pandas(pandas, sleep_duration): + h = [Panda(x) for x in pandas] + print("H", h) + + for hh in h: + hh.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # test both directions + for ho in permutations(list(range(len(h))), r=2): + print("***************** TESTING", ho) + + panda0, panda1 = h[ho[0]], h[ho[1]] + + # **** test health packet **** + print("health", ho[0], h[ho[0]].health()) + + # **** test can line loopback **** + for bus, obd in [(0, False), (1, False), (2, False), (1, True), (2, True)]: + print("\ntest can", bus) + # flush + cans_echo = panda0.can_recv() + cans_loop = panda1.can_recv() + + panda0.set_obd(None) + panda1.set_obd(None) + + if obd is True: + panda0.set_obd(bus) + panda1.set_obd(bus) + bus = 3 + + # send the characters + at = random.randint(1, 2000) + st = get_test_string()[0:8] + panda0.can_send(at, st, bus) + time.sleep(0.1) + + # check for receive + cans_echo = panda0.can_recv() + cans_loop = panda1.can_recv() + + print("Bus", bus, "echo", cans_echo, "loop", cans_loop) + + assert len(cans_echo) == 1 + assert len(cans_loop) == 1 + + assert cans_echo[0][0] == at + assert cans_loop[0][0] == at + + assert cans_echo[0][2] == st + assert cans_loop[0][2] == st + + assert cans_echo[0][3] == 0x80 | bus + if cans_loop[0][3] != bus: + print("EXPECTED %d GOT %d" % (bus, cans_loop[0][3])) + assert cans_loop[0][3] == bus + + print("CAN pass", bus, ho) + time.sleep(sleep_duration) + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", type=int, help="Number of test iterations to run") + parser.add_argument("-sleep", type=int, help="Sleep time between tests", default=0) + args = parser.parse_args() + + if args.n is None: + while True: + run_test(sleep_duration=args.sleep) + else: + for _ in range(args.n): + run_test(sleep_duration=args.sleep) diff --git a/panda/tests/message_drop_test.py b/panda/tests/message_drop_test.py new file mode 100755 index 0000000000..bf485e4545 --- /dev/null +++ b/panda/tests/message_drop_test.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +import os +import usb1 +import time +import struct +import itertools +import threading +from typing import Any + +from panda import Panda + +JUNGLE = "JUNGLE" in os.environ +if JUNGLE: + from panda import PandaJungle + +# Generate unique messages +NUM_MESSAGES_PER_BUS = 10000 +messages = [bytes(struct.pack("Q", i)) for i in range(NUM_MESSAGES_PER_BUS)] +tx_messages = list(itertools.chain.from_iterable([[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] for msg in messages)) + +def flood_tx(panda): + print('Sending!') + transferred = 0 + while True: + try: + print(f"Sending block {transferred}-{len(tx_messages)}: ", end="") + panda.can_send_many(tx_messages[transferred:], timeout=10) + print("OK") + break + except usb1.USBErrorTimeout as e: + transferred += (e.transferred // 16) + print("timeout, transferred: ", transferred) + + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + +if __name__ == "__main__": + serials = Panda.list() + receiver: Panda | PandaJungle + if JUNGLE: + sender = Panda() + receiver = PandaJungle() + else: + if len(serials) != 2: + raise Exception("Connect two pandas to perform this test!") + sender = Panda(serials[0]) + receiver = Panda(serials[1]) + receiver.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # Start transmisson + threading.Thread(target=flood_tx, args=(sender,)).start() + + # Receive as much as we can, and stop when there hasn't been anything for a second + rx: list[Any] = [] + old_len = 0 + last_change = time.monotonic() + while time.monotonic() - last_change < 1: + if old_len < len(rx): + last_change = time.monotonic() + old_len = len(rx) + + rx.extend(receiver.can_recv()) + print(f"Received {len(rx)} messages") + + # Check if we received everything + for bus in range(3): + received_msgs = {bytes(m[2]) for m in filter(lambda m, b=bus: m[3] == b, rx)} # type: ignore + dropped_msgs = set(messages).difference(received_msgs) + print(f"Bus {bus} dropped msgs: {len(list(dropped_msgs))} / {len(messages)}") diff --git a/panda/tests/misra/.gitignore b/panda/tests/misra/.gitignore new file mode 100644 index 0000000000..fc9ac228cb --- /dev/null +++ b/panda/tests/misra/.gitignore @@ -0,0 +1,5 @@ +*.pdf +*.txt +.output.log +new_table +cppcheck/ diff --git a/panda/tests/misra/checkers.txt b/panda/tests/misra/checkers.txt new file mode 100644 index 0000000000..a9cc72173c --- /dev/null +++ b/panda/tests/misra/checkers.txt @@ -0,0 +1,867 @@ +Cppcheck checkers list from test_misra.sh: + + + + + +TEST variant options: +--enable=all --disable=unusedFunction -DPANDA --addon=misra -DSTM32F4 -DSTM32F413xx /board/main.c + + +Critical errors +--------------- +No critical errors, all files were checked. +Important: Analysis is still not guaranteed to be 'complete' it is possible there are false negatives. + + +Open source checkers +-------------------- +Yes Check64BitPortability::pointerassignment +Yes CheckAssert::assertWithSideEffects +Yes CheckAutoVariables::assignFunctionArg +Yes CheckAutoVariables::autoVariables +Yes CheckAutoVariables::checkVarLifetime +No CheckBool::checkAssignBoolToFloat require:style,c++ +Yes CheckBool::checkAssignBoolToPointer +No CheckBool::checkBitwiseOnBoolean require:style,inconclusive +Yes CheckBool::checkComparisonOfBoolExpressionWithInt +No CheckBool::checkComparisonOfBoolWithBool require:style,c++ +No CheckBool::checkComparisonOfBoolWithInt require:warning,c++ +No CheckBool::checkComparisonOfFuncReturningBool require:style,c++ +Yes CheckBool::checkIncrementBoolean +Yes CheckBool::pointerArithBool +Yes CheckBool::returnValueOfFunctionReturningBool +No CheckBoost::checkBoostForeachModification +Yes CheckBufferOverrun::analyseWholeProgram +Yes CheckBufferOverrun::argumentSize +Yes CheckBufferOverrun::arrayIndex +Yes CheckBufferOverrun::arrayIndexThenCheck +Yes CheckBufferOverrun::bufferOverflow +Yes CheckBufferOverrun::negativeArraySize +Yes CheckBufferOverrun::objectIndex +Yes CheckBufferOverrun::pointerArithmetic +No CheckBufferOverrun::stringNotZeroTerminated require:warning,inconclusive +Yes CheckClass::analyseWholeProgram +No CheckClass::checkConst require:style,inconclusive +No CheckClass::checkConstructors require:style,warning +No CheckClass::checkCopyConstructors require:warning +No CheckClass::checkDuplInheritedMembers require:warning +No CheckClass::checkExplicitConstructors require:style +No CheckClass::checkMemset +No CheckClass::checkMissingOverride require:style,c++03 +No CheckClass::checkReturnByReference require:performance +No CheckClass::checkSelfInitialization +No CheckClass::checkThisUseAfterFree require:warning +No CheckClass::checkUnsafeClassRefMember require:warning,safeChecks +No CheckClass::checkUselessOverride require:style +No CheckClass::checkVirtualFunctionCallInConstructor require:warning +No CheckClass::initializationListUsage require:performance +No CheckClass::initializerListOrder require:style,inconclusive +No CheckClass::operatorEqRetRefThis require:style +No CheckClass::operatorEqToSelf require:warning +No CheckClass::privateFunctions require:style +No CheckClass::thisSubtraction require:warning +No CheckClass::virtualDestructor +Yes CheckCondition::alwaysTrueFalse +Yes CheckCondition::assignIf +Yes CheckCondition::checkAssignmentInCondition +Yes CheckCondition::checkBadBitmaskCheck +Yes CheckCondition::checkCompareValueOutOfTypeRange +Yes CheckCondition::checkDuplicateConditionalAssign +Yes CheckCondition::checkIncorrectLogicOperator +Yes CheckCondition::checkInvalidTestForOverflow +Yes CheckCondition::checkModuloAlwaysTrueFalse +Yes CheckCondition::checkPointerAdditionResultNotNull +Yes CheckCondition::clarifyCondition +Yes CheckCondition::comparison +Yes CheckCondition::duplicateCondition +Yes CheckCondition::multiCondition +Yes CheckCondition::multiCondition2 +No CheckExceptionSafety::checkCatchExceptionByValue require:style +No CheckExceptionSafety::checkRethrowCopy require:style +No CheckExceptionSafety::deallocThrow require:warning +No CheckExceptionSafety::destructors require:warning +No CheckExceptionSafety::nothrowThrows +No CheckExceptionSafety::rethrowNoCurrentException +No CheckExceptionSafety::unhandledExceptionSpecification require:style,inconclusive +Yes CheckFunctions::checkIgnoredReturnValue +Yes CheckFunctions::checkMathFunctions +Yes CheckFunctions::checkMissingReturn +Yes CheckFunctions::checkProhibitedFunctions +Yes CheckFunctions::invalidFunctionUsage +Yes CheckFunctions::memsetInvalid2ndParam +Yes CheckFunctions::memsetZeroBytes +No CheckFunctions::returnLocalStdMove require:performance,c++11 +Yes CheckFunctions::useStandardLibrary +No CheckIO::checkCoutCerrMisusage require:c +Yes CheckIO::checkFileUsage +Yes CheckIO::checkWrongPrintfScanfArguments +Yes CheckIO::invalidScanf +Yes CheckLeakAutoVar::check +No CheckMemoryLeakInClass::check +Yes CheckMemoryLeakInFunction::checkReallocUsage +Yes CheckMemoryLeakNoVar::check +No CheckMemoryLeakNoVar::checkForUnsafeArgAlloc +Yes CheckMemoryLeakStructMember::check +Yes CheckNullPointer::analyseWholeProgram +Yes CheckNullPointer::arithmetic +Yes CheckNullPointer::nullConstantDereference +Yes CheckNullPointer::nullPointer +No CheckOther::checkAccessOfMovedVariable require:c++11,warning +Yes CheckOther::checkCastIntToCharAndBack +Yes CheckOther::checkCharVariable +Yes CheckOther::checkComparePointers +Yes CheckOther::checkComparisonFunctionIsAlwaysTrueOrFalse +Yes CheckOther::checkConstPointer +No CheckOther::checkConstVariable require:style,c++ +No CheckOther::checkDuplicateBranch require:style,inconclusive +Yes CheckOther::checkDuplicateExpression +Yes CheckOther::checkEvaluationOrder +Yes CheckOther::checkFuncArgNamesDifferent +No CheckOther::checkIncompleteArrayFill require:warning,portability,inconclusive +Yes CheckOther::checkIncompleteStatement +No CheckOther::checkInterlockedDecrement require:windows-platform +Yes CheckOther::checkInvalidFree +Yes CheckOther::checkKnownArgument +Yes CheckOther::checkKnownPointerToBool +No CheckOther::checkMisusedScopedObject require:style,c++ +Yes CheckOther::checkModuloOfOne +Yes CheckOther::checkNanInArithmeticExpression +Yes CheckOther::checkNegativeBitwiseShift +Yes CheckOther::checkOverlappingWrite +No CheckOther::checkPassByReference require:performance,c++ +Yes CheckOther::checkRedundantAssignment +No CheckOther::checkRedundantCopy require:c++,performance,inconclusive +Yes CheckOther::checkRedundantPointerOp +Yes CheckOther::checkShadowVariables +Yes CheckOther::checkSignOfUnsignedVariable +No CheckOther::checkSuspiciousCaseInSwitch require:warning,inconclusive +No CheckOther::checkSuspiciousSemicolon require:warning,inconclusive +Yes CheckOther::checkUnreachableCode +Yes CheckOther::checkUnusedLabel +Yes CheckOther::checkVarFuncNullUB +Yes CheckOther::checkVariableScope +Yes CheckOther::checkZeroDivision +Yes CheckOther::clarifyCalculation +Yes CheckOther::clarifyStatement +Yes CheckOther::invalidPointerCast +Yes CheckOther::redundantBitwiseOperationInSwitch +No CheckOther::warningOldStylePointerCast require:style,c++ +No CheckPostfixOperator::postfixOperator require:performance +Yes CheckSizeof::checkSizeofForArrayParameter +Yes CheckSizeof::checkSizeofForNumericParameter +Yes CheckSizeof::checkSizeofForPointerSize +Yes CheckSizeof::sizeofCalculation +Yes CheckSizeof::sizeofFunction +Yes CheckSizeof::sizeofVoid +Yes CheckSizeof::sizeofsizeof +No CheckSizeof::suspiciousSizeofCalculation require:warning,inconclusive +No CheckStl::checkDereferenceInvalidIterator require:warning +No CheckStl::checkDereferenceInvalidIterator2 +No CheckStl::checkFindInsert require:performance +No CheckStl::checkMutexes require:warning +No CheckStl::erase +No CheckStl::eraseIteratorOutOfBounds +No CheckStl::if_find require:warning,performance +No CheckStl::invalidContainer +No CheckStl::iterators +No CheckStl::knownEmptyContainer require:style +No CheckStl::misMatchingContainerIterator +No CheckStl::misMatchingContainers +No CheckStl::missingComparison require:warning +No CheckStl::negativeIndex +No CheckStl::outOfBounds +No CheckStl::outOfBoundsIndexExpression +No CheckStl::redundantCondition require:style +No CheckStl::size require:performance,c++03 +No CheckStl::stlBoundaries +No CheckStl::stlOutOfBounds +No CheckStl::string_c_str +No CheckStl::useStlAlgorithm require:style +No CheckStl::uselessCalls require:performance,warning +Yes CheckString::checkAlwaysTrueOrFalseStringCompare +Yes CheckString::checkIncorrectStringCompare +Yes CheckString::checkSuspiciousStringCompare +Yes CheckString::overlappingStrcmp +Yes CheckString::sprintfOverlappingData +Yes CheckString::strPlusChar +Yes CheckString::stringLiteralWrite +Yes CheckType::checkFloatToIntegerOverflow +Yes CheckType::checkIntegerOverflow +Yes CheckType::checkLongCast +Yes CheckType::checkSignConversion +Yes CheckType::checkTooBigBitwiseShift +Yes CheckUninitVar::check +Yes CheckUninitVar::valueFlowUninit +No CheckUnusedFunctions::check require:unusedFunction +Yes CheckUnusedVar::checkFunctionVariableUsage +Yes CheckUnusedVar::checkStructMemberUsage +Yes CheckVaarg::va_list_usage +Yes CheckVaarg::va_start_argument + + +Premium checkers +---------------- +Not available, Cppcheck Premium is not used + + +Autosar +------- +Not available, Cppcheck Premium is not used + + +Cert C +------ +Not available, Cppcheck Premium is not used + + +Cert C++ +-------- +Not available, Cppcheck Premium is not used + + +Misra C 2012 +------------ +Yes Misra C 2012: 1.1 +Yes Misra C 2012: 1.2 +Yes Misra C 2012: 1.3 +Yes Misra C 2012: 1.4 amendment:2 +No Misra C 2012: 1.5 amendment:3 require:premium +Yes Misra C 2012: 2.1 +Yes Misra C 2012: 2.2 +Yes Misra C 2012: 2.3 +Yes Misra C 2012: 2.4 +Yes Misra C 2012: 2.5 +Yes Misra C 2012: 2.6 +Yes Misra C 2012: 2.7 +Yes Misra C 2012: 2.8 +Yes Misra C 2012: 3.1 +Yes Misra C 2012: 3.2 +Yes Misra C 2012: 4.1 +Yes Misra C 2012: 4.2 +Yes Misra C 2012: 5.1 +Yes Misra C 2012: 5.2 +Yes Misra C 2012: 5.3 +Yes Misra C 2012: 5.4 +Yes Misra C 2012: 5.5 +Yes Misra C 2012: 5.6 +Yes Misra C 2012: 5.7 +Yes Misra C 2012: 5.8 +Yes Misra C 2012: 5.9 +Yes Misra C 2012: 6.1 +Yes Misra C 2012: 6.2 +No Misra C 2012: 6.3 +Yes Misra C 2012: 7.1 +Yes Misra C 2012: 7.2 +Yes Misra C 2012: 7.3 +Yes Misra C 2012: 7.4 +No Misra C 2012: 7.5 +No Misra C 2012: 7.6 +Yes Misra C 2012: 8.1 +Yes Misra C 2012: 8.2 +No Misra C 2012: 8.3 +Yes Misra C 2012: 8.4 +Yes Misra C 2012: 8.5 +Yes Misra C 2012: 8.6 +Yes Misra C 2012: 8.7 +Yes Misra C 2012: 8.8 +Yes Misra C 2012: 8.9 +Yes Misra C 2012: 8.10 +Yes Misra C 2012: 8.11 +Yes Misra C 2012: 8.12 +Yes Misra C 2012: 8.13 +Yes Misra C 2012: 8.14 +No Misra C 2012: 8.15 +No Misra C 2012: 8.16 +No Misra C 2012: 8.17 +Yes Misra C 2012: 9.1 +Yes Misra C 2012: 9.2 +Yes Misra C 2012: 9.3 +Yes Misra C 2012: 9.4 +Yes Misra C 2012: 9.5 +No Misra C 2012: 9.6 +No Misra C 2012: 9.7 +Yes Misra C 2012: 10.1 +Yes Misra C 2012: 10.2 +Yes Misra C 2012: 10.3 +Yes Misra C 2012: 10.4 +Yes Misra C 2012: 10.5 +Yes Misra C 2012: 10.6 +Yes Misra C 2012: 10.7 +Yes Misra C 2012: 10.8 +Yes Misra C 2012: 11.1 +Yes Misra C 2012: 11.2 +Yes Misra C 2012: 11.3 +Yes Misra C 2012: 11.4 +Yes Misra C 2012: 11.5 +Yes Misra C 2012: 11.6 +Yes Misra C 2012: 11.7 +Yes Misra C 2012: 11.8 +Yes Misra C 2012: 11.9 +No Misra C 2012: 11.10 +Yes Misra C 2012: 12.1 +Yes Misra C 2012: 12.2 +Yes Misra C 2012: 12.3 +Yes Misra C 2012: 12.4 +Yes Misra C 2012: 12.5 amendment:1 +No Misra C 2012: 12.6 amendment:4 require:premium +Yes Misra C 2012: 13.1 +No Misra C 2012: 13.2 +Yes Misra C 2012: 13.3 +Yes Misra C 2012: 13.4 +Yes Misra C 2012: 13.5 +Yes Misra C 2012: 13.6 +Yes Misra C 2012: 14.1 +Yes Misra C 2012: 14.2 +Yes Misra C 2012: 14.3 +Yes Misra C 2012: 14.4 +Yes Misra C 2012: 15.1 +Yes Misra C 2012: 15.2 +Yes Misra C 2012: 15.3 +Yes Misra C 2012: 15.4 +Yes Misra C 2012: 15.5 +Yes Misra C 2012: 15.6 +Yes Misra C 2012: 15.7 +Yes Misra C 2012: 16.1 +Yes Misra C 2012: 16.2 +Yes Misra C 2012: 16.3 +Yes Misra C 2012: 16.4 +Yes Misra C 2012: 16.5 +Yes Misra C 2012: 16.6 +Yes Misra C 2012: 16.7 +Yes Misra C 2012: 17.1 +Yes Misra C 2012: 17.2 +Yes Misra C 2012: 17.3 +No Misra C 2012: 17.4 +Yes Misra C 2012: 17.5 +Yes Misra C 2012: 17.6 +Yes Misra C 2012: 17.7 +Yes Misra C 2012: 17.8 +No Misra C 2012: 17.9 +No Misra C 2012: 17.10 +No Misra C 2012: 17.11 +No Misra C 2012: 17.12 +No Misra C 2012: 17.13 +Yes Misra C 2012: 18.1 +Yes Misra C 2012: 18.2 +Yes Misra C 2012: 18.3 +Yes Misra C 2012: 18.4 +Yes Misra C 2012: 18.5 +Yes Misra C 2012: 18.6 +Yes Misra C 2012: 18.7 +Yes Misra C 2012: 18.8 +No Misra C 2012: 18.9 +No Misra C 2012: 18.10 +Yes Misra C 2012: 19.1 +Yes Misra C 2012: 19.2 +Yes Misra C 2012: 20.1 +Yes Misra C 2012: 20.2 +Yes Misra C 2012: 20.3 +Yes Misra C 2012: 20.4 +Yes Misra C 2012: 20.5 +Yes Misra C 2012: 20.6 +Yes Misra C 2012: 20.7 +Yes Misra C 2012: 20.8 +Yes Misra C 2012: 20.9 +Yes Misra C 2012: 20.10 +Yes Misra C 2012: 20.11 +Yes Misra C 2012: 20.12 +Yes Misra C 2012: 20.13 +Yes Misra C 2012: 20.14 +Yes Misra C 2012: 21.1 +Yes Misra C 2012: 21.2 +Yes Misra C 2012: 21.3 +Yes Misra C 2012: 21.4 +Yes Misra C 2012: 21.5 +Yes Misra C 2012: 21.6 +Yes Misra C 2012: 21.7 +Yes Misra C 2012: 21.8 +Yes Misra C 2012: 21.9 +Yes Misra C 2012: 21.10 +Yes Misra C 2012: 21.11 +Yes Misra C 2012: 21.12 +Yes Misra C 2012: 21.13 amendment:1 +Yes Misra C 2012: 21.14 amendment:1 +Yes Misra C 2012: 21.15 amendment:1 +Yes Misra C 2012: 21.16 amendment:1 +Yes Misra C 2012: 21.17 amendment:1 +Yes Misra C 2012: 21.18 amendment:1 +Yes Misra C 2012: 21.19 amendment:1 +Yes Misra C 2012: 21.20 amendment:1 +Yes Misra C 2012: 21.21 amendment:3 +No Misra C 2012: 21.22 amendment:3 require:premium +No Misra C 2012: 21.23 amendment:3 require:premium +No Misra C 2012: 21.24 amendment:3 require:premium +No Misra C 2012: 21.25 amendment:4 require:premium +No Misra C 2012: 21.26 amendment:4 require:premium +Yes Misra C 2012: 22.1 +Yes Misra C 2012: 22.2 +Yes Misra C 2012: 22.3 +Yes Misra C 2012: 22.4 +Yes Misra C 2012: 22.5 +Yes Misra C 2012: 22.6 +Yes Misra C 2012: 22.7 amendment:1 +Yes Misra C 2012: 22.8 amendment:1 +Yes Misra C 2012: 22.9 amendment:1 +Yes Misra C 2012: 22.10 amendment:1 +No Misra C 2012: 22.11 amendment:4 require:premium +No Misra C 2012: 22.12 amendment:4 require:premium +No Misra C 2012: 22.13 amendment:4 require:premium +No Misra C 2012: 22.14 amendment:4 require:premium +No Misra C 2012: 22.15 amendment:4 require:premium +No Misra C 2012: 22.16 amendment:4 require:premium +No Misra C 2012: 22.17 amendment:4 require:premium +No Misra C 2012: 22.18 amendment:4 require:premium +No Misra C 2012: 22.19 amendment:4 require:premium +No Misra C 2012: 22.20 amendment:4 require:premium +No Misra C 2012: 23.1 amendment:3 require:premium +No Misra C 2012: 23.2 amendment:3 require:premium +No Misra C 2012: 23.3 amendment:3 require:premium +No Misra C 2012: 23.4 amendment:3 require:premium +No Misra C 2012: 23.5 amendment:3 require:premium +No Misra C 2012: 23.6 amendment:3 require:premium +No Misra C 2012: 23.7 amendment:3 require:premium +No Misra C 2012: 23.8 amendment:3 require:premium + + +Misra C++ 2008 +-------------- +Not available, Cppcheck Premium is not used + + +Misra C++ 2023 +-------------- +Not available, Cppcheck Premium is not used + + + + + +TEST variant options: +--enable=all --disable=unusedFunction -DPANDA --addon=misra -DSTM32H7 -DSTM32H725xx /board/main.c + + +Critical errors +--------------- +No critical errors, all files were checked. +Important: Analysis is still not guaranteed to be 'complete' it is possible there are false negatives. + + +Open source checkers +-------------------- +Yes Check64BitPortability::pointerassignment +Yes CheckAssert::assertWithSideEffects +Yes CheckAutoVariables::assignFunctionArg +Yes CheckAutoVariables::autoVariables +Yes CheckAutoVariables::checkVarLifetime +No CheckBool::checkAssignBoolToFloat require:style,c++ +Yes CheckBool::checkAssignBoolToPointer +No CheckBool::checkBitwiseOnBoolean require:style,inconclusive +Yes CheckBool::checkComparisonOfBoolExpressionWithInt +No CheckBool::checkComparisonOfBoolWithBool require:style,c++ +No CheckBool::checkComparisonOfBoolWithInt require:warning,c++ +No CheckBool::checkComparisonOfFuncReturningBool require:style,c++ +Yes CheckBool::checkIncrementBoolean +Yes CheckBool::pointerArithBool +Yes CheckBool::returnValueOfFunctionReturningBool +No CheckBoost::checkBoostForeachModification +Yes CheckBufferOverrun::analyseWholeProgram +Yes CheckBufferOverrun::argumentSize +Yes CheckBufferOverrun::arrayIndex +Yes CheckBufferOverrun::arrayIndexThenCheck +Yes CheckBufferOverrun::bufferOverflow +Yes CheckBufferOverrun::negativeArraySize +Yes CheckBufferOverrun::objectIndex +Yes CheckBufferOverrun::pointerArithmetic +No CheckBufferOverrun::stringNotZeroTerminated require:warning,inconclusive +Yes CheckClass::analyseWholeProgram +No CheckClass::checkConst require:style,inconclusive +No CheckClass::checkConstructors require:style,warning +No CheckClass::checkCopyConstructors require:warning +No CheckClass::checkDuplInheritedMembers require:warning +No CheckClass::checkExplicitConstructors require:style +No CheckClass::checkMemset +No CheckClass::checkMissingOverride require:style,c++03 +No CheckClass::checkReturnByReference require:performance +No CheckClass::checkSelfInitialization +No CheckClass::checkThisUseAfterFree require:warning +No CheckClass::checkUnsafeClassRefMember require:warning,safeChecks +No CheckClass::checkUselessOverride require:style +No CheckClass::checkVirtualFunctionCallInConstructor require:warning +No CheckClass::initializationListUsage require:performance +No CheckClass::initializerListOrder require:style,inconclusive +No CheckClass::operatorEqRetRefThis require:style +No CheckClass::operatorEqToSelf require:warning +No CheckClass::privateFunctions require:style +No CheckClass::thisSubtraction require:warning +No CheckClass::virtualDestructor +Yes CheckCondition::alwaysTrueFalse +Yes CheckCondition::assignIf +Yes CheckCondition::checkAssignmentInCondition +Yes CheckCondition::checkBadBitmaskCheck +Yes CheckCondition::checkCompareValueOutOfTypeRange +Yes CheckCondition::checkDuplicateConditionalAssign +Yes CheckCondition::checkIncorrectLogicOperator +Yes CheckCondition::checkInvalidTestForOverflow +Yes CheckCondition::checkModuloAlwaysTrueFalse +Yes CheckCondition::checkPointerAdditionResultNotNull +Yes CheckCondition::clarifyCondition +Yes CheckCondition::comparison +Yes CheckCondition::duplicateCondition +Yes CheckCondition::multiCondition +Yes CheckCondition::multiCondition2 +No CheckExceptionSafety::checkCatchExceptionByValue require:style +No CheckExceptionSafety::checkRethrowCopy require:style +No CheckExceptionSafety::deallocThrow require:warning +No CheckExceptionSafety::destructors require:warning +No CheckExceptionSafety::nothrowThrows +No CheckExceptionSafety::rethrowNoCurrentException +No CheckExceptionSafety::unhandledExceptionSpecification require:style,inconclusive +Yes CheckFunctions::checkIgnoredReturnValue +Yes CheckFunctions::checkMathFunctions +Yes CheckFunctions::checkMissingReturn +Yes CheckFunctions::checkProhibitedFunctions +Yes CheckFunctions::invalidFunctionUsage +Yes CheckFunctions::memsetInvalid2ndParam +Yes CheckFunctions::memsetZeroBytes +No CheckFunctions::returnLocalStdMove require:performance,c++11 +Yes CheckFunctions::useStandardLibrary +No CheckIO::checkCoutCerrMisusage require:c +Yes CheckIO::checkFileUsage +Yes CheckIO::checkWrongPrintfScanfArguments +Yes CheckIO::invalidScanf +Yes CheckLeakAutoVar::check +No CheckMemoryLeakInClass::check +Yes CheckMemoryLeakInFunction::checkReallocUsage +Yes CheckMemoryLeakNoVar::check +No CheckMemoryLeakNoVar::checkForUnsafeArgAlloc +Yes CheckMemoryLeakStructMember::check +Yes CheckNullPointer::analyseWholeProgram +Yes CheckNullPointer::arithmetic +Yes CheckNullPointer::nullConstantDereference +Yes CheckNullPointer::nullPointer +No CheckOther::checkAccessOfMovedVariable require:c++11,warning +Yes CheckOther::checkCastIntToCharAndBack +Yes CheckOther::checkCharVariable +Yes CheckOther::checkComparePointers +Yes CheckOther::checkComparisonFunctionIsAlwaysTrueOrFalse +Yes CheckOther::checkConstPointer +No CheckOther::checkConstVariable require:style,c++ +No CheckOther::checkDuplicateBranch require:style,inconclusive +Yes CheckOther::checkDuplicateExpression +Yes CheckOther::checkEvaluationOrder +Yes CheckOther::checkFuncArgNamesDifferent +No CheckOther::checkIncompleteArrayFill require:warning,portability,inconclusive +Yes CheckOther::checkIncompleteStatement +No CheckOther::checkInterlockedDecrement require:windows-platform +Yes CheckOther::checkInvalidFree +Yes CheckOther::checkKnownArgument +Yes CheckOther::checkKnownPointerToBool +No CheckOther::checkMisusedScopedObject require:style,c++ +Yes CheckOther::checkModuloOfOne +Yes CheckOther::checkNanInArithmeticExpression +Yes CheckOther::checkNegativeBitwiseShift +Yes CheckOther::checkOverlappingWrite +No CheckOther::checkPassByReference require:performance,c++ +Yes CheckOther::checkRedundantAssignment +No CheckOther::checkRedundantCopy require:c++,performance,inconclusive +Yes CheckOther::checkRedundantPointerOp +Yes CheckOther::checkShadowVariables +Yes CheckOther::checkSignOfUnsignedVariable +No CheckOther::checkSuspiciousCaseInSwitch require:warning,inconclusive +No CheckOther::checkSuspiciousSemicolon require:warning,inconclusive +Yes CheckOther::checkUnreachableCode +Yes CheckOther::checkUnusedLabel +Yes CheckOther::checkVarFuncNullUB +Yes CheckOther::checkVariableScope +Yes CheckOther::checkZeroDivision +Yes CheckOther::clarifyCalculation +Yes CheckOther::clarifyStatement +Yes CheckOther::invalidPointerCast +Yes CheckOther::redundantBitwiseOperationInSwitch +No CheckOther::warningOldStylePointerCast require:style,c++ +No CheckPostfixOperator::postfixOperator require:performance +Yes CheckSizeof::checkSizeofForArrayParameter +Yes CheckSizeof::checkSizeofForNumericParameter +Yes CheckSizeof::checkSizeofForPointerSize +Yes CheckSizeof::sizeofCalculation +Yes CheckSizeof::sizeofFunction +Yes CheckSizeof::sizeofVoid +Yes CheckSizeof::sizeofsizeof +No CheckSizeof::suspiciousSizeofCalculation require:warning,inconclusive +No CheckStl::checkDereferenceInvalidIterator require:warning +No CheckStl::checkDereferenceInvalidIterator2 +No CheckStl::checkFindInsert require:performance +No CheckStl::checkMutexes require:warning +No CheckStl::erase +No CheckStl::eraseIteratorOutOfBounds +No CheckStl::if_find require:warning,performance +No CheckStl::invalidContainer +No CheckStl::iterators +No CheckStl::knownEmptyContainer require:style +No CheckStl::misMatchingContainerIterator +No CheckStl::misMatchingContainers +No CheckStl::missingComparison require:warning +No CheckStl::negativeIndex +No CheckStl::outOfBounds +No CheckStl::outOfBoundsIndexExpression +No CheckStl::redundantCondition require:style +No CheckStl::size require:performance,c++03 +No CheckStl::stlBoundaries +No CheckStl::stlOutOfBounds +No CheckStl::string_c_str +No CheckStl::useStlAlgorithm require:style +No CheckStl::uselessCalls require:performance,warning +Yes CheckString::checkAlwaysTrueOrFalseStringCompare +Yes CheckString::checkIncorrectStringCompare +Yes CheckString::checkSuspiciousStringCompare +Yes CheckString::overlappingStrcmp +Yes CheckString::sprintfOverlappingData +Yes CheckString::strPlusChar +Yes CheckString::stringLiteralWrite +Yes CheckType::checkFloatToIntegerOverflow +Yes CheckType::checkIntegerOverflow +Yes CheckType::checkLongCast +Yes CheckType::checkSignConversion +Yes CheckType::checkTooBigBitwiseShift +Yes CheckUninitVar::check +Yes CheckUninitVar::valueFlowUninit +No CheckUnusedFunctions::check require:unusedFunction +Yes CheckUnusedVar::checkFunctionVariableUsage +Yes CheckUnusedVar::checkStructMemberUsage +Yes CheckVaarg::va_list_usage +Yes CheckVaarg::va_start_argument + + +Premium checkers +---------------- +Not available, Cppcheck Premium is not used + + +Autosar +------- +Not available, Cppcheck Premium is not used + + +Cert C +------ +Not available, Cppcheck Premium is not used + + +Cert C++ +-------- +Not available, Cppcheck Premium is not used + + +Misra C 2012 +------------ +Yes Misra C 2012: 1.1 +Yes Misra C 2012: 1.2 +Yes Misra C 2012: 1.3 +Yes Misra C 2012: 1.4 amendment:2 +No Misra C 2012: 1.5 amendment:3 require:premium +Yes Misra C 2012: 2.1 +Yes Misra C 2012: 2.2 +Yes Misra C 2012: 2.3 +Yes Misra C 2012: 2.4 +Yes Misra C 2012: 2.5 +Yes Misra C 2012: 2.6 +Yes Misra C 2012: 2.7 +Yes Misra C 2012: 2.8 +Yes Misra C 2012: 3.1 +Yes Misra C 2012: 3.2 +Yes Misra C 2012: 4.1 +Yes Misra C 2012: 4.2 +Yes Misra C 2012: 5.1 +Yes Misra C 2012: 5.2 +Yes Misra C 2012: 5.3 +Yes Misra C 2012: 5.4 +Yes Misra C 2012: 5.5 +Yes Misra C 2012: 5.6 +Yes Misra C 2012: 5.7 +Yes Misra C 2012: 5.8 +Yes Misra C 2012: 5.9 +Yes Misra C 2012: 6.1 +Yes Misra C 2012: 6.2 +No Misra C 2012: 6.3 +Yes Misra C 2012: 7.1 +Yes Misra C 2012: 7.2 +Yes Misra C 2012: 7.3 +Yes Misra C 2012: 7.4 +No Misra C 2012: 7.5 +No Misra C 2012: 7.6 +Yes Misra C 2012: 8.1 +Yes Misra C 2012: 8.2 +No Misra C 2012: 8.3 +Yes Misra C 2012: 8.4 +Yes Misra C 2012: 8.5 +Yes Misra C 2012: 8.6 +Yes Misra C 2012: 8.7 +Yes Misra C 2012: 8.8 +Yes Misra C 2012: 8.9 +Yes Misra C 2012: 8.10 +Yes Misra C 2012: 8.11 +Yes Misra C 2012: 8.12 +Yes Misra C 2012: 8.13 +Yes Misra C 2012: 8.14 +No Misra C 2012: 8.15 +No Misra C 2012: 8.16 +No Misra C 2012: 8.17 +Yes Misra C 2012: 9.1 +Yes Misra C 2012: 9.2 +Yes Misra C 2012: 9.3 +Yes Misra C 2012: 9.4 +Yes Misra C 2012: 9.5 +No Misra C 2012: 9.6 +No Misra C 2012: 9.7 +Yes Misra C 2012: 10.1 +Yes Misra C 2012: 10.2 +Yes Misra C 2012: 10.3 +Yes Misra C 2012: 10.4 +Yes Misra C 2012: 10.5 +Yes Misra C 2012: 10.6 +Yes Misra C 2012: 10.7 +Yes Misra C 2012: 10.8 +Yes Misra C 2012: 11.1 +Yes Misra C 2012: 11.2 +Yes Misra C 2012: 11.3 +Yes Misra C 2012: 11.4 +Yes Misra C 2012: 11.5 +Yes Misra C 2012: 11.6 +Yes Misra C 2012: 11.7 +Yes Misra C 2012: 11.8 +Yes Misra C 2012: 11.9 +No Misra C 2012: 11.10 +Yes Misra C 2012: 12.1 +Yes Misra C 2012: 12.2 +Yes Misra C 2012: 12.3 +Yes Misra C 2012: 12.4 +Yes Misra C 2012: 12.5 amendment:1 +No Misra C 2012: 12.6 amendment:4 require:premium +Yes Misra C 2012: 13.1 +No Misra C 2012: 13.2 +Yes Misra C 2012: 13.3 +Yes Misra C 2012: 13.4 +Yes Misra C 2012: 13.5 +Yes Misra C 2012: 13.6 +Yes Misra C 2012: 14.1 +Yes Misra C 2012: 14.2 +Yes Misra C 2012: 14.3 +Yes Misra C 2012: 14.4 +Yes Misra C 2012: 15.1 +Yes Misra C 2012: 15.2 +Yes Misra C 2012: 15.3 +Yes Misra C 2012: 15.4 +Yes Misra C 2012: 15.5 +Yes Misra C 2012: 15.6 +Yes Misra C 2012: 15.7 +Yes Misra C 2012: 16.1 +Yes Misra C 2012: 16.2 +Yes Misra C 2012: 16.3 +Yes Misra C 2012: 16.4 +Yes Misra C 2012: 16.5 +Yes Misra C 2012: 16.6 +Yes Misra C 2012: 16.7 +Yes Misra C 2012: 17.1 +Yes Misra C 2012: 17.2 +Yes Misra C 2012: 17.3 +No Misra C 2012: 17.4 +Yes Misra C 2012: 17.5 +Yes Misra C 2012: 17.6 +Yes Misra C 2012: 17.7 +Yes Misra C 2012: 17.8 +No Misra C 2012: 17.9 +No Misra C 2012: 17.10 +No Misra C 2012: 17.11 +No Misra C 2012: 17.12 +No Misra C 2012: 17.13 +Yes Misra C 2012: 18.1 +Yes Misra C 2012: 18.2 +Yes Misra C 2012: 18.3 +Yes Misra C 2012: 18.4 +Yes Misra C 2012: 18.5 +Yes Misra C 2012: 18.6 +Yes Misra C 2012: 18.7 +Yes Misra C 2012: 18.8 +No Misra C 2012: 18.9 +No Misra C 2012: 18.10 +Yes Misra C 2012: 19.1 +Yes Misra C 2012: 19.2 +Yes Misra C 2012: 20.1 +Yes Misra C 2012: 20.2 +Yes Misra C 2012: 20.3 +Yes Misra C 2012: 20.4 +Yes Misra C 2012: 20.5 +Yes Misra C 2012: 20.6 +Yes Misra C 2012: 20.7 +Yes Misra C 2012: 20.8 +Yes Misra C 2012: 20.9 +Yes Misra C 2012: 20.10 +Yes Misra C 2012: 20.11 +Yes Misra C 2012: 20.12 +Yes Misra C 2012: 20.13 +Yes Misra C 2012: 20.14 +Yes Misra C 2012: 21.1 +Yes Misra C 2012: 21.2 +Yes Misra C 2012: 21.3 +Yes Misra C 2012: 21.4 +Yes Misra C 2012: 21.5 +Yes Misra C 2012: 21.6 +Yes Misra C 2012: 21.7 +Yes Misra C 2012: 21.8 +Yes Misra C 2012: 21.9 +Yes Misra C 2012: 21.10 +Yes Misra C 2012: 21.11 +Yes Misra C 2012: 21.12 +Yes Misra C 2012: 21.13 amendment:1 +Yes Misra C 2012: 21.14 amendment:1 +Yes Misra C 2012: 21.15 amendment:1 +Yes Misra C 2012: 21.16 amendment:1 +Yes Misra C 2012: 21.17 amendment:1 +Yes Misra C 2012: 21.18 amendment:1 +Yes Misra C 2012: 21.19 amendment:1 +Yes Misra C 2012: 21.20 amendment:1 +Yes Misra C 2012: 21.21 amendment:3 +No Misra C 2012: 21.22 amendment:3 require:premium +No Misra C 2012: 21.23 amendment:3 require:premium +No Misra C 2012: 21.24 amendment:3 require:premium +No Misra C 2012: 21.25 amendment:4 require:premium +No Misra C 2012: 21.26 amendment:4 require:premium +Yes Misra C 2012: 22.1 +Yes Misra C 2012: 22.2 +Yes Misra C 2012: 22.3 +Yes Misra C 2012: 22.4 +Yes Misra C 2012: 22.5 +Yes Misra C 2012: 22.6 +Yes Misra C 2012: 22.7 amendment:1 +Yes Misra C 2012: 22.8 amendment:1 +Yes Misra C 2012: 22.9 amendment:1 +Yes Misra C 2012: 22.10 amendment:1 +No Misra C 2012: 22.11 amendment:4 require:premium +No Misra C 2012: 22.12 amendment:4 require:premium +No Misra C 2012: 22.13 amendment:4 require:premium +No Misra C 2012: 22.14 amendment:4 require:premium +No Misra C 2012: 22.15 amendment:4 require:premium +No Misra C 2012: 22.16 amendment:4 require:premium +No Misra C 2012: 22.17 amendment:4 require:premium +No Misra C 2012: 22.18 amendment:4 require:premium +No Misra C 2012: 22.19 amendment:4 require:premium +No Misra C 2012: 22.20 amendment:4 require:premium +No Misra C 2012: 23.1 amendment:3 require:premium +No Misra C 2012: 23.2 amendment:3 require:premium +No Misra C 2012: 23.3 amendment:3 require:premium +No Misra C 2012: 23.4 amendment:3 require:premium +No Misra C 2012: 23.5 amendment:3 require:premium +No Misra C 2012: 23.6 amendment:3 require:premium +No Misra C 2012: 23.7 amendment:3 require:premium +No Misra C 2012: 23.8 amendment:3 require:premium + + +Misra C++ 2008 +-------------- +Not available, Cppcheck Premium is not used + + +Misra C++ 2023 +-------------- +Not available, Cppcheck Premium is not used diff --git a/panda/tests/misra/coverage_table b/panda/tests/misra/coverage_table new file mode 100644 index 0000000000..0395aba0d6 --- /dev/null +++ b/panda/tests/misra/coverage_table @@ -0,0 +1,156 @@ +1.1 +1.2 X (Addon) +1.3 X (Cppcheck) +2.1 X (Cppcheck) +2.2 X (Addon) +2.3 X (Addon) +2.4 X (Addon) +2.5 X (Addon) +2.6 X (Cppcheck) +2.7 X (Addon) +3.1 X (Addon) +3.2 X (Addon) +4.1 X (Addon) +4.2 X (Addon) +5.1 X (Addon) +5.2 X (Addon) +5.3 X (Cppcheck) +5.4 X (Addon) +5.5 X (Addon) +5.6 X (Addon) +5.7 X (Addon) +5.8 X (Addon) +5.9 X (Addon) +6.1 X (Addon) +6.2 X (Addon) +7.1 X (Addon) +7.2 X (Addon) +7.3 X (Addon) +7.4 X (Addon) +8.1 X (Addon) +8.2 X (Addon) +8.3 X (Cppcheck) +8.4 X (Addon) +8.5 X (Addon) +8.6 X (Addon) +8.7 X (Addon) +8.8 X (Addon) +8.9 X (Addon) +8.10 X (Addon) +8.11 X (Addon) +8.12 X (Addon) +8.13 X (Cppcheck) +8.14 X (Addon) +9.1 X (Cppcheck) +9.2 X (Addon) +9.3 X (Addon) +9.4 X (Addon) +9.5 X (Addon) +10.1 X (Addon) +10.2 X (Addon) +10.3 X (Addon) +10.4 X (Addon) +10.5 X (Addon) +10.6 X (Addon) +10.7 X (Addon) +10.8 X (Addon) +11.1 X (Addon) +11.2 X (Addon) +11.3 X (Addon) +11.4 X (Addon) +11.5 X (Addon) +11.6 X (Addon) +11.7 X (Addon) +11.8 X (Addon) +11.9 X (Addon) +12.1 X (Addon) +12.2 X (Addon) +12.3 X (Addon) +12.4 X (Addon) +13.1 X (Addon) +13.2 X (Cppcheck) +13.3 X (Addon) +13.4 X (Addon) +13.5 X (Addon) +13.6 X (Addon) +14.1 X (Addon) +14.2 X (Addon) +14.3 X (Cppcheck) +14.4 X (Addon) +15.1 X (Addon) +15.2 X (Addon) +15.3 X (Addon) +15.4 X (Addon) +15.5 X (Addon) +15.6 X (Addon) +15.7 X (Addon) +16.1 X (Addon) +16.2 X (Addon) +16.3 X (Addon) +16.4 X (Addon) +16.5 X (Addon) +16.6 X (Addon) +16.7 X (Addon) +17.1 X (Addon) +17.2 X (Addon) +17.3 X (Addon) +17.4 X (Cppcheck) +17.5 X (Cppcheck) +17.6 X (Addon) +17.7 X (Addon) +17.8 X (Addon) +18.1 X (Cppcheck) +18.2 X (Cppcheck) +18.3 X (Cppcheck) +18.4 X (Addon) +18.5 X (Addon) +18.6 X (Cppcheck) +18.7 X (Addon) +18.8 X (Addon) +19.1 X (Cppcheck) +19.2 X (Addon) +20.1 X (Addon) +20.2 X (Addon) +20.3 X (Addon) +20.4 X (Addon) +20.5 X (Addon) +20.6 X (Cppcheck) +20.7 X (Addon) +20.8 X (Addon) +20.9 X (Addon) +20.10 X (Addon) +20.11 X (Addon) +20.12 X (Addon) +20.13 X (Addon) +20.14 X (Addon) +21.1 X (Addon) +21.2 X (Addon) +21.3 X (Addon) +21.4 X (Addon) +21.5 X (Addon) +21.6 X (Addon) +21.7 X (Addon) +21.8 X (Addon) +21.9 X (Addon) +21.10 X (Addon) +21.11 X (Addon) +21.12 X (Addon) +21.13 X (Cppcheck) +21.14 X (Addon) +21.15 X (Addon) +21.16 X (Addon) +21.17 X (Cppcheck) +21.18 X (Cppcheck) +21.19 X (Addon) +21.20 X (Addon) +21.21 X (Addon) +22.1 X (Cppcheck) +22.2 X (Cppcheck) +22.3 X (Cppcheck) +22.4 X (Cppcheck) +22.5 X (Addon) +22.6 X (Cppcheck) +22.7 X (Addon) +22.8 X (Addon) +22.9 X (Addon) +22.10 X (Addon) diff --git a/panda/tests/misra/install.sh b/panda/tests/misra/install.sh new file mode 100755 index 0000000000..e0a743b72e --- /dev/null +++ b/panda/tests/misra/install.sh @@ -0,0 +1,18 @@ +#!/bin/bash +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +: "${CPPCHECK_DIR:=$DIR/cppcheck/}" + +if [ ! -d "$CPPCHECK_DIR" ]; then + git clone https://github.com/danmar/cppcheck.git $CPPCHECK_DIR +fi + +cd $CPPCHECK_DIR + +VERS="2.14.1" +git fetch --all --tags --force +git checkout $VERS + +#make clean +make MATCHCOMPILTER=yes CXXFLAGS="-O2" -j8 diff --git a/panda/tests/misra/suppressions.txt b/panda/tests/misra/suppressions.txt new file mode 100644 index 0000000000..5c0b953b51 --- /dev/null +++ b/panda/tests/misra/suppressions.txt @@ -0,0 +1,27 @@ +# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well +misra-c2012-11.4 +# Advisory: casting from void pointer to type pointer is ok. Done by STM libraries as well +misra-c2012-11.5 +# Advisory: as stated in the Misra document, use of goto statements in accordance to 15.2 and 15.3 is ok +misra-c2012-15.1 +# Advisory: union types can be used +misra-c2012-19.2 +# Advisory: The # and ## preprocessor operators should not be used +misra-c2012-20.10 + +# needed since not all of these suppressions are applicable to all builds +unmatchedSuppression + +# All interrupt handlers are defined, including ones we don't use +unusedFunction:*/interrupt_handlers*.h + +# all of the below suppressions are from new checks introduced after updating +# cppcheck from 2.5 -> 2.13. they are listed here to separate the update from +# fixing the violations and all are intended to be removed soon after +misra-c2012-2.5 # unused macros. a few legit, rest aren't common between F4/H7 builds. should we do this in the unusedFunction pass? +misra-c2012-8.7 +misra-c2012-8.4 +misra-c2012-21.15 + +# FIXME: violations are in ST's F4 headers +misra-c2012-12.2 diff --git a/panda/tests/misra/test_misra.sh b/panda/tests/misra/test_misra.sh new file mode 100755 index 0000000000..e65f6a5e5a --- /dev/null +++ b/panda/tests/misra/test_misra.sh @@ -0,0 +1,85 @@ +#!/bin/bash +set -e + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +PANDA_DIR=$(realpath $DIR/../../) + +GREEN="\e[1;32m" +YELLOW="\e[1;33m" +RED="\e[1;31m" +NC='\033[0m' + +: "${CPPCHECK_DIR:=$DIR/cppcheck/}" + +# install cppcheck if missing +if [ -z "${SKIP_CPPCHECK_INSTALL}" ]; then + $DIR/install.sh +fi + +# ensure checked in coverage table is up to date +cd $DIR +if [ -z "$SKIP_TABLES_DIFF" ]; then + python $CPPCHECK_DIR/addons/misra.py -generate-table > coverage_table + if ! git diff --quiet coverage_table; then + echo -e "${YELLOW}MISRA coverage table doesn't match. Update and commit:${NC}" + exit 3 + fi +fi + +cd $PANDA_DIR +if [ -z "${SKIP_BUILD}" ]; then + scons -j8 +fi + +CHECKLIST=$DIR/checkers.txt +echo "Cppcheck checkers list from test_misra.sh:" > $CHECKLIST + +cppcheck() { + # get all gcc defines: arm-none-eabi-gcc -dM -E - < /dev/null + COMMON_DEFINES="-D__GNUC__=9 -UCMSIS_NVIC_VIRTUAL -UCMSIS_VECTAB_VIRTUAL" + + # note that cppcheck build cache results in inconsistent results as of v2.13.0 + OUTPUT=$DIR/.output.log + + echo -e "\n\n\n\n\nTEST variant options:" >> $CHECKLIST + echo -e ""${@//$PANDA_DIR/}"\n\n" >> $CHECKLIST # (absolute path removed) + + $CPPCHECK_DIR/cppcheck --inline-suppr -I $PANDA_DIR/board/ \ + -I "$(arm-none-eabi-gcc -print-file-name=include)" \ + -I $PANDA_DIR/board/stm32f4/inc/ -I $PANDA_DIR/board/stm32h7/inc/ \ + --suppressions-list=$DIR/suppressions.txt --suppress=*:*inc/* \ + --suppress=*:*include/* --error-exitcode=2 --check-level=exhaustive \ + --platform=arm32-wchar_t4 $COMMON_DEFINES --checkers-report=$CHECKLIST.tmp \ + --std=c11 "$@" |& tee $OUTPUT + + cat $CHECKLIST.tmp >> $CHECKLIST + rm $CHECKLIST.tmp + # cppcheck bug: some MISRA errors won't result in the error exit code, + # so check the output (https://trac.cppcheck.net/ticket/12440#no1) + if grep -e "misra violation" -e "error" -e "style: " $OUTPUT > /dev/null; then + printf "${RED}** FAILED: MISRA violations found!${NC}\n" + exit 1 + fi +} + +PANDA_OPTS="--enable=all --disable=unusedFunction -DPANDA --addon=misra" + +printf "\n${GREEN}** PANDA F4 CODE **${NC}\n" +cppcheck $PANDA_OPTS -DSTM32F4 -DSTM32F413xx $PANDA_DIR/board/main.c + +printf "\n${GREEN}** PANDA H7 CODE **${NC}\n" +cppcheck $PANDA_OPTS -DSTM32H7 -DSTM32H725xx $PANDA_DIR/board/main.c + +# unused needs to run globally +#printf "\n${GREEN}** UNUSED ALL CODE **${NC}\n" +#cppcheck --enable=unusedFunction --quiet $PANDA_DIR/board/ + +printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n" + + +# ensure list of checkers is up to date +cd $DIR +if [ -z "$SKIP_TABLES_DIFF" ] && ! git diff --quiet $CHECKLIST; then + echo -e "\n${YELLOW}WARNING: Cppcheck checkers.txt report has changed. Review and commit...${NC}" + exit 4 +fi diff --git a/panda/tests/misra/test_mutation.py b/panda/tests/misra/test_mutation.py new file mode 100755 index 0000000000..796b93dd27 --- /dev/null +++ b/panda/tests/misra/test_mutation.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import os +import glob +import pytest +import shutil +import subprocess +import tempfile +import random + +HERE = os.path.abspath(os.path.dirname(__file__)) +ROOT = os.path.join(HERE, "../../") + +IGNORED_PATHS = ( + 'board/obj', + 'board/jungle', + 'board/stm32h7/inc', + 'board/stm32f4/inc', + 'board/fake_stm.h', + + # bootstub only files + 'board/flasher.h', + 'board/bootstub.c', + 'board/bootstub_declarations.h', + 'board/stm32h7/llflash.h', + 'board/stm32f4/llflash.h', +) + +mutations = [ + # default + (None, None, False), + # F4 only + ("board/stm32f4/llbxcan.h", "s/1U/1/g", True), + # H7 only + ("board/stm32h7/llfdcan.h", "s/return ret;/if (true) { return ret; } else { return false; }/g", True), + # general safety + ("board/safety/safety_toyota.h", "s/is_lkas_msg =.*;/is_lkas_msg = addr == 1 || addr == 2;/g", True), +] + +patterns = [ + # misra-c2012-13.3 + "$a void test(int tmp) { int tmp2 = tmp++ + 2; if (tmp2) {;}}", + # misra-c2012-13.4 + "$a int test(int x, int y) { return (x=2) && (y=2); }", + # misra-c2012-13.5 + "$a void test(int tmp) { if (true && tmp++) {;} }", + # misra-c2012-13.6 + "$a void test(int tmp) { if (sizeof(tmp++)) {;} }", + # misra-c2012-14.1 + "$a void test(float len) { for (float j = 0; j < len; j++) {;} }", + # misra-c2012-14.4 + "$a void test(int len) { if (len - 8) {;} }", + # misra-c2012-16.4 + r"$a void test(int temp) {switch (temp) { case 1: ; }}\n", + # misra-c2012-17.8 + "$a void test(int cnt) { for (cnt=0;;cnt++) {;} }", + # misra-c2012-20.4 + r"$a #define auto 1\n", + # misra-c2012-20.5 + r"$a #define TEST 1\n#undef TEST\n", +] + +all_files = glob.glob('board/**', root_dir=ROOT, recursive=True) +files = [f for f in all_files if f.endswith(('.c', '.h')) and not f.startswith(IGNORED_PATHS)] +assert len(files) > 70, all(d in files for d in ('board/main.c', 'board/stm32f4/llbxcan.h', 'board/stm32h7/llfdcan.h', 'board/safety/safety_toyota.h')) + +for p in patterns: + mutations.append((random.choice(files), p, True)) + +@pytest.mark.parametrize("fn, patch, should_fail", mutations) +def test_misra_mutation(fn, patch, should_fail): + with tempfile.TemporaryDirectory() as tmp: + shutil.copytree(ROOT, tmp, dirs_exist_ok=True) + + # apply patch + if fn is not None: + r = os.system(f"cd {tmp} && sed -i '{patch}' {fn}") + assert r == 0 + + # run test + r = subprocess.run("SKIP_TABLES_DIFF=1 tests/misra/test_misra.sh", cwd=tmp, shell=True) + failed = r.returncode != 0 + assert failed == should_fail diff --git a/panda/tests/read_flash_spi.py b/panda/tests/read_flash_spi.py new file mode 100755 index 0000000000..133062be71 --- /dev/null +++ b/panda/tests/read_flash_spi.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +from panda import Panda, PandaDFU + +if __name__ == "__main__": + try: + from openpilot.system.hardware import HARDWARE + HARDWARE.recover_internal_panda() + Panda.wait_for_dfu(None, 5) + except Exception: + pass + + p = PandaDFU(None) + cfg = p.get_mcu_type().config + + def readmem(addr, length, fn): + print(f"reading {hex(addr)} {hex(length)} bytes to {fn}") + max_size = 255 + with open(fn, "wb") as f: + to_read = length + while to_read > 0: + l = min(to_read, max_size) + dat = p._handle.read(addr, l) + assert len(dat) == l + f.write(dat) + + to_read -= len(dat) + addr += len(dat) + + addr = cfg.bootstub_address + for i, sector_size in enumerate(cfg.sector_sizes): + readmem(addr, sector_size, f"sector_{i}.bin") + addr += sector_size diff --git a/panda/tests/read_st_flash.sh b/panda/tests/read_st_flash.sh new file mode 100755 index 0000000000..ffcfd7bbfe --- /dev/null +++ b/panda/tests/read_st_flash.sh @@ -0,0 +1,6 @@ +#!/bin/bash +rm -f /tmp/dump_bootstub +rm -f /tmp/dump_main +dfu-util -a 0 -s 0x08000000 -U /tmp/dump_bootstub +dfu-util -a 0 -s 0x08004000 -U /tmp/dump_main + diff --git a/panda/tests/read_winusb_descriptors.py b/panda/tests/read_winusb_descriptors.py new file mode 100755 index 0000000000..5d311c9ea9 --- /dev/null +++ b/panda/tests/read_winusb_descriptors.py @@ -0,0 +1,34 @@ +#!/usr/bin/env python3 +# type: ignore +from panda import Panda +from hexdump import hexdump + +DEBUG = False + +if __name__ == "__main__": + p = Panda() + + length = p._handle.controlRead(Panda.REQUEST_IN, 0x06, 3 << 8 | 238, 0, 1) + print('Microsoft OS String Descriptor') + dat = p._handle.controlRead(Panda.REQUEST_IN, 0x06, 3 << 8 | 238, 0, length[0]) + if DEBUG: + print(f'LEN: {hex(length[0])}') + hexdump("".join(map(chr, dat))) + + ms_vendor_code = dat[16] + if DEBUG: + print(f'MS_VENDOR_CODE: {hex(length[0])}') + + print('\nMicrosoft Compatible ID Feature Descriptor') + length = p._handle.controlRead(Panda.REQUEST_IN, ms_vendor_code, 0, 4, 1) + if DEBUG: + print(f'LEN: {hex(length[0])}') + dat = p._handle.controlRead(Panda.REQUEST_IN, ms_vendor_code, 0, 4, length[0]) + hexdump("".join(map(chr, dat))) + + print('\nMicrosoft Extended Properties Feature Descriptor') + length = p._handle.controlRead(Panda.REQUEST_IN, ms_vendor_code, 0, 5, 1) + if DEBUG: + print(f'LEN: {hex(length[0])}') + dat = p._handle.controlRead(Panda.REQUEST_IN, ms_vendor_code, 0, 5, length[0]) + hexdump("".join(map(chr, dat))) diff --git a/panda/tests/reflash_internal_panda.py b/panda/tests/reflash_internal_panda.py new file mode 100755 index 0000000000..c2ad9f8964 --- /dev/null +++ b/panda/tests/reflash_internal_panda.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +import time +from panda import Panda, PandaDFU + +class GPIO: + STM_RST_N = 124 + STM_BOOT0 = 134 + HUB_RST_N = 30 + + +def gpio_init(pin, output): + with open(f"/sys/class/gpio/gpio{pin}/direction", 'wb') as f: + f.write(b"out" if output else b"in") + +def gpio_set(pin, high): + with open(f"/sys/class/gpio/gpio{pin}/value", 'wb') as f: + f.write(b"1" if high else b"0") + + +if __name__ == "__main__": + for pin in (GPIO.STM_RST_N, GPIO.STM_BOOT0, GPIO.HUB_RST_N): + gpio_init(pin, True) + + # reset USB hub + gpio_set(GPIO.HUB_RST_N, 0) + time.sleep(0.5) + gpio_set(GPIO.HUB_RST_N, 1) + + # flash bootstub + print("resetting into DFU") + gpio_set(GPIO.STM_RST_N, 1) + gpio_set(GPIO.STM_BOOT0, 1) + time.sleep(1) + gpio_set(GPIO.STM_RST_N, 0) + gpio_set(GPIO.STM_BOOT0, 0) + time.sleep(1) + + print("flashing bootstub") + PandaDFU(None).recover() + + gpio_set(GPIO.STM_RST_N, 1) + time.sleep(0.5) + gpio_set(GPIO.STM_RST_N, 0) + time.sleep(1) + + print("flashing app") + p = Panda() + assert p.bootstub + p.flash() diff --git a/panda/tests/relay_test.py b/panda/tests/relay_test.py new file mode 100755 index 0000000000..68789b17c8 --- /dev/null +++ b/panda/tests/relay_test.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python +import time +from panda import Panda + +p = Panda() + +while True: + p.set_safety_mode(Panda.SAFETY_TOYOTA) + p.send_heartbeat() + print("ON") + time.sleep(1) + p.set_safety_mode(Panda.SAFETY_NOOUTPUT) + p.send_heartbeat() + print("OFF") + time.sleep(1) + diff --git a/panda/tests/restore_flash_spi.py b/panda/tests/restore_flash_spi.py new file mode 100755 index 0000000000..c23b298289 --- /dev/null +++ b/panda/tests/restore_flash_spi.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +from panda import Panda, PandaDFU, STBootloaderSPIHandle + +if __name__ == "__main__": + try: + from openpilot.system.hardware import HARDWARE + HARDWARE.recover_internal_panda() + Panda.wait_for_dfu(None, 5) + except Exception: + pass + + p = PandaDFU(None) + assert isinstance(p._handle, STBootloaderSPIHandle) + cfg = p.get_mcu_type().config + + print("restoring from backup...") + addr = cfg.bootstub_address + for i, sector_size in enumerate(cfg.sector_sizes): + print(f"- sector #{i}") + p._handle.erase_sector(i) + with open(f"sector_{i}.bin", "rb") as f: + dat = f.read() + assert len(dat) == sector_size + p._handle.program(addr, dat) + addr += len(dat) + + p.reset() diff --git a/panda/tests/safety_replay/.gitignore b/panda/tests/safety_replay/.gitignore new file mode 100644 index 0000000000..192fb0945e --- /dev/null +++ b/panda/tests/safety_replay/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/tools/__init__.py b/panda/tests/safety_replay/__init__.py similarity index 100% rename from tools/__init__.py rename to panda/tests/safety_replay/__init__.py diff --git a/panda/tests/safety_replay/helpers.py b/panda/tests/safety_replay/helpers.py new file mode 100644 index 0000000000..a1a6cd3a59 --- /dev/null +++ b/panda/tests/safety_replay/helpers.py @@ -0,0 +1,80 @@ +import panda.tests.libpanda.libpanda_py as libpanda_py +from panda import Panda + +def to_signed(d, bits): + ret = d + if d >= (1 << (bits - 1)): + ret = d - (1 << bits) + return ret + +def is_steering_msg(mode, param, addr): + ret = False + if mode in (Panda.SAFETY_HONDA_NIDEC, Panda.SAFETY_HONDA_BOSCH): + ret = (addr == 0xE4) or (addr == 0x194) or (addr == 0x33D) or (addr == 0x33DA) or (addr == 0x33DB) + elif mode == Panda.SAFETY_TOYOTA: + ret = addr == (0x191 if param & Panda.FLAG_TOYOTA_LTA else 0x2E4) + elif mode == Panda.SAFETY_GM: + ret = addr == 384 + elif mode == Panda.SAFETY_HYUNDAI: + ret = addr == 832 + elif mode == Panda.SAFETY_CHRYSLER: + ret = addr == 0x292 + elif mode == Panda.SAFETY_SUBARU: + ret = addr == 0x122 + elif mode == Panda.SAFETY_FORD: + ret = addr == 0x3d3 + elif mode == Panda.SAFETY_NISSAN: + ret = addr == 0x169 + return ret + +def get_steer_value(mode, param, to_send): + torque, angle = 0, 0 + if mode in (Panda.SAFETY_HONDA_NIDEC, Panda.SAFETY_HONDA_BOSCH): + torque = (to_send.data[0] << 8) | to_send.data[1] + torque = to_signed(torque, 16) + elif mode == Panda.SAFETY_TOYOTA: + if param & Panda.FLAG_TOYOTA_LTA: + angle = (to_send.data[1] << 8) | to_send.data[2] + angle = to_signed(angle, 16) + else: + torque = (to_send.data[1] << 8) | (to_send.data[2]) + torque = to_signed(torque, 16) + elif mode == Panda.SAFETY_GM: + torque = ((to_send.data[0] & 0x7) << 8) | to_send.data[1] + torque = to_signed(torque, 11) + elif mode == Panda.SAFETY_HYUNDAI: + torque = (((to_send.data[3] & 0x7) << 8) | to_send.data[2]) - 1024 + elif mode == Panda.SAFETY_CHRYSLER: + torque = (((to_send.data[0] & 0x7) << 8) | to_send.data[1]) - 1024 + elif mode == Panda.SAFETY_SUBARU: + torque = ((to_send.data[3] & 0x1F) << 8) | to_send.data[2] + torque = -to_signed(torque, 13) + elif mode == Panda.SAFETY_FORD: + angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000 + elif mode == Panda.SAFETY_NISSAN: + angle = (to_send.data[0] << 10) | (to_send.data[1] << 2) | (to_send.data[2] >> 6) + angle = -angle + (1310 * 100) + return torque, angle + +def package_can_msg(msg): + return libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) + +def init_segment(safety, lr, mode, param): + sendcan = (msg for msg in lr if msg.which() == 'sendcan') + steering_msgs = (can for msg in sendcan for can in msg.sendcan if is_steering_msg(mode, param, can.address)) + + msg = next(steering_msgs, None) + if msg is None: + # no steering msgs + return + + to_send = package_can_msg(msg) + torque, angle = get_steer_value(mode, param, to_send) + if torque != 0: + safety.set_controls_allowed(1) + safety.set_desired_torque_last(torque) + elif angle != 0: + safety.set_controls_allowed(1) + safety.set_desired_angle_last(angle) + safety.set_angle_meas(angle, angle) + assert safety.safety_tx_hook(to_send), "failed to initialize panda safety for segment" diff --git a/panda/tests/safety_replay/replay_drive.py b/panda/tests/safety_replay/replay_drive.py new file mode 100755 index 0000000000..df3e0554f6 --- /dev/null +++ b/panda/tests/safety_replay/replay_drive.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python3 +import argparse +import os +from collections import Counter + +from panda.tests.libpanda import libpanda_py +from panda.tests.safety_replay.helpers import package_can_msg, init_segment + +# replay a drive to check for safety violations +def replay_drive(lr, safety_mode, param, alternative_experience, segment=False): + safety = libpanda_py.libpanda + + err = safety.set_safety_hooks(safety_mode, param) + assert err == 0, "invalid safety mode: %d" % safety_mode + safety.set_alternative_experience(alternative_experience) + + if segment: + init_segment(safety, lr, safety_mode, param) + lr.reset() + + rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0 + safety_tick_rx_invalid = False + blocked_addrs = Counter() + invalid_addrs = set() + + can_msgs = [m for m in lr if m.which() in ('can', 'sendcan')] + start_t = can_msgs[0].logMonoTime + end_t = can_msgs[-1].logMonoTime + for msg in can_msgs: + safety.set_timer((msg.logMonoTime // 1000) % 0xFFFFFFFF) + + # skip start and end of route, warm up/down period + if msg.logMonoTime - start_t > 1e9 and end_t - msg.logMonoTime > 1e9: + safety.safety_tick_current_safety_config() + safety_tick_rx_invalid |= not safety.safety_config_valid() or safety_tick_rx_invalid + + if msg.which() == 'sendcan': + for canmsg in msg.sendcan: + to_send = package_can_msg(canmsg) + sent = safety.safety_tx_hook(to_send) + if not sent: + tx_blocked += 1 + tx_controls_blocked += safety.get_controls_allowed() + blocked_addrs[canmsg.address] += 1 + + if "DEBUG" in os.environ: + print("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9)) + tx_controls += safety.get_controls_allowed() + tx_tot += 1 + elif msg.which() == 'can': + # ignore msgs we sent + for canmsg in filter(lambda m: m.src < 128, msg.can): + to_push = package_can_msg(canmsg) + recv = safety.safety_rx_hook(to_push) + if not recv: + rx_invalid += 1 + invalid_addrs.add(canmsg.address) + rx_tot += 1 + + print("\nRX") + print("total rx msgs:", rx_tot) + print("invalid rx msgs:", rx_invalid) + print("safety tick rx invalid:", safety_tick_rx_invalid) + print("invalid addrs:", invalid_addrs) + print("\nTX") + print("total openpilot msgs:", tx_tot) + print("total msgs with controls allowed:", tx_controls) + print("blocked msgs:", tx_blocked) + print("blocked with controls allowed:", tx_controls_blocked) + print("blocked addrs:", blocked_addrs) + + return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid + +if __name__ == "__main__": + from openpilot.tools.lib.logreader import LogReader + + parser = argparse.ArgumentParser(description="Replay CAN messages from a route or segment through a safety mode", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("route_or_segment_name", nargs='+') + parser.add_argument("--mode", type=int, help="Override the safety mode from the log") + parser.add_argument("--param", type=int, help="Override the safety param from the log") + parser.add_argument("--alternative-experience", type=int, help="Override the alternative experience from the log") + args = parser.parse_args() + + lr = LogReader(args.route_or_segment_name[0]) + + if None in (args.mode, args.param, args.alternative_experience): + for msg in lr: + if msg.which() == 'carParams': + if args.mode is None: + args.mode = msg.carParams.safetyConfigs[-1].safetyModel.raw + if args.param is None: + args.param = msg.carParams.safetyConfigs[-1].safetyParam + if args.alternative_experience is None: + args.alternative_experience = msg.carParams.alternativeExperience + break + else: + raise Exception("carParams not found in log. Set safety mode and param manually.") + + lr.reset() + + print(f"replaying {args.route_or_segment_name[0]} with safety mode {args.mode}, param {args.param}, alternative experience {args.alternative_experience}") + replay_drive(lr, args.mode, args.param, args.alternative_experience, segment=len(lr.logreader_identifiers) == 1) diff --git a/panda/tests/setup_device_ci.sh b/panda/tests/setup_device_ci.sh new file mode 100755 index 0000000000..b45c6b466b --- /dev/null +++ b/panda/tests/setup_device_ci.sh @@ -0,0 +1,74 @@ +#!/usr/bin/bash + +set -e + +if [ -z "$SOURCE_DIR" ]; then + echo "SOURCE_DIR must be set" + exit 1 +fi + +if [ -z "$GIT_COMMIT" ]; then + echo "GIT_COMMIT must be set" + exit 1 +fi + +if [ -z "$TEST_DIR" ]; then + echo "TEST_DIR must be set" + exit 1 +fi + +CONTINUE_PATH="/data/continue.sh" +tee $CONTINUE_PATH << EOF +#!/usr/bin/bash + +sudo abctl --set_success + +# patch sshd config +sudo mount -o rw,remount / +sudo sed -i "s,/data/params/d/GithubSshKeys,/usr/comma/setup_keys," /etc/ssh/sshd_config +sudo systemctl daemon-reload +sudo systemctl restart ssh +sudo systemctl disable ssh-param-watcher.path +sudo systemctl disable ssh-param-watcher.service +sudo mount -o ro,remount / + +while true; do + if ! sudo systemctl is-active -q ssh; then + sudo systemctl start ssh + fi + sleep 5s +done + +sleep infinity +EOF +chmod +x $CONTINUE_PATH + + +# set up environment +if [ ! -d "$SOURCE_DIR" ]; then + git clone https://github.com/commaai/panda.git $SOURCE_DIR +fi + +# setup device/SOM state +SOM_ST_IO=49 +echo $SOM_ST_IO > /sys/class/gpio/export || true +echo out > /sys/class/gpio/gpio${SOM_ST_IO}/direction +echo 1 > /sys/class/gpio/gpio${SOM_ST_IO}/value + +# checkout panda commit +cd $SOURCE_DIR + +rm -f .git/index.lock +git reset --hard +git fetch --no-tags --no-recurse-submodules -j4 --verbose --depth 1 origin $GIT_COMMIT +find . -maxdepth 1 -not -path './.git' -not -name '.' -not -name '..' -exec rm -rf '{}' \; +git reset --hard $GIT_COMMIT +git checkout $GIT_COMMIT +git clean -xdff + +echo "git checkout done, t=$SECONDS" +du -hs $SOURCE_DIR $SOURCE_DIR/.git + +rsync -a --delete $SOURCE_DIR $TEST_DIR + +echo "$TEST_DIR synced with $GIT_COMMIT, t=$SECONDS" diff --git a/panda/tests/som/on-device.py b/panda/tests/som/on-device.py new file mode 100755 index 0000000000..421271d6a8 --- /dev/null +++ b/panda/tests/som/on-device.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import os +import time + +from panda import Panda + + +if __name__ == "__main__": + flag_set = False + while True: + try: + with Panda(disable_checks=False) as p: + if not flag_set: + p.set_heartbeat_disabled() + p.set_safety_mode(Panda.SAFETY_ELM327, 30) + flag_set = True + + # shutdown when told + ch = p.can_health(0) + if ch['can_data_speed'] == 1000: + os.system("sudo poweroff") + except Exception as e: + print(str(e)) + time.sleep(0.5) diff --git a/panda/tests/som/test_bootkick.py b/panda/tests/som/test_bootkick.py new file mode 100644 index 0000000000..324d249293 --- /dev/null +++ b/panda/tests/som/test_bootkick.py @@ -0,0 +1,153 @@ +import time +import pytest + +from panda import Panda, PandaJungle + +PANDA_SERIAL = "28002d000451323431333839" +JUNGLE_SERIAL = "26001c001451313236343430" + +OBDC_PORT = 1 + +@pytest.fixture(autouse=True, scope="function") +def pj(): + jungle = PandaJungle(JUNGLE_SERIAL) + jungle.flash() + + jungle.reset() + jungle.set_ignition(False) + + yield jungle + + #jungle.set_panda_power(False) + jungle.close() + +@pytest.fixture(scope="function") +def p(pj): + # note that the 3X's panda lib isn't updated, which + # shold be fine since it only uses stable APIs + pj.set_panda_power(True) + assert Panda.wait_for_panda(PANDA_SERIAL, 10) + p = Panda(PANDA_SERIAL) + p.flash() + p.reset() + yield p + p.close() + +def setup_state(panda, jungle, state): + jungle.set_panda_power(0) + + if state == "off": + wait_for_full_poweroff(jungle) + elif state == "normal boot": + jungle.set_panda_individual_power(OBDC_PORT, 1) + elif state == "QDL": + time.sleep(0.5) + jungle.set_panda_individual_power(OBDC_PORT, 1) + elif state == "ready to bootkick": + wait_for_full_poweroff(jungle) + jungle.set_panda_individual_power(OBDC_PORT, 1) + wait_for_boot(panda, jungle) + set_som_shutdown_flag(panda) + panda.set_safety_mode(Panda.SAFETY_SILENT) + panda.send_heartbeat() + wait_for_som_shutdown(panda, jungle) + else: + raise ValueError(f"unkown state: {state}") + + +def wait_for_som_shutdown(panda, jungle): + st = time.monotonic() + while panda.read_som_gpio(): + # can take a while for the SOM to fully shutdown + if time.monotonic() - st > 120: + raise Exception("SOM didn't shutdown in time") + if check_som_boot_flag(panda): + raise Exception(f"SOM rebooted instead of shutdown: {time.monotonic() - st}s") + time.sleep(0.5) + dt = time.monotonic() - st + print("waiting for shutdown", round(dt)) + dt = time.monotonic() - st + print(f"took {dt:.2f}s for SOM to shutdown") + +def wait_for_full_poweroff(jungle, timeout=30): + st = time.monotonic() + + time.sleep(15) + while PANDA_SERIAL in Panda.list(): + if time.monotonic() - st > timeout: + raise Exception("took too long for device to turn off") + + health = jungle.health() + assert all(health[f"ch{i}_power"] < 0.1 for i in range(1, 7)) + +def check_som_boot_flag(panda): + h = panda.health() + return h['safety_mode'] == Panda.SAFETY_ELM327 and h['safety_param'] == 30 + +def set_som_shutdown_flag(panda): + panda.set_can_data_speed_kbps(0, 1000) + +def wait_for_boot(panda, jungle, reset_expected=False, bootkick=False, timeout=120): + st = time.monotonic() + + Panda.wait_for_panda(PANDA_SERIAL, timeout) + panda.reconnect() + if bootkick: + assert panda.health()['uptime'] > 20 + else: + assert panda.health()['uptime'] < 3 + + for i in range(3): + assert not check_som_boot_flag(panda) + time.sleep(1) + + # wait for SOM to bootup + while not check_som_boot_flag(panda): + if time.monotonic() - st > timeout: + raise Exception("SOM didn't boot in time") + time.sleep(1.0) + + assert panda.health()['som_reset_triggered'] == reset_expected + +def test_cold_boot(p, pj): + setup_state(p, pj, "off") + setup_state(p, pj, "normal boot") + wait_for_boot(p, pj) + +def test_bootkick_ignition_line(p, pj): + setup_state(p, pj, "ready to bootkick") + pj.set_ignition(True) + wait_for_boot(p, pj, bootkick=True) + +@pytest.mark.skip("test isn't reliable yet") +def test_bootkick_can_ignition(p, pj): + setup_state(p, pj, "ready to bootkick") + for _ in range(10): + # Mazda ignition signal + pj.can_send(0x9E, b'\xc0\x00\x00\x00\x00\x00\x00\x00', 0) + time.sleep(0.5) + wait_for_boot(p, pj, bootkick=True) + +def test_recovery_from_qdl(p, pj): + setup_state(p, pj, "ready to bootkick") + + # put into QDL using the FORCE_USB_BOOT pin + for i in range(10): + pj.set_header_pin(i, 1) + + # try to boot + time.sleep(1) + pj.set_ignition(True) + time.sleep(3) + + # release FORCE_USB_BOOT + for i in range(10): + pj.set_header_pin(i, 0) + + # normally, this GPIO is set immediately since it's first enabled in the ABL + for i in range(17): + assert not p.read_som_gpio() + time.sleep(1) + + # should boot after 45s + wait_for_boot(p, pj, reset_expected=True, bootkick=True, timeout=120) diff --git a/panda/tests/som_debug.sh b/panda/tests/som_debug.sh new file mode 100755 index 0000000000..9bb4219713 --- /dev/null +++ b/panda/tests/som_debug.sh @@ -0,0 +1,7 @@ +#!/usr/bin/bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +cd $DIR + +PYTHONUNBUFFERED=1 NO_COLOR=1 CLAIM=1 PORT=4 ./debug_console.py diff --git a/panda/tests/spam_can.py b/panda/tests/spam_can.py new file mode 100755 index 0000000000..3154cc0463 --- /dev/null +++ b/panda/tests/spam_can.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import os +import random + +from panda import Panda + +def get_test_string(): + return b"test" + os.urandom(10) + +if __name__ == "__main__": + p = Panda() + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + print("Spamming all buses...") + while True: + at = random.randint(1, 2000) + st = get_test_string()[0:8] + bus = random.randint(0, 2) + p.can_send(at, st, bus) + # print("Sent message on bus: ", bus) diff --git a/panda/tests/standalone_test.py b/panda/tests/standalone_test.py new file mode 100755 index 0000000000..7ec5559697 --- /dev/null +++ b/panda/tests/standalone_test.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +import struct +import time + +from panda import Panda + +if __name__ == "__main__": + p = Panda() + print(p.get_serial()) + print(p.health()) + + t1 = time.time() + for _ in range(100): + p.get_serial() + t2 = time.time() + print("100 requests took %.2f ms" % ((t2 - t1) * 1000)) + + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + a = 0 + while True: + # flood + msg = b"\xaa" * 4 + struct.pack("I", a) + p.can_send(0xaa, msg, 0) + p.can_send(0xaa, msg, 1) + p.can_send(0xaa, msg, 4) + time.sleep(0.01) + + dat = p.can_recv() + if len(dat) > 0: + print(dat) + a += 1 diff --git a/panda/tests/test_rsa.c b/panda/tests/test_rsa.c new file mode 100644 index 0000000000..5c784e23a4 --- /dev/null +++ b/panda/tests/test_rsa.c @@ -0,0 +1,34 @@ +/* +gcc -DTEST_RSA test_rsa.c ../crypto/rsa.c ../crypto/sha.c && ./a.out +*/ + +#include +#include + +#define MAX_LEN 0x40000 +char buf[MAX_LEN]; + +#include "../crypto/sha.h" +#include "../crypto/rsa.h" +#include "../obj/cert.h" + +int main() { + FILE *f = fopen("../obj/panda.bin", "rb"); + int tlen = fread(buf, 1, MAX_LEN, f); + fclose(f); + printf("read %d\n", tlen); + uint32_t *_app_start = (uint32_t *)buf; + + int len = _app_start[0]; + char digest[SHA_DIGEST_SIZE]; + SHA_hash(&_app_start[1], len-4, digest); + printf("SHA hash done\n"); + + if (!RSA_verify(&rsa_key, ((void*)&_app_start[0]) + len, RSANUMBYTES, digest, SHA_DIGEST_SIZE)) { + printf("RSA fail\n"); + } else { + printf("RSA match!!!\n"); + } + + return 0; +} diff --git a/panda/tests/usbprotocol/test.sh b/panda/tests/usbprotocol/test.sh new file mode 100755 index 0000000000..8e3886da7d --- /dev/null +++ b/panda/tests/usbprotocol/test.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -e + +# Loops over all HW_TYPEs, see board/boards/board_declarations.h +for hw_type in {0..7}; do + echo "Testing HW_TYPE: $hw_type" + HW_TYPE=$hw_type python -m unittest discover . +done diff --git a/panda/tests/usbprotocol/test_comms.py b/panda/tests/usbprotocol/test_comms.py new file mode 100755 index 0000000000..8efefef7ca --- /dev/null +++ b/panda/tests/usbprotocol/test_comms.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import random +import unittest + +from panda import Panda, DLC_TO_LEN, USBPACKET_MAX_SIZE, pack_can_buffer, unpack_can_buffer +from panda.tests.libpanda import libpanda_py + +lpp = libpanda_py.libpanda + +CHUNK_SIZE = USBPACKET_MAX_SIZE +TX_QUEUES = (lpp.tx1_q, lpp.tx2_q, lpp.tx3_q) + + +def unpackage_can_msg(pkt): + dat_len = DLC_TO_LEN[pkt[0].data_len_code] + dat = bytes(pkt[0].data[0:dat_len]) + return pkt[0].addr, 0, dat, pkt[0].bus + + +def random_can_messages(n, bus=None): + msgs = [] + for _ in range(n): + if bus is None: + bus = random.randint(0, 3) + address = random.randint(1, (1 << 29) - 1) + data = bytes([random.getrandbits(8) for _ in range(DLC_TO_LEN[random.randrange(0, len(DLC_TO_LEN))])]) + msgs.append((address, 0, data, bus)) + return msgs + + +class TestPandaComms(unittest.TestCase): + def setUp(self): + lpp.comms_can_reset() + + def test_tx_queues(self): + for bus in range(len(TX_QUEUES)): + message = (0x100, 0, b"test", bus) + + can_pkt_tx = libpanda_py.make_CANPacket(message[0], message[3], message[2]) + can_pkt_rx = libpanda_py.ffi.new('CANPacket_t *') + + assert lpp.can_push(TX_QUEUES[bus], can_pkt_tx), "CAN push failed" + assert lpp.can_pop(TX_QUEUES[bus], can_pkt_rx), "CAN pop failed" + + assert unpackage_can_msg(can_pkt_rx) == message + + def test_comms_reset_rx(self): + # store some test messages in the queue + test_msg = (0x100, 0, b"test", 0) + for _ in range(100): + can_pkt_tx = libpanda_py.make_CANPacket(test_msg[0], test_msg[3], test_msg[2]) + lpp.can_push(lpp.rx_q, can_pkt_tx) + + # read a small chunk such that we have some overflow + TINY_CHUNK_SIZE = 6 + dat = libpanda_py.ffi.new(f"uint8_t[{TINY_CHUNK_SIZE}]") + rx_len = lpp.comms_can_read(dat, TINY_CHUNK_SIZE) + assert rx_len == TINY_CHUNK_SIZE, "comms_can_read returned too little data" + + _, overflow = unpack_can_buffer(bytes(dat)) + assert len(overflow) > 0, "overflow buffer should not be empty" + + # reset the comms to clear the overflow buffer on the panda side + lpp.comms_can_reset() + + # read a large chunk, which should now contain valid messages + LARGE_CHUNK_SIZE = 512 + dat = libpanda_py.ffi.new(f"uint8_t[{LARGE_CHUNK_SIZE}]") + rx_len = lpp.comms_can_read(dat, LARGE_CHUNK_SIZE) + assert rx_len == LARGE_CHUNK_SIZE, "comms_can_read returned too little data" + + msgs, _ = unpack_can_buffer(bytes(dat)) + assert len(msgs) > 0, "message buffer should not be empty" + for m in msgs: + assert m == test_msg, "message buffer should contain valid test messages" + + def test_comms_reset_tx(self): + # store some test messages in the queue + test_msg = (0x100, 0, b"test", 0) + packed = pack_can_buffer([test_msg for _ in range(100)]) + + # write a small chunk such that we have some overflow + TINY_CHUNK_SIZE = 6 + lpp.comms_can_write(packed[0][:TINY_CHUNK_SIZE], TINY_CHUNK_SIZE) + + # reset the comms to clear the overflow buffer on the panda side + lpp.comms_can_reset() + + # write a full valid chunk, which should now contain valid messages + lpp.comms_can_write(packed[1], len(packed[1])) + + # read the messages from the queue and make sure they're valid + queue_msgs = [] + pkt = libpanda_py.ffi.new('CANPacket_t *') + while lpp.can_pop(TX_QUEUES[0], pkt): + queue_msgs.append(unpackage_can_msg(pkt)) + + assert len(queue_msgs) > 0, "message buffer should not be empty" + for m in queue_msgs: + assert m == test_msg, "message buffer should contain valid test messages" + + + def test_can_send_usb(self): + lpp.set_safety_hooks(Panda.SAFETY_ALLOUTPUT, 0) + + for bus in range(3): + with self.subTest(bus=bus): + for _ in range(100): + msgs = random_can_messages(200, bus=bus) + packed = pack_can_buffer(msgs) + + # Simulate USB bulk chunks + for buf in packed: + for i in range(0, len(buf), CHUNK_SIZE): + chunk_len = min(CHUNK_SIZE, len(buf) - i) + lpp.comms_can_write(buf[i:i+chunk_len], chunk_len) + + # Check that they ended up in the right buffers + queue_msgs = [] + pkt = libpanda_py.ffi.new('CANPacket_t *') + while lpp.can_pop(TX_QUEUES[bus], pkt): + queue_msgs.append(unpackage_can_msg(pkt)) + + self.assertEqual(len(queue_msgs), len(msgs)) + self.assertEqual(queue_msgs, msgs) + + def test_can_receive_usb(self): + msgs = random_can_messages(50000) + packets = [libpanda_py.make_CANPacket(m[0], m[3], m[2]) for m in msgs] + + rx_msgs = [] + overflow_buf = b"" + while len(packets) > 0: + # Push into queue + while lpp.can_slots_empty(lpp.rx_q) > 0 and len(packets) > 0: + lpp.can_push(lpp.rx_q, packets.pop(0)) + + # Simulate USB bulk IN chunks + MAX_TRANSFER_SIZE = 16384 + dat = libpanda_py.ffi.new(f"uint8_t[{CHUNK_SIZE}]") + while True: + buf = b"" + while len(buf) < MAX_TRANSFER_SIZE: + max_size = min(CHUNK_SIZE, MAX_TRANSFER_SIZE - len(buf)) + rx_len = lpp.comms_can_read(dat, max_size) + buf += bytes(dat[0:rx_len]) + if rx_len < max_size: + break + + if len(buf) == 0: + break + unpacked_msgs, overflow_buf = unpack_can_buffer(overflow_buf + buf) + rx_msgs.extend(unpacked_msgs) + + self.assertEqual(len(rx_msgs), len(msgs)) + self.assertEqual(rx_msgs, msgs) + + +if __name__ == "__main__": + unittest.main() diff --git a/panda/tests/usbprotocol/test_pandalib.py b/panda/tests/usbprotocol/test_pandalib.py new file mode 100755 index 0000000000..c03f246f31 --- /dev/null +++ b/panda/tests/usbprotocol/test_pandalib.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +import random +import unittest + +from panda import pack_can_buffer, unpack_can_buffer, DLC_TO_LEN + +class PandaTestPackUnpack(unittest.TestCase): + def test_panda_lib_pack_unpack(self): + overflow_buf = b'' + + to_pack = [] + for _ in range(10000): + address = random.randint(1, (1 << 29) - 1) + data = bytes([random.getrandbits(8) for _ in range(DLC_TO_LEN[random.randrange(0, len(DLC_TO_LEN))])]) + to_pack.append((address, 0, data, 0)) + + packed = pack_can_buffer(to_pack) + unpacked = [] + for dat in packed: + msgs, overflow_buf = unpack_can_buffer(overflow_buf + dat) + unpacked.extend(msgs) + + self.assertEqual(unpacked, to_pack) + +if __name__ == "__main__": + unittest.main() diff --git a/pyproject.toml b/pyproject.toml index 51396ca39b..007245540f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -1,3 +1,12 @@ +[project] +name = "openpilot" +requires-python = ">= 3.11" +readme = "README.md" +license = {text = "MIT License"} + +[project.urls] +Homepage = "https://comma.ai" + [tool.pytest.ini_options] minversion = "6.0" addopts = "--ignore=openpilot/ --ignore=cereal/ --ignore=opendbc/ --ignore=panda/ --ignore=rednose_repo/ --ignore=tinygrad_repo/ --ignore=teleoprtc_repo/ -Werror --strict-config --strict-markers --durations=10 -n auto --dist=loadgroup" @@ -11,18 +20,18 @@ markers = [ ] testpaths = [ "common", - "selfdrive/athena", - "selfdrive/boardd", + "selfdrive/pandad", "selfdrive/car", "selfdrive/controls", "selfdrive/locationd", "selfdrive/monitoring", "selfdrive/navd/tests", - "selfdrive/thermald", "selfdrive/test/longitudinal_maneuvers", "selfdrive/test/process_replay/test_fuzzy.py", + "system/updated", + "system/athena", "system/camerad", - "system/hardware/tici", + "system/hardware", "system/loggerd", "system/proclogd", "system/tests", @@ -30,7 +39,8 @@ testpaths = [ "system/webrtc", "tools/lib/tests", "tools/replay", - "tools/cabana" + "tools/cabana", + "cereal/messaging/tests", ] [tool.mypy] @@ -63,6 +73,9 @@ warn_unused_ignores=true # restrict dynamic typing warn_return_any=true +# allow implicit optionals for default args +implicit_optional = true + [tool.poetry] name = "openpilot" @@ -74,42 +87,58 @@ readme = "README.md" repository = "https://github.com/commaai/openpilot" documentation = "https://docs.comma.ai" - [tool.poetry.dependencies] python = "~3.11" -aiohttp = "*" -aiortc = "*" -casadi = "==3.6.3" + +# multiple users +sounddevice = "*" # micd + soundd +pyserial = "*" # pigeond + qcomgpsd +requests = "*" # many one-off uses +sympy = "*" # rednose + friends +crcmod = "*" # cars + qcomgpsd + +# hardwared +smbus2 = "*" # configuring amp + +# core cffi = "*" -crcmod = "*" +scons = "*" +pycapnp = "*" Cython = "*" -future-fstrings = "*" # for acados -json-rpc = "*" -libusb1 = "*" numpy = "*" + +# body / webrtcd +aiohttp = "*" +aiortc = "*" +pyaudio = "*" + +# panda +libusb1 = "*" +spidev = { version = "*", platform = "linux" } + +# modeld onnx = ">=1.14.0" onnxruntime = { version = ">=1.16.3", platform = "linux", markers = "platform_machine == 'aarch64'" } onnxruntime-gpu = { version = ">=1.16.3", platform = "linux", markers = "platform_machine == 'x86_64'" } -psutil = "*" -pyaudio = "*" -pycapnp = "*" -pycryptodome = "*" -PyJWT = "*" -pyserial = "*" + +# logging pyzmq = "*" -requests = "*" -scons = "*" sentry-sdk = "*" -smbus2 = "*" -sounddevice = "*" -spidev = { version = "*", platform = "linux" } -sympy = "*" + +# athena +PyJWT = "*" +json-rpc = "*" websocket_client = "*" +# acados deps +casadi = "*" +future-fstrings = "*" + # these should be removed -markdown-it-py = "*" -timezonefinder = "*" +psutil = "*" +timezonefinder = "*" # just used for nav ETA setproctitle = "*" +pycryptodome = "*" # used in updated/casync, panda, body, and a test [tool.poetry.group.dev.dependencies] av = "*" @@ -119,14 +148,14 @@ breathe = "*" control = "*" coverage = "*" dictdiffer = "*" -ft4222 = "*" flaky = "*" hypothesis = "~6.47" inputs = "*" Jinja2 = "*" lru-dict = "*" matplotlib = "*" -metadrive-simulator = { version = "0.4.2.3", markers = "platform_machine != 'aarch64'" } # no linux/aarch64 wheels for certain dependencies +# No release for this fix https://github.com/metadriverse/metadrive/issues/632. Pinned to this commit until next release +metadrive-simulator = {git = "https://github.com/metadriverse/metadrive.git", rev ="233a3a1698be7038ec3dd050ca10b547b4b3324c", markers = "platform_machine != 'aarch64'" } # no linux/aarch64 wheels for certain dependencies mpld3 = "*" mypy = "*" myst-parser = "*" @@ -137,6 +166,7 @@ pprofile = "*" polyline = "*" pre-commit = "*" pyautogui = "*" +pytools = "<=2024.1.3" # our pinned version of pyopencl use a broken version of pytools pyopencl = "==2023.1.4" # 2024.1 is broken on arm64 pygame = "*" pywinctl = "*" @@ -148,12 +178,15 @@ pytest-subtests = "*" pytest-xdist = "*" pytest-timeout = "*" pytest-randomly = "*" +pytest-asyncio = "*" +pytest-mock = "*" +pytest-repeat = "*" +rerun-sdk = "*" ruff = "*" sphinx = "*" sphinx-rtd-theme = "*" sphinx-sitemap = "*" tabulate = "*" -tenacity = "*" types-requests = "*" types-tabulate = "*" tqdm = "*" @@ -167,11 +200,30 @@ build-backend = "poetry.core.masonry.api" # https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml [tool.ruff] -lint.select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF008", "RUF100", "A", "B", "TID251"] -lint.ignore = ["E741", "E402", "C408", "ISC003", "B027", "B024"] +indent-width = 2 +lint.select = [ + "E", "F", "W", "PIE", "C4", "ISC", "A", "B", + "NPY", # numpy + "UP", # pyupgrade + "TRY302", "TRY400", "TRY401", # try/excepts + "RUF008", "RUF100", + "TID251" +] +lint.ignore = [ + "E741", + "E402", + "C408", + "ISC003", + "B027", + "B024", + "NPY002", # new numpy random syntax is worse + "UP038", # (x, y) -> x|y for isinstance +] line-length = 160 target-version="py311" exclude = [ + "body", + "cereal", "panda", "opendbc", "rednose_repo", @@ -187,6 +239,10 @@ lint.flake8-implicit-str-concat.allow-multiline=false "system".msg = "Use openpilot.system" "third_party".msg = "Use openpilot.third_party" "tools".msg = "Use openpilot.tools" +"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!" +"unittest".msg = "Use pytest" [tool.coverage.run] concurrency = ["multiprocessing", "thread"] +[tool.ruff.format] +quote-style = "preserve" diff --git a/rednose b/rednose new file mode 120000 index 0000000000..674cfec35f --- /dev/null +++ b/rednose @@ -0,0 +1 @@ +rednose_repo/rednose \ No newline at end of file diff --git a/rednose_repo/.dockerignore b/rednose_repo/.dockerignore new file mode 100644 index 0000000000..5b2d46270d --- /dev/null +++ b/rednose_repo/.dockerignore @@ -0,0 +1 @@ +.sconsign.dblite diff --git a/rednose_repo/.editorconfig b/rednose_repo/.editorconfig new file mode 100644 index 0000000000..e0da061025 --- /dev/null +++ b/rednose_repo/.editorconfig @@ -0,0 +1,11 @@ +root = true + +[*] +end_of_line = lf +insert_final_newline = true +trim_trailing_whitespace = true + +[{*.py, *.pyx, *pxd}] +charset = utf-8 +indent_style = space +indent_size = 2 diff --git a/rednose_repo/.gitignore b/rednose_repo/.gitignore new file mode 100644 index 0000000000..bf2b50d221 --- /dev/null +++ b/rednose_repo/.gitignore @@ -0,0 +1,151 @@ +generated/ +.sconsign.dblite +*.swp +*.tmp + +# Cython intermediates +*_pyx.cpp +*_pyx.h +*_pyx_api.h +*.os + +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.a +*.o +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ diff --git a/rednose_repo/README.md b/rednose_repo/README.md new file mode 100644 index 0000000000..264958bfcd --- /dev/null +++ b/rednose_repo/README.md @@ -0,0 +1,51 @@ +## Introduction +The kalman filter framework described here is an incredibly powerful tool for any optimization problem, +but particularly for visual odometry, sensor fusion localization or SLAM. It is designed to provide very +accurate results, work online or offline, be fairly computationally efficient, be easy to design filters with in +python. + +![](examples/kinematic_kf.png) + + +## Feature walkthrough + +### Extended Kalman Filter with symbolic Jacobian computation +Most dynamic systems can be described as a Hidden Markov Process. To estimate the state of such a system with noisy +measurements one can use a Recursive Bayesian estimator. For a linear Markov Process a regular linear Kalman filter is optimal. +Unfortunately, a lot of systems are non-linear. Extended Kalman Filters can model systems by linearizing the non-linear +system at every step, this provides a close to optimal estimator when the linearization is good enough. If the linearization +introduces too much noise, one can use an Iterated Extended Kalman Filter, Unscented Kalman Filter or a Particle Filter. For +most applications those estimators are overkill. They add a lot of complexity and require a lot of additional compute. + +Conventionally Extended Kalman Filters are implemented by writing the system's dynamic equations and then manually symbolically +calculating the Jacobians for the linearization. For complex systems this is time consuming and very prone to calculation errors. +This library symbolically computes the Jacobians using sympy to simplify the system's definition and remove the possibility of introducing calculation errors. + +### Error State Kalman Filter +3D localization algorithms usually also require estimating orientation of an object in 3D. Orientation is generally represented +with euler angles or quaternions. + +Euler angles have several problems, there are multiple ways to represent the same orientation, +gimbal lock can cause the loss of a degree of freedom and lastly their behaviour is very non-linear when errors are large. +Quaternions with one strictly positive dimension don't suffer from these issues, but have another set of problems. +Quaternions need to be normalized otherwise they will grow unbounded, but this cannot be cleanly enforced in a kalman filter. +Most importantly though a quaternion has 4 dimensions, but only represents 3 degrees of freedom, so there is one redundant dimension. + +Kalman filters are designed to minimize the error of the system's state. It is possible to have a kalman filter where state and the error of the state are represented in a different space. As long as there is an error function that can compute the error based on the true state and estimated state. It is problematic to have redundant dimensions in the error of the kalman filter, but not in the state. A good compromise then, is to use the quaternion to represent the system's attitude state and use euler angles to describe the error in attitude. This library supports and defining an arbitrary error that is in a different space than the state. [Joan Solà](https://arxiv.org/abs/1711.02508) has written a comprehensive description of using ESKFs for robust 3D orientation estimation. + +### Multi-State Constraint Kalman Filter +How do you integrate feature-based visual odometry with a Kalman filter? The problem is that one cannot write an observation equation for 2D feature observations in image space for a localization kalman filter. One needs to give the feature observation a depth so it has a 3D position, then one can write an obvervation equation in the kalman filter. This is possible by tracking the feature across frames and then estimating the depth. However, the solution is not that simple, the depth estimated by tracking the feature across frames depends on the location of the camera at those frames, and thus the state of the kalman filter. This creates a positive feedback loop where the kalman filter wrongly gains confidence in it's position because the feature position updates reinforce it. + +The solution is to use an [MSCKF](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf), which this library fully supports. + +### Rauch–Tung–Striebel smoothing +When doing offline estimation with a kalman filter there can be an initialization period where states are badly estimated. +Global estimators don't suffer from this, to make our kalman filter competitive with global optimizers we can run the filter +backwards using an RTS smoother. Those combined with potentially multiple forward and backwards passes of the data should make +performance very close to global optimization. + +### Mahalanobis distance outlier rejector +A lot of measurements do not come from a Gaussian distribution and as such have outliers that do not fit the statistical model +of the Kalman filter. This can cause a lot of performance issues if not dealt with. This library allows the use of a mahalanobis +distance statistical test on the incoming measurements to deal with this. Note that good initialization is critical to prevent +good measurements from being rejected. diff --git a/rednose_repo/SConstruct b/rednose_repo/SConstruct new file mode 100644 index 0000000000..c668d1befa --- /dev/null +++ b/rednose_repo/SConstruct @@ -0,0 +1,59 @@ +import os +import subprocess +import sysconfig +import numpy as np + +arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() + +common = '' + +python_path = sysconfig.get_paths()['include'] +cpppath = [ + '#', + '#rednose', + '#rednose/examples/generated', + '/usr/lib/include', + python_path, + np.get_include(), +] + +env = Environment( + ENV=os.environ, + CC='clang', + CXX='clang++', + CCFLAGS=[ + "-g", + "-fPIC", + "-O2", + "-Werror=implicit-function-declaration", + "-Werror=incompatible-pointer-types", + "-Werror=int-conversion", + "-Werror=return-type", + "-Werror=format-extra-args", + "-Wshadow", + ], + LIBPATH=["#rednose/examples/generated"], + CFLAGS="-std=gnu11", + CXXFLAGS="-std=c++1z", + CPPPATH=cpppath, + REDNOSE_ROOT=Dir("#").abspath, + tools=["default", "cython", "rednose_filter"], +) + +# Cython build enviroment +envCython = env.Clone() +envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"] + +envCython["LIBS"] = [] +if arch == "Darwin": + envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"] +elif arch == "aarch64": + envCython["LINKFLAGS"] = ["-shared"] + envCython["LIBS"] = [os.path.basename(python_path)] +else: + envCython["LINKFLAGS"] = ["-pthread", "-shared"] + +Export('env', 'envCython', 'common') + +SConscript(['#rednose/SConscript']) +SConscript(['#examples/SConscript']) diff --git a/rednose_repo/examples/SConscript b/rednose_repo/examples/SConscript new file mode 100644 index 0000000000..3d178ade1c --- /dev/null +++ b/rednose_repo/examples/SConscript @@ -0,0 +1,19 @@ +Import('env') + +gen_dir = Dir('generated/').abspath + +env.RednoseCompileFilter( + target="live", + filter_gen_script="live_kf.py", + output_dir=gen_dir, +) +env.RednoseCompileFilter( + target="kinematic", + filter_gen_script="kinematic_kf.py", + output_dir=gen_dir, +) +env.RednoseCompileFilter( + target="compare", + filter_gen_script="test_compare.py", + output_dir=gen_dir, +) diff --git a/rednose_repo/examples/__init__.py b/rednose_repo/examples/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rednose_repo/examples/kinematic_kf.png b/rednose_repo/examples/kinematic_kf.png new file mode 100644 index 0000000000..acb0e442c0 Binary files /dev/null and b/rednose_repo/examples/kinematic_kf.png differ diff --git a/rednose_repo/examples/kinematic_kf.py b/rednose_repo/examples/kinematic_kf.py new file mode 100755 index 0000000000..29049a7b98 --- /dev/null +++ b/rednose_repo/examples/kinematic_kf.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +import sys + +import numpy as np +import sympy as sp + +from rednose.helpers.kalmanfilter import KalmanFilter + +if __name__ == '__main__': # generating sympy code + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module + + +class ObservationKind(): + UNKNOWN = 0 + NO_OBSERVATION = 1 + POSITION = 1 + + names = [ + 'Unknown', + 'No observation', + 'Position' + ] + + @classmethod + def to_string(cls, kind): + return cls.names[kind] + + +class States(): + POSITION = slice(0, 1) + VELOCITY = slice(1, 2) + + +class KinematicKalman(KalmanFilter): + name = 'kinematic' + + initial_x = np.array([0.5, 0.0]) + + # state covariance + initial_P_diag = np.array([1.0**2, 1.0**2]) + + # process noise + Q = np.diag([0.1**2, 2.0**2]) + + obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)} + + @staticmethod + def generate_code(generated_dir): + name = KinematicKalman.name + dim_state = KinematicKalman.initial_x.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + position = state[States.POSITION, :][0,:] + velocity = state[States.VELOCITY, :][0,:] + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.POSITION.start, 0] = velocity + f_sym = state + dt * state_dot + + obs_eqs = [ + [sp.Matrix([position]), ObservationKind.POSITION, None], + ] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state) + + def __init__(self, generated_dir): + dim_state = self.initial_x.shape[0] + dim_state_err = self.initial_P_diag.shape[0] + + # init filter + self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + KinematicKalman.generate_code(generated_dir) diff --git a/rednose_repo/examples/live_kf.py b/rednose_repo/examples/live_kf.py new file mode 100755 index 0000000000..6e3c21d93e --- /dev/null +++ b/rednose_repo/examples/live_kf.py @@ -0,0 +1,342 @@ +#!/usr/bin/env python3 +import sys +import numpy as np + +from rednose.helpers import KalmanError + +if __name__ == '__main__': # Generating sympy + import sympy as sp + from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module + +EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth) + + +class ObservationKind(): + UNKNOWN = 0 + NO_OBSERVATION = 1 + GPS_NED = 2 + ODOMETRIC_SPEED = 3 + PHONE_GYRO = 4 + GPS_VEL = 5 + PSEUDORANGE_GPS = 6 + PSEUDORANGE_RATE_GPS = 7 + SPEED = 8 + NO_ROT = 9 + PHONE_ACCEL = 10 + ORB_POINT = 11 + ECEF_POS = 12 + CAMERA_ODO_TRANSLATION = 13 + CAMERA_ODO_ROTATION = 14 + ORB_FEATURES = 15 + MSCKF_TEST = 16 + FEATURE_TRACK_TEST = 17 + LANE_PT = 18 + IMU_FRAME = 19 + PSEUDORANGE_GLONASS = 20 + PSEUDORANGE_RATE_GLONASS = 21 + PSEUDORANGE = 22 + PSEUDORANGE_RATE = 23 + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + ] + + @classmethod + def to_string(cls, kind): + return cls.names[kind] + + +class States(): + ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters + ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef + ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s + ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s + GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases + ODO_SCALE = slice(16, 17) # odometer scale + ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2 + IMU_OFFSET = slice(20, 23) # imu offset angles in radians + + # Error-state has different slices because it is an ESKF + ECEF_POS_ERR = slice(0, 3) + ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error + ECEF_VELOCITY_ERR = slice(6, 9) + ANGULAR_VELOCITY_ERR = slice(9, 12) + GYRO_BIAS_ERR = slice(12, 15) + ODO_SCALE_ERR = slice(15, 16) + ACCELERATION_ERR = slice(16, 19) + IMU_OFFSET_ERR = slice(19, 22) + + +class LiveKalman(): + name = 'live' + + initial_x = np.array([-2.7e6, 4.2e6, 3.8e6, + 1, 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, + 0, 0, 0, + 0, 0, 0]) + + # state covariance + initial_P_diag = np.array([10000**2, 10000**2, 10000**2, + 10**2, 10**2, 10**2, + 10**2, 10**2, 10**2, + 1**2, 1**2, 1**2, + 0.05**2, 0.05**2, 0.05**2, + 0.02**2, + 1**2, 1**2, 1**2, + (0.01)**2, (0.01)**2, (0.01)**2]) + + # process noise + Q = np.diag([0.03**2, 0.03**2, 0.03**2, + 0.0**2, 0.0**2, 0.0**2, + 0.0**2, 0.0**2, 0.0**2, + 0.1**2, 0.1**2, 0.1**2, + (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2, + (0.02 / 100)**2, + 3**2, 3**2, 3**2, + (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2]) + + @staticmethod + def generate_code(generated_dir): + name = LiveKalman.name + dim_state = LiveKalman.initial_x.shape[0] + dim_state_err = LiveKalman.initial_P_diag.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + x, y, z = state[States.ECEF_POS, :] + q = state[States.ECEF_ORIENTATION, :] + v = state[States.ECEF_VELOCITY, :] + vx, vy, vz = v + omega = state[States.ANGULAR_VELOCITY, :] + vroll, vpitch, vyaw = omega + roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :] + odo_scale = state[States.ODO_SCALE, :][0,:] + acceleration = state[States.ACCELERATION, :] + imu_angles = state[States.IMU_OFFSET, :] + + dt = sp.Symbol('dt') + + # calibration and attitude rotation matrices + quat_rot = quat_rotate(*q) + + # Got the quat predict equations from here + # A New Quaternion-Based Kalman Filter for + # Real-Time Attitude Estimation Using the Two-Step + # Geometrically-Intuitive Correction Algorithm + A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw], + [vroll, 0, vyaw, -vpitch], + [vpitch, -vyaw, 0, vroll], + [vyaw, vpitch, -vroll, 0]]) + q_dot = A * q + + # Time derivative of the state as a function of state + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.ECEF_POS, :] = v + state_dot[States.ECEF_ORIENTATION, :] = q_dot + state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration + + # Basic descretization, 1st order intergrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1) + state_err = sp.Matrix(state_err_sym) + quat_err = state_err[States.ECEF_ORIENTATION_ERR, :] + v_err = state_err[States.ECEF_VELOCITY_ERR, :] + omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :] + acceleration_err = state_err[States.ACCELERATION_ERR, :] + + # Time derivative of the state error as a function of state error and state + quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2]) + q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err) + state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1))) + state_err_dot[States.ECEF_POS_ERR, :] = v_err + state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot + state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err) + f_err_sym = state_err + dt * state_err_dot + + # Observation matrix modifier + H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) + H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start) + H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:] + H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop) + + # these error functions are defined so that say there + # is a nominal x and true x: + # true x = err_function(nominal x, delta x) + # delta x = inv_err_function(nominal x, true x) + nom_x = sp.MatrixSymbol('nom_x', dim_state, 1) + true_x = sp.MatrixSymbol('true_x', dim_state, 1) + delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) + + err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) + delta_quat = sp.Matrix(np.ones(4)) + delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :]) + err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :]) + err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat + err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :]) + + inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) + inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0]) + delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0] + inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:]) + inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0]) + + eskf_params = [[err_function_sym, nom_x, delta_x], + [inv_err_function_sym, nom_x, true_x], + H_mod_sym, f_err_sym, state_err_sym] + # + # Observation functions + # + imu_rot = euler_rotate(*imu_angles) + h_gyro_sym = imu_rot * sp.Matrix([vroll + roll_bias, + vpitch + pitch_bias, + vyaw + yaw_bias]) + + pos = sp.Matrix([x, y, z]) + gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos) + h_acc_sym = imu_rot * (gravity + acceleration) + h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw]) + + speed = sp.sqrt(vx**2 + vy**2 + vz**2) + h_speed_sym = sp.Matrix([speed * odo_scale]) + + h_pos_sym = sp.Matrix([x, y, z]) + h_imu_frame_sym = sp.Matrix(imu_angles) + + h_relative_motion = sp.Matrix(quat_rot.T * v) + + obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None], + [h_gyro_sym, ObservationKind.PHONE_GYRO, None], + [h_phone_rot_sym, ObservationKind.NO_ROT, None], + [h_acc_sym, ObservationKind.PHONE_ACCEL, None], + [h_pos_sym, ObservationKind.ECEF_POS, None], + [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None], + [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None], + [h_imu_frame_sym, ObservationKind.IMU_FRAME, None]] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params) + + def __init__(self, generated_dir): + self.dim_state = self.initial_x.shape[0] + self.dim_state_err = self.initial_P_diag.shape[0] + + self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2), + ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]), + ObservationKind.NO_ROT: np.diag([0.00025**2, 0.00025**2, 0.00025**2]), + ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])} + + # init filter + self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err) + + @property + def x(self): + return self.filter.state() + + @property + def t(self): + return self.filter.filter_time + + @property + def P(self): + return self.filter.covs() + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=True) + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + if kind == ObservationKind.CAMERA_ODO_TRANSLATION: + r = self.predict_and_update_odo_trans(data, t, kind) + elif kind == ObservationKind.CAMERA_ODO_ROTATION: + r = self.predict_and_update_odo_rot(data, t, kind) + elif kind == ObservationKind.ODOMETRIC_SPEED: + r = self.predict_and_update_odo_speed(data, t, kind) + else: + r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + # Normalize quats + quat_norm = np.linalg.norm(self.filter.x[3:7, 0]) + + # Should not continue if the quats behave this weirdly + if not (0.1 < quat_norm < 10): + raise KalmanError("Kalman filter quaternions unstable") + + self.filter.x[States.ECEF_ORIENTATION, 0] = self.filter.x[States.ECEF_ORIENTATION, 0] / quat_norm + + return r + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + def predict_and_update_odo_speed(self, speed, t, kind): + z = np.array(speed) + R = np.zeros((len(speed), 1, 1)) + for i, _ in enumerate(z): + R[i, :, :] = np.diag([0.2**2]) + return self.filter.predict_and_update_batch(t, kind, z, R) + + def predict_and_update_odo_trans(self, trans, t, kind): + z = trans[:, :3] + R = np.zeros((len(trans), 3, 3)) + for i, _ in enumerate(z): + R[i, :, :] = np.diag(trans[i, 3:]**2) + return self.filter.predict_and_update_batch(t, kind, z, R) + + def predict_and_update_odo_rot(self, rot, t, kind): + z = rot[:, :3] + R = np.zeros((len(rot), 3, 3)) + for i, _ in enumerate(z): + R[i, :, :] = np.diag(rot[i, 3:]**2) + return self.filter.predict_and_update_batch(t, kind, z, R) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + LiveKalman.generate_code(generated_dir) diff --git a/rednose_repo/examples/test_compare.py b/rednose_repo/examples/test_compare.py new file mode 100755 index 0000000000..0b2cdd3e6a --- /dev/null +++ b/rednose_repo/examples/test_compare.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +import os +import sys +import sympy as sp +import numpy as np +import unittest + +if __name__ == '__main__': # generating sympy code + from rednose.helpers.ekf_sym import gen_code +else: + from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx # pylint: disable=no-name-in-module + from rednose.helpers.ekf_sym import EKF_sym as EKF_sym2 + + +GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) + + +class ObservationKind: + UNKNOWN = 0 + NO_OBSERVATION = 1 + POSITION = 1 + + names = [ + 'Unknown', + 'No observation', + 'Position' + ] + + @classmethod + def to_string(cls, kind): + return cls.names[kind] + + +class States: + POSITION = slice(0, 1) + VELOCITY = slice(1, 2) + + +class CompareFilter: + name = "compare" + + initial_x = np.array([0.5, 0.0]) + initial_P_diag = np.array([1.0**2, 1.0**2]) + Q = np.diag([0.1**2, 2.0**2]) + obs_noise = {ObservationKind.POSITION: np.atleast_2d(0.1**2)} + + @staticmethod + def generate_code(generated_dir): + name = CompareFilter.name + dim_state = CompareFilter.initial_x.shape[0] + + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + position = state[States.POSITION, :][0,:] + velocity = state[States.VELOCITY, :][0,:] + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.POSITION.start, 0] = velocity + f_sym = state + dt * state_dot + + obs_eqs = [ + [sp.Matrix([position]), ObservationKind.POSITION, None], + ] + + gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state) + + def __init__(self, generated_dir): + dim_state = self.initial_x.shape[0] + dim_state_err = self.initial_P_diag.shape[0] + + # init filter + self.filter_py = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) + self.filter_pyx = EKF_sym2(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), dim_state, dim_state_err) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + +class TestCompare(unittest.TestCase): + def test_compare(self): + np.random.seed(0) + + kf = CompareFilter(GENERATED_DIR) + + # Simple simulation + dt = 0.01 + ts = np.arange(0, 5, step=dt) + xs = np.empty(ts.shape) + + # Simulate + x = 0.0 + for i, v in enumerate(np.sin(ts * 5)): + xs[i] = x + x += v * dt + + # insert late observation + switch = (20, 40) + ts[switch[0]], ts[switch[1]] = ts[switch[1]], ts[switch[0]] + xs[switch[0]], xs[switch[1]] = xs[switch[1]], xs[switch[0]] + + for t, x in zip(ts, xs): + # get measurement + meas = np.random.normal(x, 0.1) + z = np.array([[meas]]) + R = kf.get_R(ObservationKind.POSITION, 1) + + # Update kf + kf.filter_py.predict_and_update_batch(t, ObservationKind.POSITION, z, R) + kf.filter_pyx.predict_and_update_batch(t, ObservationKind.POSITION, z, R) + + self.assertAlmostEqual(kf.filter_py.get_filter_time(), kf.filter_pyx.get_filter_time()) + self.assertTrue(np.allclose(kf.filter_py.state(), kf.filter_pyx.state())) + self.assertTrue(np.allclose(kf.filter_py.covs(), kf.filter_pyx.covs())) + + +if __name__ == "__main__": + generated_dir = sys.argv[2] + CompareFilter.generate_code(generated_dir) diff --git a/rednose_repo/examples/test_kinematic_kf.py b/rednose_repo/examples/test_kinematic_kf.py new file mode 100644 index 0000000000..749eaf04f4 --- /dev/null +++ b/rednose_repo/examples/test_kinematic_kf.py @@ -0,0 +1,86 @@ +import os +import numpy as np +import unittest + +from kinematic_kf import KinematicKalman, ObservationKind, States # pylint: disable=import-error + +GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated')) + +class TestKinematic(unittest.TestCase): + def test_kinematic_kf(self): + np.random.seed(0) + + kf = KinematicKalman(GENERATED_DIR) + + # Simple simulation + dt = 0.01 + ts = np.arange(0, 5, step=dt) + vs = np.sin(ts * 5) + + x = 0.0 + xs = [] + + xs_meas = [] + + xs_kf = [] + vs_kf = [] + + xs_kf_std = [] + vs_kf_std = [] + + for t, v in zip(ts, vs): + xs.append(x) + + # Update kf + meas = np.random.normal(x, 0.1) + xs_meas.append(meas) + kf.predict_and_observe(t, ObservationKind.POSITION, [meas]) + + # Retrieve kf values + state = kf.x + xs_kf.append(float(state[States.POSITION].item())) + vs_kf.append(float(state[States.VELOCITY].item())) + std = np.sqrt(kf.P) + xs_kf_std.append(float(std[States.POSITION, States.POSITION].item())) + vs_kf_std.append(float(std[States.VELOCITY, States.VELOCITY].item())) + + # Update simulation + x += v * dt + + xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std = (np.asarray(a) for a in (xs, xs_meas, xs_kf, vs_kf, xs_kf_std, vs_kf_std)) + + self.assertAlmostEqual(xs_kf[-1], -0.010866289677966417) + self.assertAlmostEqual(xs_kf_std[-1], 0.04477103863330089) + self.assertAlmostEqual(vs_kf[-1], -0.8553720537261753) + self.assertAlmostEqual(vs_kf_std[-1], 0.6695762270974388) + + if "PLOT" in os.environ: + import matplotlib.pyplot as plt # pylint: disable=import-error + plt.figure() + plt.subplot(2, 1, 1) + plt.plot(ts, xs, 'k', label='Simulation') + plt.plot(ts, xs_meas, 'k.', label='Measurements') + plt.plot(ts, xs_kf, label='KF') + ax = plt.gca() + ax.fill_between(ts, xs_kf - xs_kf_std, xs_kf + xs_kf_std, alpha=.2, color='C0') + + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.legend() + + plt.subplot(2, 1, 2) + plt.plot(ts, vs, 'k', label='Simulation') + plt.plot(ts, vs_kf, label='KF') + + ax = plt.gca() + ax.fill_between(ts, vs_kf - vs_kf_std, vs_kf + vs_kf_std, alpha=.2, color='C0') + + plt.xlabel("Time [s]") + plt.ylabel("Velocity [m/s]") + plt.legend() + + plt.show() + + +if __name__ == "__main__": + unittest.main() diff --git a/rednose_repo/pyproject.toml b/rednose_repo/pyproject.toml new file mode 100644 index 0000000000..b417c708cf --- /dev/null +++ b/rednose_repo/pyproject.toml @@ -0,0 +1,9 @@ +# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml +[tool.ruff] +line-length = 160 +target-version="py311" + +[tool.ruff.lint] +select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"] +ignore = ["W292", "E741", "E402", "C408", "ISC003"] +flake8-implicit-str-concat.allow-multiline=false diff --git a/rednose/.gitignore b/rednose_repo/rednose/.gitignore similarity index 100% rename from rednose/.gitignore rename to rednose_repo/rednose/.gitignore diff --git a/rednose/SConscript b/rednose_repo/rednose/SConscript similarity index 100% rename from rednose/SConscript rename to rednose_repo/rednose/SConscript diff --git a/rednose_repo/rednose/__init__.py b/rednose_repo/rednose/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rednose/helpers/__init__.py b/rednose_repo/rednose/helpers/__init__.py similarity index 100% rename from rednose/helpers/__init__.py rename to rednose_repo/rednose/helpers/__init__.py diff --git a/rednose/helpers/chi2_lookup.py b/rednose_repo/rednose/helpers/chi2_lookup.py similarity index 100% rename from rednose/helpers/chi2_lookup.py rename to rednose_repo/rednose/helpers/chi2_lookup.py diff --git a/rednose/helpers/chi2_lookup_table.npy b/rednose_repo/rednose/helpers/chi2_lookup_table.npy similarity index 100% rename from rednose/helpers/chi2_lookup_table.npy rename to rednose_repo/rednose/helpers/chi2_lookup_table.npy diff --git a/rednose/helpers/ekf.h b/rednose_repo/rednose/helpers/ekf.h similarity index 100% rename from rednose/helpers/ekf.h rename to rednose_repo/rednose/helpers/ekf.h diff --git a/rednose/helpers/ekf_load.cc b/rednose_repo/rednose/helpers/ekf_load.cc similarity index 100% rename from rednose/helpers/ekf_load.cc rename to rednose_repo/rednose/helpers/ekf_load.cc diff --git a/rednose/helpers/ekf_load.h b/rednose_repo/rednose/helpers/ekf_load.h similarity index 100% rename from rednose/helpers/ekf_load.h rename to rednose_repo/rednose/helpers/ekf_load.h diff --git a/rednose/helpers/ekf_sym.cc b/rednose_repo/rednose/helpers/ekf_sym.cc similarity index 100% rename from rednose/helpers/ekf_sym.cc rename to rednose_repo/rednose/helpers/ekf_sym.cc diff --git a/rednose/helpers/ekf_sym.h b/rednose_repo/rednose/helpers/ekf_sym.h similarity index 100% rename from rednose/helpers/ekf_sym.h rename to rednose_repo/rednose/helpers/ekf_sym.h diff --git a/rednose/helpers/ekf_sym.py b/rednose_repo/rednose/helpers/ekf_sym.py similarity index 99% rename from rednose/helpers/ekf_sym.py rename to rednose_repo/rednose/helpers/ekf_sym.py index 8f0425cb00..e8bd6420f6 100644 --- a/rednose/helpers/ekf_sym.py +++ b/rednose_repo/rednose/helpers/ekf_sym.py @@ -464,7 +464,7 @@ class EKF_sym(): # rewind if self.filter_time is not None and t < self.filter_time: if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] - self.max_rewind_age: - self.logger.error("observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)) + self.logger.error(f"observation too old at {t:.3f} with filter at {self.filter_time:.3f}, ignoring") return None rewound = self.rewind(t) else: diff --git a/rednose/helpers/ekf_sym_pyx.pyx b/rednose_repo/rednose/helpers/ekf_sym_pyx.pyx similarity index 100% rename from rednose/helpers/ekf_sym_pyx.pyx rename to rednose_repo/rednose/helpers/ekf_sym_pyx.pyx diff --git a/rednose/helpers/kalmanfilter.py b/rednose_repo/rednose/helpers/kalmanfilter.py similarity index 92% rename from rednose/helpers/kalmanfilter.py rename to rednose_repo/rednose/helpers/kalmanfilter.py index d541faa34c..fc9fd2bf3b 100644 --- a/rednose/helpers/kalmanfilter.py +++ b/rednose_repo/rednose/helpers/kalmanfilter.py @@ -1,4 +1,4 @@ -from typing import Any, Dict +from typing import Any import numpy as np @@ -8,10 +8,10 @@ class KalmanFilter: initial_x = np.zeros((0, 0)) initial_P_diag = np.zeros((0, 0)) Q = np.zeros((0, 0)) - obs_noise: Dict[int, Any] = {} + obs_noise: dict[int, Any] = {} # Should be initialized when initializating a KalmanFilter implementation - filter = None # noqa: A003 + filter = None @property def x(self): diff --git a/rednose/helpers/sympy_helpers.py b/rednose_repo/rednose/helpers/sympy_helpers.py similarity index 100% rename from rednose/helpers/sympy_helpers.py rename to rednose_repo/rednose/helpers/sympy_helpers.py diff --git a/rednose/logger/logger.h b/rednose_repo/rednose/logger/logger.h similarity index 100% rename from rednose/logger/logger.h rename to rednose_repo/rednose/logger/logger.h diff --git a/rednose/templates/compute_pos.c b/rednose_repo/rednose/templates/compute_pos.c similarity index 100% rename from rednose/templates/compute_pos.c rename to rednose_repo/rednose/templates/compute_pos.c diff --git a/rednose/templates/ekf_c.c b/rednose_repo/rednose/templates/ekf_c.c similarity index 100% rename from rednose/templates/ekf_c.c rename to rednose_repo/rednose/templates/ekf_c.c diff --git a/rednose/templates/feature_handler.c b/rednose_repo/rednose/templates/feature_handler.c similarity index 100% rename from rednose/templates/feature_handler.c rename to rednose_repo/rednose/templates/feature_handler.c diff --git a/rednose_repo/requirements.txt b/rednose_repo/requirements.txt new file mode 100644 index 0000000000..453554b06d --- /dev/null +++ b/rednose_repo/requirements.txt @@ -0,0 +1,9 @@ +ruff +sympy +numpy +scipy +tqdm +cffi +scons +pre-commit +Cython diff --git a/rednose_repo/setup.py b/rednose_repo/setup.py new file mode 100644 index 0000000000..492b58f1e0 --- /dev/null +++ b/rednose_repo/setup.py @@ -0,0 +1,26 @@ +import os +from setuptools import setup, find_packages + +here = os.path.abspath(os.path.dirname(__file__)) + +setup( + name='rednose', + version='0.0.1', + url='https://github.com/commaai/rednose', + author='comma.ai', + author_email='harald@comma.ai', + packages=find_packages(), + platforms='any', + license='MIT', + package_data={'': ['helpers/chi2_lookup_table.npy', 'templates/*']}, + install_requires=[ + 'sympy', + 'numpy', + 'scipy', + 'tqdm', + 'cffi', + ], + ext_modules=[], + description="Kalman filter library", + long_description='See https://github.com/commaai/rednose', +) diff --git a/rednose_repo/site_scons/site_tools/cython.py b/rednose_repo/site_scons/site_tools/cython.py new file mode 100644 index 0000000000..c291475533 --- /dev/null +++ b/rednose_repo/site_scons/site_tools/cython.py @@ -0,0 +1,72 @@ +import re +import SCons +from SCons.Action import Action +from SCons.Scanner import Scanner + +pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M) +pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M) +cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M) + + +def pyx_scan(node, env, path, arg=None): + contents = node.get_text_contents() + + # from cimport ... + matches = pyx_from_import_re.findall(contents) + # cimport + matches += pyx_import_re.findall(contents) + + # Modules can be either .pxd or .pyx files + files = [m.replace('.', '/') + '.pxd' for m in matches] + files += [m.replace('.', '/') + '.pyx' for m in matches] + + # cdef extern from + files += cdef_import_re.findall(contents) + + # Handle relative imports + cur_dir = str(node.get_dir()) + files = [cur_dir + f if f.startswith('/') else f for f in files] + + # Filter out non-existing files (probably system imports) + files = [f for f in files if env.File(f).exists()] + return env.File(files) + + +pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True) +cythonAction = Action("$CYTHONCOM") + + +def create_builder(env): + try: + cython = env['BUILDERS']['Cython'] + except KeyError: + cython = SCons.Builder.Builder( + action=cythonAction, + emitter={}, + suffix=cython_suffix_emitter, + single_source=1 + ) + env.Append(SCANNERS=pyxscanner) + env['BUILDERS']['Cython'] = cython + return cython + +def cython_suffix_emitter(env, source): + return "$CYTHONCFILESUFFIX" + +def generate(env): + env["CYTHON"] = "cythonize" + env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE" + env["CYTHONCFILESUFFIX"] = ".cpp" + + c_file, _ = SCons.Tool.createCFileBuilders(env) + + c_file.suffix['.pyx'] = cython_suffix_emitter + c_file.add_action('.pyx', cythonAction) + + c_file.suffix['.py'] = cython_suffix_emitter + c_file.add_action('.py', cythonAction) + + create_builder(env) + +def exists(env): + return True diff --git a/release/build_devel.sh b/release/build_devel.sh index 8b6816e423..7fce11ca72 100755 --- a/release/build_devel.sh +++ b/release/build_devel.sh @@ -1,5 +1,4 @@ #!/usr/bin/bash - set -ex DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" @@ -44,7 +43,7 @@ git clean -xdff # do the files copy echo "[-] copying files T=$SECONDS" cd $SOURCE_DIR -cp -pR --parents $(cat release/files_*) $TARGET_DIR/ +cp -pR --parents $(./release/release_files.py) $TARGET_DIR/ # in the directory cd $TARGET_DIR @@ -64,6 +63,13 @@ date: $DATETIME master commit: $GIT_HASH " +# should be no submodules or LFS files +git submodule status +if [ ! -z "$(git lfs ls-files)" ]; then + echo "LFS files detected!" + exit 1 +fi + # ensure files are within GitHub's limit BIG_FILES="$(find . -type f -not -path './.git/*' -size +95M)" if [ ! -z "$BIG_FILES" ]; then diff --git a/release/build_release.sh b/release/build_release.sh index fc15cf6cdf..a9e00d583d 100755 --- a/release/build_release.sh +++ b/release/build_release.sh @@ -1,4 +1,6 @@ -#!/usr/bin/bash -e +#!/usr/bin/bash +set -e +set -x # git diff --name-status origin/release3-staging | grep "^A" | less @@ -9,13 +11,6 @@ cd $DIR BUILD_DIR=/data/openpilot SOURCE_DIR="$(git rev-parse --show-toplevel)" -if [ -f /TICI ]; then - FILES_SRC="release/files_tici" -else - echo "no release files set" - exit 1 -fi - if [ -z "$RELEASE_BRANCH" ]; then echo "RELEASE_BRANCH is not set" exit 1 @@ -36,8 +31,7 @@ git checkout --orphan $RELEASE_BRANCH # do the files copy echo "[-] copying files T=$SECONDS" cd $SOURCE_DIR -cp -pR --parents $(cat release/files_common) $BUILD_DIR/ -cp -pR --parents $(cat $FILES_SRC) $BUILD_DIR/ +cp -pR --parents $(./release/release_files.py) $BUILD_DIR/ # in the directory cd $BUILD_DIR @@ -54,7 +48,7 @@ git commit -a -m "openpilot v$VERSION release" # Build export PYTHONPATH="$BUILD_DIR" -scons -j$(nproc) +scons -j$(nproc) --minimal # release panda fw CERT=/data/pandaextra/certs/release RELEASE=1 scons -j$(nproc) panda/ @@ -77,6 +71,10 @@ find . -name '__pycache__' -delete rm -rf .sconsign.dblite Jenkinsfile release/ rm selfdrive/modeld/models/supercombo.onnx +find third_party/ -name '*x86*' -exec rm -r {} + +find third_party/ -name '*Darwin*' -exec rm -r {} + + + # Restore third_party git checkout third_party/ @@ -92,9 +90,9 @@ TEST_FILES="tools/" cd $SOURCE_DIR cp -pR -n --parents $TEST_FILES $BUILD_DIR/ cd $BUILD_DIR -RELEASE=1 selfdrive/test/test_onroad.py -#selfdrive/manager/test/test_manager.py -selfdrive/car/tests/test_car_interfaces.py +RELEASE=1 pytest -n0 -s selfdrive/test/test_onroad.py +#system/manager/test/test_manager.py +#pytest selfdrive/car/tests/test_car_interfaces.py rm -rf $TEST_FILES if [ ! -z "$RELEASE_BRANCH" ]; then diff --git a/release/check-submodules.sh b/release/check-submodules.sh index 5f4e307e49..bff8d7a28f 100755 --- a/release/check-submodules.sh +++ b/release/check-submodules.sh @@ -1,7 +1,7 @@ #!/bin/bash while read hash submodule ref; do - git -C $submodule fetch --depth 1000 origin master + git -C $submodule fetch --depth 2000 origin master git -C $submodule branch -r --contains $hash | grep "origin/master" if [ "$?" -eq 0 ]; then echo "$submodule ok" diff --git a/release/files_common b/release/files_common deleted file mode 100644 index 1158d2c552..0000000000 --- a/release/files_common +++ /dev/null @@ -1,559 +0,0 @@ -.gitignore -LICENSE -launch_env.sh -launch_chffrplus.sh -launch_openpilot.sh - -Jenkinsfile -SConstruct -pyproject.toml - -README.md -RELEASES.md -docs/CARS.md -docs/CONTRIBUTING.md -docs/INTEGRATION.md -docs/LIMITATIONS.md -site_scons/site_tools/cython.py - -openpilot/__init__.py -openpilot/** - -common/.gitignore -common/__init__.py -common/*.py -common/*.pyx - -common/transformations/__init__.py -common/transformations/camera.py -common/transformations/model.py - -common/transformations/SConscript -common/transformations/coordinates.py -common/transformations/coordinates.cc -common/transformations/coordinates.hpp -common/transformations/orientation.py -common/transformations/orientation.cc -common/transformations/orientation.hpp -common/transformations/transformations.pxd -common/transformations/transformations.pyx - -common/api/__init__.py - -release/* - -tools/__init__.py -tools/lib/* -tools/bodyteleop/* -tools/joystick/* -tools/replay/*.cc -tools/replay/*.h - -selfdrive/__init__.py -selfdrive/sentry.py -selfdrive/tombstoned.py -selfdrive/updated.py -selfdrive/statsd.py - -system/logmessaged.py -system/micd.py -system/version.py - -selfdrive/athena/__init__.py -selfdrive/athena/athenad.py -selfdrive/athena/manage_athenad.py -selfdrive/athena/registration.py - -selfdrive/boardd/.gitignore -selfdrive/boardd/SConscript -selfdrive/boardd/__init__.py -selfdrive/boardd/boardd.cc -selfdrive/boardd/boardd.h -selfdrive/boardd/main.cc -selfdrive/boardd/boardd.py -selfdrive/boardd/boardd_api_impl.pyx -selfdrive/boardd/can_list_to_can_capnp.cc -selfdrive/boardd/panda.cc -selfdrive/boardd/panda.h -selfdrive/boardd/spi.cc -selfdrive/boardd/panda_comms.h -selfdrive/boardd/panda_comms.cc -selfdrive/boardd/set_time.py -selfdrive/boardd/pandad.py -selfdrive/boardd/tests/test_boardd_loopback.py - -selfdrive/car/__init__.py -selfdrive/car/docs_definitions.py -selfdrive/car/car_helpers.py -selfdrive/car/fingerprints.py -selfdrive/car/interfaces.py -selfdrive/car/vin.py -selfdrive/car/disable_ecu.py -selfdrive/car/fw_versions.py -selfdrive/car/fw_query_definitions.py -selfdrive/car/ecu_addrs.py -selfdrive/car/isotp_parallel_query.py -selfdrive/car/tests/__init__.py -selfdrive/car/tests/test_car_interfaces.py -selfdrive/car/torque_data/* - -selfdrive/car/body/*.py -selfdrive/car/chrysler/*.py -selfdrive/car/ford/*.py -selfdrive/car/gm/*.py -selfdrive/car/honda/*.py -selfdrive/car/hyundai/*.py -selfdrive/car/mazda/*.py -selfdrive/car/mock/*.py -selfdrive/car/nissan/*.py -selfdrive/car/subaru/*.py -selfdrive/car/tesla/*.py -selfdrive/car/toyota/*.py -selfdrive/car/volkswagen/*.py - -selfdrive/debug/can_printer.py -selfdrive/debug/check_freq.py -selfdrive/debug/dump.py -selfdrive/debug/filter_log_message.py -selfdrive/debug/format_fingerprints.py -selfdrive/debug/get_fingerprint.py -selfdrive/debug/uiview.py - -selfdrive/debug/hyundai_enable_radar_points.py -selfdrive/debug/vw_mqb_config.py - -common/SConscript -common/version.h - -common/*.h -common/*.cc - -selfdrive/controls/__init__.py -selfdrive/controls/controlsd.py -selfdrive/controls/plannerd.py -selfdrive/controls/radard.py -selfdrive/controls/lib/__init__.py -selfdrive/controls/lib/alertmanager.py -selfdrive/controls/lib/alerts_offroad.json -selfdrive/controls/lib/desire_helper.py -selfdrive/controls/lib/drive_helpers.py -selfdrive/controls/lib/events.py -selfdrive/controls/lib/latcontrol_angle.py -selfdrive/controls/lib/latcontrol_torque.py -selfdrive/controls/lib/latcontrol_pid.py -selfdrive/controls/lib/latcontrol.py -selfdrive/controls/lib/longcontrol.py -selfdrive/controls/lib/longitudinal_planner.py -selfdrive/controls/lib/pid.py -selfdrive/controls/lib/vehicle_model.py - -selfdrive/controls/lib/lateral_mpc_lib/.gitignore -selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore -selfdrive/controls/lib/lateral_mpc_lib/* -selfdrive/controls/lib/longitudinal_mpc_lib/* - -system/__init__.py -system/*.py - -system/hardware/__init__.py -system/hardware/base.h -system/hardware/base.py -system/hardware/hw.h -system/hardware/hw.py -system/hardware/tici/__init__.py -system/hardware/tici/hardware.h -system/hardware/tici/hardware.py -system/hardware/tici/pins.py -system/hardware/tici/agnos.py -system/hardware/tici/casync.py -system/hardware/tici/agnos.json -system/hardware/tici/amplifier.py -system/hardware/tici/updater -system/hardware/tici/iwlist.py -system/hardware/tici/esim.nmconnection -system/hardware/pc/__init__.py -system/hardware/pc/hardware.h -system/hardware/pc/hardware.py - -system/ubloxd/.gitignore -system/ubloxd/SConscript -system/ubloxd/generated/* -system/ubloxd/*.h -system/ubloxd/*.cc - -selfdrive/locationd/__init__.py -selfdrive/locationd/SConscript -selfdrive/locationd/.gitignore -selfdrive/locationd/locationd.h -selfdrive/locationd/locationd.cc -selfdrive/locationd/paramsd.py -selfdrive/locationd/models/__init__.py -selfdrive/locationd/models/.gitignore -selfdrive/locationd/models/car_kf.py -selfdrive/locationd/models/live_kf.py -selfdrive/locationd/models/live_kf.h -selfdrive/locationd/models/live_kf.cc -selfdrive/locationd/models/constants.py - -selfdrive/locationd/torqued.py -selfdrive/locationd/calibrationd.py -selfdrive/locationd/helpers.py - -system/logcatd/.gitignore -system/logcatd/SConscript -system/logcatd/logcatd_systemd.cc - -system/proclogd/SConscript -system/proclogd/main.cc -system/proclogd/proclog.cc -system/proclogd/proclog.h - -system/loggerd/.gitignore -system/loggerd/SConscript -system/loggerd/encoder/encoder.cc -system/loggerd/encoder/encoder.h -system/loggerd/encoder/v4l_encoder.cc -system/loggerd/encoder/v4l_encoder.h -system/loggerd/video_writer.cc -system/loggerd/video_writer.h -system/loggerd/logger.cc -system/loggerd/logger.h -system/loggerd/loggerd.cc -system/loggerd/loggerd.h -system/loggerd/encoderd.cc -system/loggerd/bootlog.cc -system/loggerd/encoder/ffmpeg_encoder.cc -system/loggerd/encoder/ffmpeg_encoder.h - -system/loggerd/__init__.py -system/loggerd/config.py -system/loggerd/uploader.py -system/loggerd/deleter.py -system/loggerd/xattr_cache.py - -system/sensord/.gitignore -system/sensord/SConscript -system/sensord/sensors_qcom2.cc -system/sensord/sensors/*.cc -system/sensord/sensors/*.h -system/sensord/pigeond.py - -system/webrtc/__init__.py -system/webrtc/webrtcd.py -system/webrtc/schema.py -system/webrtc/device/audio.py -system/webrtc/device/video.py - -selfdrive/thermald/thermald.py -selfdrive/thermald/power_monitoring.py -selfdrive/thermald/fan_controller.py - -selfdrive/test/__init__.py -selfdrive/test/fuzzy_generation.py -selfdrive/test/helpers.py -selfdrive/test/setup_device_ci.sh -selfdrive/test/test_onroad.py -selfdrive/test/test_time_to_onroad.py - -selfdrive/ui/.gitignore -selfdrive/ui/SConscript -selfdrive/ui/*.cc -selfdrive/ui/*.h -selfdrive/ui/text -selfdrive/ui/spinner -selfdrive/ui/soundd.py -selfdrive/ui/translations/*.ts -selfdrive/ui/translations/languages.json -selfdrive/ui/update_translations.py -selfdrive/ui/tests/test_translations.py - -selfdrive/ui/qt/*.cc -selfdrive/ui/qt/*.h -selfdrive/ui/qt/network/*.cc -selfdrive/ui/qt/network/*.h -selfdrive/ui/qt/offroad/*.cc -selfdrive/ui/qt/offroad/*.h -selfdrive/ui/qt/offroad/*.qml -selfdrive/ui/qt/widgets/*.cc -selfdrive/ui/qt/widgets/*.h -selfdrive/ui/qt/maps/*.cc -selfdrive/ui/qt/maps/*.h -selfdrive/ui/qt/setup/*.cc -selfdrive/ui/qt/setup/*.h - -selfdrive/ui/installer/*.cc -selfdrive/ui/installer/*.h -selfdrive/ui/installer/*.cc - -system/camerad/SConscript -system/camerad/main.cc - -system/camerad/snapshot/* -system/camerad/cameras/camera_common.h -system/camerad/cameras/camera_common.cc -system/camerad/sensors/*.h -system/camerad/sensors/*.cc - -selfdrive/manager/__init__.py -selfdrive/manager/build.py -selfdrive/manager/helpers.py -selfdrive/manager/manager.py -selfdrive/manager/process_config.py -selfdrive/manager/process.py -selfdrive/manager/test/__init__.py -selfdrive/manager/test/test_manager.py - -selfdrive/modeld/.gitignore -selfdrive/modeld/__init__.py -selfdrive/modeld/SConscript -selfdrive/modeld/modeld.py -selfdrive/modeld/parse_model_outputs.py -selfdrive/modeld/fill_model_msg.py -selfdrive/modeld/get_model_metadata.py -selfdrive/modeld/navmodeld.py -selfdrive/modeld/dmonitoringmodeld.py -selfdrive/modeld/constants.py -selfdrive/modeld/modeld - -selfdrive/modeld/models/__init__.py -selfdrive/modeld/models/*.pxd -selfdrive/modeld/models/*.pyx - -selfdrive/modeld/models/commonmodel.cc -selfdrive/modeld/models/commonmodel.h - -selfdrive/modeld/models/supercombo.onnx - -selfdrive/modeld/models/dmonitoring_model_q.dlc - -selfdrive/modeld/models/navmodel_q.dlc - -selfdrive/modeld/transforms/loadyuv.cc -selfdrive/modeld/transforms/loadyuv.h -selfdrive/modeld/transforms/loadyuv.cl -selfdrive/modeld/transforms/transform.cc -selfdrive/modeld/transforms/transform.h -selfdrive/modeld/transforms/transform.cl - -selfdrive/modeld/thneed/*.py -selfdrive/modeld/thneed/thneed.h -selfdrive/modeld/thneed/thneed_common.cc -selfdrive/modeld/thneed/thneed_qcom2.cc -selfdrive/modeld/thneed/serialize.cc - -selfdrive/modeld/runners/__init__.py -selfdrive/modeld/runners/*.pxd -selfdrive/modeld/runners/*.pyx -selfdrive/modeld/runners/*.cc -selfdrive/modeld/runners/*.h -selfdrive/modeld/runners/*.py - -selfdrive/monitoring/dmonitoringd.py -selfdrive/monitoring/driver_monitor.py - -selfdrive/navd/.gitignore -selfdrive/navd/__init__.py -selfdrive/navd/** - -selfdrive/assets/.gitignore -selfdrive/assets/assets.qrc -selfdrive/assets/*.png -selfdrive/assets/*.svg -selfdrive/assets/body/* -selfdrive/assets/fonts/*.ttf -selfdrive/assets/icons/* -selfdrive/assets/images/* -selfdrive/assets/offroad/* -selfdrive/assets/sounds/* -selfdrive/assets/training/* -selfdrive/assets/navigation/* - -third_party/.gitignore -third_party/SConscript - -third_party/linux/** -third_party/opencl/** - -third_party/json11/json11.cpp -third_party/json11/json11.hpp - -third_party/qrcode/*.cc -third_party/qrcode/*.hpp - -third_party/kaitai/*.h -third_party/kaitai/*.cpp - -third_party/libyuv/include/** - -third_party/snpe/include/** -third_party/snpe/dsp** - -third_party/acados/.gitignore -third_party/acados/include/** -third_party/acados/acados_template/** - -third_party/bootstrap/** -third_party/qt5/larch64/bin/** -third_party/maplibre-native-qt/** - -scripts/update_now.sh -scripts/stop_updater.sh - -teleoprtc/** - -rednose_repo/site_scons/site_tools/rednose_filter.py -rednose/.gitignore -rednose/** - -body/.gitignore -body/board/SConscript -body/board/*.h -body/board/*.c -body/board/*.s -body/board/*.ld -body/board/inc/** -body/board/obj/ -body/board/bldc/** -body/board/drivers/** -body/certs/** -body/crypto/** - -cereal/.gitignore -cereal/__init__.py -cereal/car.capnp -cereal/custom.capnp -cereal/legacy.capnp -cereal/log.capnp -cereal/services.py -cereal/SConscript -cereal/include/** -cereal/logger/logger.h -cereal/messaging/.gitignore -cereal/messaging/__init__.py -cereal/messaging/bridge.cc -cereal/messaging/event.cc -cereal/messaging/event.h -cereal/messaging/impl_fake.cc -cereal/messaging/impl_fake.h -cereal/messaging/impl_msgq.cc -cereal/messaging/impl_msgq.h -cereal/messaging/impl_zmq.cc -cereal/messaging/impl_zmq.h -cereal/messaging/messaging.cc -cereal/messaging/messaging.h -cereal/messaging/messaging.pxd -cereal/messaging/messaging_pyx.pyx -cereal/messaging/msgq.cc -cereal/messaging/msgq.h -cereal/messaging/socketmaster.cc -cereal/visionipc/.gitignore -cereal/visionipc/__init__.py -cereal/visionipc/*.cc -cereal/visionipc/*.h -cereal/visionipc/*.pyx -cereal/visionipc/*.pxd - -panda/.gitignore -panda/__init__.py -panda/SConscript -panda/board/** -panda/certs/** -panda/crypto/** -panda/examples/query_fw_versions.py -panda/python/** - -opendbc/.gitignore -opendbc/__init__.py -opendbc/can/__init__.py -opendbc/can/SConscript -opendbc/can/can_define.py -opendbc/can/common.cc -opendbc/can/common.h -opendbc/can/common.pxd -opendbc/can/common_dbc.h -opendbc/can/dbc.cc -opendbc/can/packer.cc -opendbc/can/packer.py -opendbc/can/packer_pyx.pyx -opendbc/can/parser.cc -opendbc/can/parser.py -opendbc/can/parser_pyx.pyx - -opendbc/comma_body.dbc - -opendbc/chrysler_ram_hd_generated.dbc -opendbc/chrysler_ram_dt_generated.dbc -opendbc/chrysler_pacifica_2017_hybrid_generated.dbc -opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc - -opendbc/gm_global_a_powertrain_generated.dbc -opendbc/gm_global_a_object.dbc -opendbc/gm_global_a_chassis.dbc - -opendbc/FORD_CADS.dbc -opendbc/ford_fusion_2018_adas.dbc -opendbc/ford_lincoln_base_pt.dbc - -opendbc/honda_accord_2018_can_generated.dbc -opendbc/acura_ilx_2016_can_generated.dbc -opendbc/acura_rdx_2018_can_generated.dbc -opendbc/acura_rdx_2020_can_generated.dbc -opendbc/honda_civic_touring_2016_can_generated.dbc -opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc -opendbc/honda_crv_touring_2016_can_generated.dbc -opendbc/honda_crv_ex_2017_can_generated.dbc -opendbc/honda_crv_ex_2017_body_generated.dbc -opendbc/honda_crv_executive_2016_can_generated.dbc -opendbc/honda_fit_ex_2018_can_generated.dbc -opendbc/honda_odyssey_exl_2018_generated.dbc -opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc -opendbc/honda_insight_ex_2019_can_generated.dbc -opendbc/acura_ilx_2016_nidec.dbc -opendbc/honda_civic_ex_2022_can_generated.dbc - -opendbc/hyundai_canfd.dbc -opendbc/hyundai_kia_generic.dbc -opendbc/hyundai_kia_mando_front_radar_generated.dbc - -opendbc/mazda_2017.dbc - -opendbc/nissan_x_trail_2017_generated.dbc -opendbc/nissan_leaf_2018_generated.dbc - -opendbc/subaru_global_2017_generated.dbc -opendbc/subaru_global_2020_hybrid_generated.dbc -opendbc/subaru_outback_2015_generated.dbc -opendbc/subaru_outback_2019_generated.dbc -opendbc/subaru_forester_2017_generated.dbc - -opendbc/toyota_tnga_k_pt_generated.dbc -opendbc/toyota_new_mc_pt_generated.dbc -opendbc/toyota_nodsu_pt_generated.dbc -opendbc/toyota_adas.dbc -opendbc/toyota_tss2_adas.dbc - -opendbc/vw_golf_mk4.dbc -opendbc/vw_mqb_2010.dbc - -opendbc/tesla_can.dbc -opendbc/tesla_radar.dbc -opendbc/tesla_powertrain.dbc - -tinygrad_repo/openpilot/compile2.py -tinygrad_repo/extra/onnx.py -tinygrad_repo/extra/onnx_ops.py -tinygrad_repo/extra/thneed.py -tinygrad_repo/extra/utils.py -tinygrad_repo/tinygrad/codegen/kernel.py -tinygrad_repo/tinygrad/codegen/linearizer.py -tinygrad_repo/tinygrad/features/image.py -tinygrad_repo/tinygrad/features/search.py -tinygrad_repo/tinygrad/nn/* -tinygrad_repo/tinygrad/renderer/cstyle.py -tinygrad_repo/tinygrad/renderer/opencl.py -tinygrad_repo/tinygrad/runtime/lib.py -tinygrad_repo/tinygrad/runtime/ops_cpu.py -tinygrad_repo/tinygrad/runtime/ops_disk.py -tinygrad_repo/tinygrad/runtime/ops_gpu.py -tinygrad_repo/tinygrad/shape/* -tinygrad_repo/tinygrad/*.py diff --git a/release/files_pc b/release/files_pc deleted file mode 100644 index f2bf090f2c..0000000000 --- a/release/files_pc +++ /dev/null @@ -1,4 +0,0 @@ -third_party/libyuv/x86_64/** -third_party/snpe/x86_64/** -third_party/snpe/x86_64-linux-clang/** -third_party/acados/x86_64/** diff --git a/release/files_tici b/release/files_tici deleted file mode 100644 index 1771c45138..0000000000 --- a/release/files_tici +++ /dev/null @@ -1,15 +0,0 @@ -third_party/libyuv/larch64/** -third_party/snpe/larch64** -third_party/snpe/aarch64-ubuntu-gcc7.5/* -third_party/acados/larch64/** - -system/camerad/cameras/camera_qcom2.cc -system/camerad/cameras/camera_qcom2.h -system/camerad/cameras/camera_util.cc -system/camerad/cameras/camera_util.h -system/camerad/cameras/real_debayer.cl - -system/qcomgpsd/* - -selfdrive/ui/qt/spinner_larch64 -selfdrive/ui/qt/text_larch64 diff --git a/release/release_files.py b/release/release_files.py new file mode 100755 index 0000000000..031b51737d --- /dev/null +++ b/release/release_files.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python3 +import os +import re +from pathlib import Path + +HERE = os.path.abspath(os.path.dirname(__file__)) +ROOT = HERE + "/.." + +# blacklisting is for two purposes: +# - minimizing release download size +# - keeping the diff readable +blacklist = [ + "body/STL/", + + "panda/drivers/", + "panda/examples/", + "panda/tests/safety/", + + "opendbc/.*.dbc$", + "opendbc/generator/", + + "cereal/.*test.*", + "^common/tests/", + + # particularly large text files + "poetry.lock", + "third_party/catch2", + "selfdrive/car/tests/test_models.*", + + "^tools/", + "^scripts/", + "^tinygrad_repo/", + + "matlab.*.md", + + ".git/", + ".github/", + ".devcontainer/", + "Darwin/", + ".vscode", + + # common things + "LICENSE", + "Dockerfile", + ".pre-commit", + + # no LFS or submodules in release + ".lfsconfig", + ".gitattributes", + ".git$", + ".gitmodules", +] + +# gets you through the blacklist +whitelist = [ + "tools/lib/", + "tools/bodyteleop/", + + "tinygrad_repo/openpilot/compile2.py", + "tinygrad_repo/extra/onnx.py", + "tinygrad_repo/extra/onnx_ops.py", + "tinygrad_repo/extra/thneed.py", + "tinygrad_repo/extra/utils.py", + "tinygrad_repo/tinygrad/codegen/kernel.py", + "tinygrad_repo/tinygrad/codegen/linearizer.py", + "tinygrad_repo/tinygrad/features/image.py", + "tinygrad_repo/tinygrad/features/search.py", + "tinygrad_repo/tinygrad/nn/*", + "tinygrad_repo/tinygrad/renderer/cstyle.py", + "tinygrad_repo/tinygrad/renderer/opencl.py", + "tinygrad_repo/tinygrad/runtime/lib.py", + "tinygrad_repo/tinygrad/runtime/ops_cpu.py", + "tinygrad_repo/tinygrad/runtime/ops_disk.py", + "tinygrad_repo/tinygrad/runtime/ops_gpu.py", + "tinygrad_repo/tinygrad/shape/*", + "tinygrad_repo/tinygrad/.*.py", + + # TODO: do this automatically + "opendbc/comma_body.dbc", + "opendbc/chrysler_ram_hd_generated.dbc", + "opendbc/chrysler_ram_dt_generated.dbc", + "opendbc/chrysler_pacifica_2017_hybrid_generated.dbc", + "opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc", + "opendbc/gm_global_a_powertrain_generated.dbc", + "opendbc/gm_global_a_object.dbc", + "opendbc/gm_global_a_chassis.dbc", + "opendbc/FORD_CADS.dbc", + "opendbc/ford_fusion_2018_adas.dbc", + "opendbc/ford_lincoln_base_pt.dbc", + "opendbc/honda_accord_2018_can_generated.dbc", + "opendbc/acura_ilx_2016_can_generated.dbc", + "opendbc/acura_rdx_2018_can_generated.dbc", + "opendbc/acura_rdx_2020_can_generated.dbc", + "opendbc/honda_civic_touring_2016_can_generated.dbc", + "opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc", + "opendbc/honda_crv_touring_2016_can_generated.dbc", + "opendbc/honda_crv_ex_2017_can_generated.dbc", + "opendbc/honda_crv_ex_2017_body_generated.dbc", + "opendbc/honda_crv_executive_2016_can_generated.dbc", + "opendbc/honda_fit_ex_2018_can_generated.dbc", + "opendbc/honda_odyssey_exl_2018_generated.dbc", + "opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc", + "opendbc/honda_insight_ex_2019_can_generated.dbc", + "opendbc/acura_ilx_2016_nidec.dbc", + "opendbc/honda_civic_ex_2022_can_generated.dbc", + "opendbc/hyundai_canfd.dbc", + "opendbc/hyundai_kia_generic.dbc", + "opendbc/hyundai_kia_mando_front_radar_generated.dbc", + "opendbc/mazda_2017.dbc", + "opendbc/nissan_x_trail_2017_generated.dbc", + "opendbc/nissan_leaf_2018_generated.dbc", + "opendbc/subaru_global_2017_generated.dbc", + "opendbc/subaru_global_2020_hybrid_generated.dbc", + "opendbc/subaru_outback_2015_generated.dbc", + "opendbc/subaru_outback_2019_generated.dbc", + "opendbc/subaru_forester_2017_generated.dbc", + "opendbc/toyota_tnga_k_pt_generated.dbc", + "opendbc/toyota_new_mc_pt_generated.dbc", + "opendbc/toyota_nodsu_pt_generated.dbc", + "opendbc/toyota_adas.dbc", + "opendbc/toyota_tss2_adas.dbc", + "opendbc/vw_golf_mk4.dbc", + "opendbc/vw_mqb_2010.dbc", + "opendbc/tesla_can.dbc", + "opendbc/tesla_radar_bosch_generated.dbc", + "opendbc/tesla_radar_continental_generated.dbc", + "opendbc/tesla_powertrain.dbc", +] + + +if __name__ == "__main__": + for f in Path(ROOT).rglob("**/*"): + if not (f.is_file() or f.is_symlink()): + continue + + rf = str(f.relative_to(ROOT)) + blacklisted = any(re.search(p, rf) for p in blacklist) + whitelisted = any(re.search(p, rf) for p in whitelist) + if blacklisted and not whitelisted: + continue + + print(rf) diff --git a/release/verify.sh b/release/verify.sh deleted file mode 100755 index ec5266bd81..0000000000 --- a/release/verify.sh +++ /dev/null @@ -1,16 +0,0 @@ -#!/bin/bash - -set -e - -RED="\033[0;31m" -GREEN="\033[0;32m" -CLEAR="\033[0m" - -BRANCHES="devel release3" -for b in $BRANCHES; do - if git diff --quiet origin/$b origin/$b-staging && [ "$(git rev-parse origin/$b)" = "$(git rev-parse origin/$b-staging)" ]; then - printf "%-10s $GREEN ok $CLEAR\n" "$b" - else - printf "%-10s $RED mismatch $CLEAR\n" "$b" - fi -done diff --git a/scripts/stop_updater.sh b/scripts/stop_updater.sh deleted file mode 100755 index 4243d30e9f..0000000000 --- a/scripts/stop_updater.sh +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env sh - -# Stop updater -pkill -2 -f selfdrive.updated - -# Remove pending update -rm -f /data/safe_staging/finalized/.overlay_consistent diff --git a/scripts/update_now.sh b/scripts/update_now.sh deleted file mode 100755 index 3f0193f081..0000000000 --- a/scripts/update_now.sh +++ /dev/null @@ -1,4 +0,0 @@ -#!/usr/bin/env sh - -# Send SIGHUP to updater -pkill -1 -f selfdrive.updated diff --git a/selfdrive/SConscript b/selfdrive/SConscript new file mode 100644 index 0000000000..52898f758f --- /dev/null +++ b/selfdrive/SConscript @@ -0,0 +1,7 @@ +SConscript(['pandad/SConscript']) +SConscript(['controls/lib/lateral_mpc_lib/SConscript']) +SConscript(['controls/lib/longitudinal_mpc_lib/SConscript']) +SConscript(['locationd/SConscript']) +SConscript(['navd/SConscript']) +SConscript(['modeld/SConscript']) +SConscript(['ui/SConscript']) \ No newline at end of file diff --git a/selfdrive/assets/compress-images.sh b/selfdrive/assets/compress-images.sh new file mode 100755 index 0000000000..a1a4f8bb40 --- /dev/null +++ b/selfdrive/assets/compress-images.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +echo "compressing training guide images" +optipng -o7 -strip all training/* + +# This can sometimes provide smaller images +# mogrify -quality 100 -format jpg training/* diff --git a/selfdrive/assets/strip-svg-metadata.sh b/selfdrive/assets/strip-svg-metadata.sh new file mode 100755 index 0000000000..a8b35eadde --- /dev/null +++ b/selfdrive/assets/strip-svg-metadata.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# sudo apt install scour + +for svg in $(find icons/ -type f | grep svg$); do + # scour doesn't support overwriting input file + scour $svg --remove-metadata $svg.tmp + mv $svg.tmp $svg +done diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore deleted file mode 100644 index e8daa2ef27..0000000000 --- a/selfdrive/boardd/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -boardd -boardd_api_impl.cpp -tests/test_boardd_usbprotocol diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript deleted file mode 100644 index 666763d9b0..0000000000 --- a/selfdrive/boardd/SConscript +++ /dev/null @@ -1,11 +0,0 @@ -Import('env', 'envCython', 'common', 'cereal', 'messaging') - -libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) - -env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs) -env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) - -envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) -if GetOption('extras'): - env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/boardd/set_time.py b/selfdrive/boardd/set_time.py deleted file mode 100755 index fe17f64e82..0000000000 --- a/selfdrive/boardd/set_time.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python3 -import os -import datetime -from panda import Panda - -from openpilot.common.time import MIN_DATE - -def set_time(logger): - sys_time = datetime.datetime.today() - if sys_time > MIN_DATE: - logger.info("System time valid") - return - - try: - ps = Panda.list() - if len(ps) == 0: - logger.error("Failed to set time, no pandas found") - return - - for s in ps: - with Panda(serial=s) as p: - if not p.is_internal(): - continue - - # Set system time from panda RTC time - panda_time = p.get_datetime() - if panda_time > MIN_DATE: - logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'") - os.system(f"TZ=UTC date -s '{panda_time}'") - break - except Exception: - logger.exception("Failed to fetch time from panda") - -if __name__ == "__main__": - import logging - logging.basicConfig(level=logging.DEBUG) - - set_time(logging) diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py deleted file mode 100755 index 148ce9a25d..0000000000 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ /dev/null @@ -1,102 +0,0 @@ -#!/usr/bin/env python3 -import os -import copy -import random -import time -import unittest -from collections import defaultdict -from pprint import pprint - -import cereal.messaging as messaging -from cereal import car, log -from openpilot.common.params import Params -from openpilot.common.timeout import Timeout -from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp -from openpilot.selfdrive.car import make_can_msg -from openpilot.system.hardware import TICI -from openpilot.selfdrive.test.helpers import phone_only, with_processes - - -class TestBoardd(unittest.TestCase): - - @classmethod - def setUpClass(cls): - os.environ['STARTED'] = '1' - os.environ['BOARDD_LOOPBACK'] = '1' - - @phone_only - @with_processes(['pandad']) - def test_loopback(self): - params = Params() - params.put_bool("IsOnroad", False) - - with Timeout(90, "boardd didn't start"): - sm = messaging.SubMaster(['pandaStates']) - while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ - any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): - sm.update(1000) - - num_pandas = len(sm['pandaStates']) - expected_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1 - self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \ - connect another panda for multipanda tests.") - - # boardd safety setting relies on these params - cp = car.CarParams.new_message() - - safety_config = car.CarParams.SafetyConfig.new_message() - safety_config.safetyModel = car.CarParams.SafetyModel.allOutput - cp.safetyConfigs = [safety_config]*num_pandas - - params.put_bool("IsOnroad", True) - params.put_bool("FirmwareQueryDone", True) - params.put_bool("ControlsReady", True) - params.put("CarParams", cp.to_bytes()) - - sendcan = messaging.pub_sock('sendcan') - can = messaging.sub_sock('can', conflate=False, timeout=100) - sm = messaging.SubMaster(['pandaStates']) - time.sleep(0.5) - - n = 200 - for i in range(n): - print(f"boardd loopback {i}/{n}") - - sent_msgs = defaultdict(set) - for _ in range(random.randrange(20, 100)): - to_send = [] - for __ in range(random.randrange(20)): - bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) - addr = random.randrange(1, 1<<29) - dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) - sent_msgs[bus].add((addr, dat)) - to_send.append(make_can_msg(addr, dat, bus)) - sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) - - sent_loopback = copy.deepcopy(sent_msgs) - sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()}) - sent_total = {k: len(v) for k, v in sent_loopback.items()} - for _ in range(100 * 5): - sm.update(0) - recvd = messaging.drain_sock(can, wait_for_one=True) - for msg in recvd: - for m in msg.can: - key = (m.address, m.dat) - assert key in sent_loopback[m.src], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}" - sent_loopback[m.src].discard(key) - - if all(len(v) == 0 for v in sent_loopback.values()): - break - - # if a set isn't empty, messages got dropped - pprint(sent_msgs) - pprint(sent_loopback) - print({k: len(x) for k, x in sent_loopback.items()}) - print(sum([len(x) for x in sent_loopback.values()])) - pprint(sm['pandaStates']) # may drop messages due to RX buffer overflow - for bus in sent_loopback.keys(): - assert not len(sent_loopback[bus]), f"loop {i}: bus {bus} missing {len(sent_loopback[bus])} out of {sent_total[bus]} messages" - - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/car/CARS_template.md b/selfdrive/car/CARS_template.md new file mode 100644 index 0000000000..2f49e79b81 --- /dev/null +++ b/selfdrive/car/CARS_template.md @@ -0,0 +1,74 @@ +{% set footnote_tag = '[{}](#footnotes)' %} +{% set star_icon = '[![star](assets/icon-star-{}.svg)](##)' %} +{% set video_icon = '' %} +{# Force hardware column wider by using a blank image with max width. #} +{% set width_tag = '%s
     ' %} +{% set hardware_col_name = 'Hardware Needed' %} +{% set wide_hardware_col_name = width_tag|format(hardware_col_name) -%} + + + +# Supported Cars + +A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. + +# {{all_car_docs | length}} Supported Cars + +|{{Column | map(attribute='value') | join('|') | replace(hardware_col_name, wide_hardware_col_name)}}| +|---|---|---|{% for _ in range((Column | length) - 3) %}{{':---:|'}}{% endfor +%} +{% for car_docs in all_car_docs %} +|{% for column in Column %}{{car_docs.get_column(column, star_icon, video_icon, footnote_tag)}}|{% endfor %} + +{% endfor %} + +### Footnotes +{% for footnote in footnotes %} +{{loop.index}}{{footnote}}
    +{% endfor %} + +## Community Maintained Cars +Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/). + +# Don't see your car here? + +**openpilot can support many more cars than it currently does.** There are a few reasons your car may not be supported. +If your car doesn't fit into any of the incompatibility criteria here, then there's a good chance it can be supported! We're adding support for new cars all the time. **We don't have a roadmap for car support**, and in fact, most car support comes from users like you! + +### Which cars are able to be supported? + +openpilot uses the existing steering, gas, and brake interfaces in your car. If your car lacks any one of these interfaces, openpilot will not be able to control the car. If your car has [ACC](https://en.wikipedia.org/wiki/Adaptive_cruise_control) and any form of [LKAS](https://en.wikipedia.org/wiki/Automated_Lane_Keeping_Systems)/[LCA](https://en.wikipedia.org/wiki/Lane_centering), then it almost certainly has these interfaces. These features generally started shipping on cars around 2016. Note that manufacturers will often make their own [marketing terms](https://en.wikipedia.org/wiki/Adaptive_cruise_control#Vehicle_models_supporting_adaptive_cruise_control) for these features, such as Hyundai's "Smart Cruise Control" branding of Adaptive Cruise Control. + +If your car has the following packages or features, then it's a good candidate for support. + +| Make | Required Package/Features | +| ---- | ------------------------- | +| Acura | Any car with AcuraWatch Plus will work. AcuraWatch Plus comes standard on many newer models. | +| Ford | Any car with Lane Centering will likely work. | +| Honda | Any car with Honda Sensing will work. Honda Sensing comes standard on many newer models. | +| Subaru | Any car with EyeSight will work. EyeSight comes standard on many newer models. | +| Nissan | Any car with ProPILOT will likely work. | +| Toyota & Lexus | Any car that has Toyota/Lexus Safety Sense with "Lane Departure Alert with Steering Assist (LDA w/SA)" and/or "Lane Tracing Assist (LTA)" will work. Note that LDA without Steering Assist will not work. These features come standard on most newer models. | +| Hyundai, Kia, & Genesis | Any car with Smart Cruise Control (SCC) and Lane Following Assist (LFA) or Lane Keeping Assist (LKAS) will work. LKAS/LFA comes standard on most newer models. Any form of SCC will work, such as NSCC. | +| Chrysler, Jeep, & Ram | Any car with LaneSense and Adaptive Cruise Control will likely work. These come standard on many newer models. | + +### FlexRay + +All the cars that openpilot supports use a [CAN bus](https://en.wikipedia.org/wiki/CAN_bus) for communication between all the car's computers, however a CAN bus isn't the only way that the computers in your car can communicate. Most, if not all, vehicles from the following manufacturers use [FlexRay](https://en.wikipedia.org/wiki/FlexRay) instead of a CAN bus: **BMW, Mercedes, Audi, Land Rover, and some Volvo**. These cars may one day be supported, but we have no immediate plans to support FlexRay. + +### Toyota Security + +openpilot does not yet support these Toyota models due to a new message authentication method. +[Vote](https://comma.ai/shop#toyota-security) if you'd like to see openpilot support on these models. + +* Toyota RAV4 Prime 2021+ +* Toyota Sienna 2021+ +* Toyota Venza 2021+ +* Toyota Sequoia 2023+ +* Toyota Tundra 2022+ +* Toyota Highlander 2024+ +* Toyota Corolla Cross 2022+ (only US model) +* Toyota Camry 2025+ +* Lexus NX 2022+ +* Toyota bZ4x 2023+ +* Subaru Solterra 2023+ + diff --git a/selfdrive/car/README.md b/selfdrive/car/README.md new file mode 100644 index 0000000000..2c49cf2051 --- /dev/null +++ b/selfdrive/car/README.md @@ -0,0 +1,19 @@ +## Car port structure + +### interface.py +Generic interface to send and receive messages from CAN (controlsd uses this to communicate with car) + +### fingerprints.py +Fingerprints for matching to a specific car + +### carcontroller.py +Builds CAN messages to send to car + +##### carstate.py +Reads CAN from car and builds openpilot CarState message + +##### values.py +Limits for actuation, general constants for cars, and supported car documentation + +##### radar_interface.py +Interface for parsing radar points from the car diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index c90ae50ab9..4a1df550d0 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -1,11 +1,15 @@ # functions common among cars from collections import namedtuple -from typing import Dict, List, Optional +from dataclasses import dataclass +from enum import IntFlag, ReprEnum, EnumType +from dataclasses import replace import capnp from cereal import car from openpilot.common.numpy_fast import clip, interp +from openpilot.common.utils import Freezable +from openpilot.selfdrive.car.docs_definitions import CarDocs # kg of standard extra cargo to count for drive, gas, etc... @@ -24,9 +28,9 @@ def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float: return val_steady -def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: Dict[int, capnp.lib.capnp._EnumModule], - unpressed_btn: int = 0) -> List[capnp.lib.capnp._DynamicStructBuilder]: - events: List[capnp.lib.capnp._DynamicStructBuilder] = [] +def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, capnp.lib.capnp._EnumModule], + unpressed_btn: int = 0) -> list[capnp.lib.capnp._DynamicStructBuilder]: + events: list[capnp.lib.capnp._DynamicStructBuilder] = [] if cur_btn == prev_btn: return events @@ -73,7 +77,10 @@ def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor return tire_stiffness_front, tire_stiffness_rear -def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]: +DbcDict = dict[str, str] + + +def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> DbcDict: return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} @@ -158,41 +165,6 @@ def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_fra return above_limit_frames, request -def crc8_pedal(data): - crc = 0xFF # standard init value - poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1 - size = len(data) - for i in range(size - 1, -1, -1): - crc ^= data[i] - for _ in range(8): - if ((crc & 0x80) != 0): - crc = ((crc << 1) ^ poly) & 0xFF - else: - crc <<= 1 - return crc - - -def create_gas_interceptor_command(packer, gas_amount, idx): - # Common gas pedal msg generator - enable = gas_amount > 0.001 - - values = { - "ENABLE": enable, - "COUNTER_PEDAL": idx & 0xF, - } - - if enable: - values["GAS_COMMAND"] = gas_amount * 255. - values["GAS_COMMAND2"] = gas_amount * 255. - - dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2] - - checksum = crc8_pedal(dat[:-1]) - values["CHECKSUM_PEDAL"] = checksum - - return packer.make_can_msg("GAS_COMMAND", 0, values) - - def make_can_msg(addr, dat, bus): return [addr, 0, dat, bus] @@ -208,7 +180,7 @@ def get_safety_config(safety_model, safety_param = None): class CanBusBase: offset: int - def __init__(self, CP, fingerprint: Optional[Dict[int, Dict[int, int]]]) -> None: + def __init__(self, CP, fingerprint: dict[int, dict[int, int]] | None) -> None: if CP is None: assert fingerprint is not None num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1 @@ -236,3 +208,71 @@ class CanSignalRateCalculator: self.previous_value = current_value return self.rate + + +@dataclass(frozen=True, kw_only=True) +class CarSpecs: + mass: float # kg, curb weight + wheelbase: float # meters + steerRatio: float + centerToFrontRatio: float = 0.5 + minSteerSpeed: float = 0.0 # m/s + minEnableSpeed: float = -1.0 # m/s + tireStiffnessFactor: float = 1.0 + + def override(self, **kwargs): + return replace(self, **kwargs) + + +@dataclass(order=True) +class PlatformConfig(Freezable): + car_docs: list[CarDocs] + specs: CarSpecs + + dbc_dict: DbcDict + + flags: int = 0 + + platform_str: str | None = None + + def __hash__(self) -> int: + return hash(self.platform_str) + + def override(self, **kwargs): + return replace(self, **kwargs) + + def init(self): + pass + + def __post_init__(self): + self.init() + + +class PlatformsType(EnumType): + def __new__(metacls, cls, bases, classdict, *, boundary=None, _simple=False, **kwds): + for key in classdict._member_names.keys(): + cfg: PlatformConfig = classdict[key] + cfg.platform_str = key + cfg.freeze() + return super().__new__(metacls, cls, bases, classdict, boundary=boundary, _simple=_simple, **kwds) + + +class Platforms(str, ReprEnum, metaclass=PlatformsType): + config: PlatformConfig + + def __new__(cls, platform_config: PlatformConfig): + member = str.__new__(cls, platform_config.platform_str) + member.config = platform_config + member._value_ = platform_config.platform_str + return member + + def __repr__(self): + return f"<{self.__class__.__name__}.{self.name}>" + + @classmethod + def create_dbc_map(cls) -> dict[str, DbcDict]: + return {p: p.config.dbc_dict for p in cls} + + @classmethod + def with_flags(cls, flags: IntFlag) -> set['Platforms']: + return {p for p in cls if p.config.flags & flags} diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index d15f11d3a4..259126c416 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker from openpilot.selfdrive.car.body import bodycan from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.pid import PIDController @@ -14,7 +15,7 @@ MAX_POS_INTEGRATOR = 0.2 # meters MAX_TURN_INTEGRATOR = 0.1 # meters -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.frame = 0 self.packer = CANPacker(dbc_name) @@ -74,7 +75,7 @@ class CarController: can_sends = [] can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.accel = torque_l new_actuators.steer = torque_r new_actuators.steerOutputCan = torque_r diff --git a/selfdrive/car/body/fingerprints.py b/selfdrive/car/body/fingerprints.py index 6efaabc137..ab7a5f8d7b 100644 --- a/selfdrive/car/body/fingerprints.py +++ b/selfdrive/car/body/fingerprints.py @@ -8,13 +8,13 @@ Ecu = car.CarParams.Ecu FINGERPRINTS = { - CAR.BODY: [{ + CAR.COMMA_BODY: [{ 513: 8, 516: 8, 514: 3, 515: 4 }], } FW_VERSIONS = { - CAR.BODY: { + CAR.COMMA_BODY: { (Ecu.engine, 0x720, None): [ b'0.0.01', b'0.3.00a', diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 12a2d5f304..f797a7ecf8 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -14,14 +14,10 @@ class CarInterface(CarInterfaceBase): ret.minSteerSpeed = -math.inf ret.maxLateralAccel = math.inf # TODO: set to a reasonable value - ret.steerRatio = 0.5 ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0. - ret.mass = 9 - ret.wheelbase = 0.406 ret.wheelSpeedFactor = SPEED_FROM_RPM - ret.centerToFront = ret.wheelbase * 0.44 ret.radarUnavailable = True ret.openpilotLongitudinalControl = True @@ -41,6 +37,3 @@ class CarInterface(CarInterfaceBase): self.frame += 1 return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index 33119bf0fd..a1195f7cb5 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -1,9 +1,6 @@ -from enum import StrEnum -from typing import Dict - from cereal import car -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -22,13 +19,12 @@ class CarControllerParams: pass -class CAR(StrEnum): - BODY = "COMMA BODY" - - -CAR_INFO: Dict[str, CarInfo] = { - CAR.BODY: CarInfo("comma body", package="All"), -} +class CAR(Platforms): + COMMA_BODY = PlatformConfig( + [CarDocs("comma body", package="All")], + CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44), + dbc_dict('comma_body', None), + ) FW_QUERY_CONFIG = FwQueryConfig( @@ -41,7 +37,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.BODY: dbc_dict('comma_body', None), -} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 95028e10c5..66097339b1 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,18 +1,18 @@ import os import time -from typing import Callable, Dict, List, Optional, Tuple +from collections.abc import Callable from cereal import car from openpilot.common.params import Params -from openpilot.common.basedir import BASEDIR -from openpilot.system.version import is_comma_remote, is_tested_branch from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing +from openpilot.selfdrive.car.mock.values import CAR as MOCK from openpilot.common.swaglog import cloudlog import cereal.messaging as messaging from openpilot.selfdrive.car import gen_empty_fingerprint +from openpilot.system.version import get_build_metadata FRAME_FINGERPRINT = 100 # 1s @@ -20,7 +20,8 @@ EventName = car.CarEvent.EventName def get_startup_event(car_recognized, controller_available, fw_seen): - if is_comma_remote() and is_tested_branch(): + build_metadata = get_build_metadata() + if build_metadata.openpilot.comma_remote and build_metadata.tested_channel: event = EventName.startup else: event = EventName.startupMaster @@ -47,23 +48,14 @@ def load_interfaces(brand_names): for brand_name in brand_names: path = f'openpilot.selfdrive.car.{brand_name}' CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface - - if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carstate.py'): - CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState - else: - CarState = None - - if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): - CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController - else: - CarController = None - + CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState + CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController for model_name in brand_names[brand_name]: ret[model_name] = (CarInterface, CarController, CarState) return ret -def _get_interface_names() -> Dict[str, List[str]]: +def _get_interface_names() -> dict[str, list[str]]: # returns a dict of brand name and its respective models brand_names = {} for brand_name, brand_models in get_interface_attr("CAR").items(): @@ -77,7 +69,7 @@ interface_names = _get_interface_names() interfaces = load_interfaces(interface_names) -def can_fingerprint(next_can: Callable) -> Tuple[Optional[str], Dict[int, dict]]: +def can_fingerprint(next_can: Callable) -> tuple[str | None, 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 @@ -147,10 +139,10 @@ def fingerprint(logcan, sendcan, num_pandas): # VIN query only reliably works through OBDII vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1)) ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas) - car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas) + car_fw = get_fw_versions_ordered(logcan, sendcan, vin, ecu_rx_addrs, num_pandas=num_pandas) cached = False - exact_fw_match, fw_candidates = match_fw_to_car(car_fw) + exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin) else: vin_rx_addr, vin_rx_bus, vin = -1, -1, VIN_UNKNOWN exact_fw_match, fw_candidates, car_fw = True, set(), [] @@ -189,33 +181,39 @@ def fingerprint(logcan, sendcan, num_pandas): cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match, cached=cached, fw_count=len(car_fw), ecu_responses=list(ecu_rx_addrs), vin_rx_addr=vin_rx_addr, vin_rx_bus=vin_rx_bus, fingerprints=repr(finger), fw_query_time=fw_query_time, error=True) + return car_fingerprint, finger, vin, car_fw, source, exact_match +def get_car_interface(CP): + CarInterface, CarController, CarState = interfaces[CP.carFingerprint] + return CarInterface(CP, CarController, CarState) + + def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) if candidate is None: cloudlog.event("car doesn't match any fingerprints", fingerprints=repr(fingerprints), error=True) - candidate = "mock" + candidate = "MOCK" - CarInterface, CarController, CarState = interfaces[candidate] + CarInterface, _, _ = interfaces[candidate] CP = CarInterface.get_params(candidate, fingerprints, car_fw, experimental_long_allowed, docs=False) CP.carVin = vin CP.carFw = car_fw CP.fingerprintSource = source CP.fuzzyFingerprint = not exact_match - return CarInterface(CP, CarController, CarState), CP + return get_car_interface(CP), CP -def write_car_param(fingerprint="mock"): +def write_car_param(platform=MOCK.MOCK): params = Params() - CarInterface, _, _ = interfaces[fingerprint] - CP = CarInterface.get_non_essential_params(fingerprint) + CarInterface, _, _ = interfaces[platform] + CP = CarInterface.get_non_essential_params(platform) params.put("CarParams", CP.to_bytes()) def get_demo_car_params(): - fingerprint="mock" - CarInterface, _, _ = interfaces[fingerprint] - CP = CarInterface.get_non_essential_params(fingerprint) + platform = MOCK.MOCK + CarInterface, _, _ = interfaces[platform] + CP = CarInterface.get_non_essential_params(platform) return CP diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py new file mode 100755 index 0000000000..d9ee020ba4 --- /dev/null +++ b/selfdrive/car/card.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +import os +import time + +import cereal.messaging as messaging + +from cereal import car + +from panda import ALTERNATIVE_EXPERIENCE + +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL + +from openpilot.selfdrive.pandad import can_list_to_can_capnp +from openpilot.selfdrive.car.car_helpers import get_car, get_one_can +from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.controls.lib.events import Events + +REPLAY = "REPLAY" in os.environ + +EventName = car.CarEvent.EventName + + +class Car: + CI: CarInterfaceBase + + def __init__(self, CI=None): + self.can_sock = messaging.sub_sock('can', timeout=20) + self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) + self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) + + self.can_rcv_cum_timeout_counter = 0 + + self.CC_prev = car.CarControl.new_message() + self.CS_prev = car.CarState.new_message() + self.initialized_prev = False + + self.last_actuators_output = car.CarControl.Actuators.new_message() + + self.params = Params() + + if CI is None: + # wait for one pandaState and one CAN packet + print("Waiting for CAN messages...") + get_one_can(self.can_sock) + + num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + else: + self.CI, self.CP = CI, CI.CP + + # set alternative experiences from parameters + self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") + self.CP.alternativeExperience = 0 + if not self.disengage_on_accelerator: + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") + + controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly + + self.CP.passive = not controller_available or self.CP.dashcamOnly + if self.CP.passive: + safety_config = car.CarParams.SafetyConfig.new_message() + safety_config.safetyModel = car.CarParams.SafetyModel.noOutput + self.CP.safetyConfigs = [safety_config] + + # Write previous route's CarParams + prev_cp = self.params.get("CarParamsPersistent") + if prev_cp is not None: + self.params.put("CarParamsPrevRoute", prev_cp) + + # Write CarParams for controls and radard + cp_bytes = self.CP.to_bytes() + self.params.put("CarParams", cp_bytes) + self.params.put_nonblocking("CarParamsCache", cp_bytes) + self.params.put_nonblocking("CarParamsPersistent", cp_bytes) + + self.events = Events() + + # card is driven by can recv, expected at 100Hz + self.rk = Ratekeeper(100, print_delay_threshold=None) + + def state_update(self) -> car.CarState: + """carState update loop, driven by can""" + + # Update carState from CAN + can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) + CS = self.CI.update(self.CC_prev, can_strs) + + self.sm.update(0) + + can_rcv_valid = len(can_strs) > 0 + + # Check for CAN timeout + if not can_rcv_valid: + self.can_rcv_cum_timeout_counter += 1 + + if can_rcv_valid and REPLAY: + self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime + + return CS + + def update_events(self, CS: car.CarState) -> car.CarState: + self.events.clear() + + self.events.add_from_msg(CS.events) + + # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 + if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ + (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ + (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): + self.events.add(EventName.pedalPressed) + + CS.events = self.events.to_msg() + + def state_publish(self, CS: car.CarState): + """carState and carParams publish loop""" + + # carParams - logged every 50 seconds (> 1 per segment) + if self.sm.frame % int(50. / DT_CTRL) == 0: + cp_send = messaging.new_message('carParams') + cp_send.valid = True + cp_send.carParams = self.CP + self.pm.send('carParams', cp_send) + + # publish new carOutput + co_send = messaging.new_message('carOutput') + co_send.valid = self.sm.all_checks(['carControl']) + co_send.carOutput.actuatorsOutput = self.last_actuators_output + self.pm.send('carOutput', co_send) + + # kick off controlsd step while we actuate the latest carControl packet + cs_send = messaging.new_message('carState') + cs_send.valid = CS.canValid + cs_send.carState = CS + cs_send.carState.canErrorCounter = self.can_rcv_cum_timeout_counter + cs_send.carState.cumLagMs = -self.rk.remaining * 1000. + self.pm.send('carState', cs_send) + + def controls_update(self, CS: car.CarState, CC: car.CarControl): + """control update loop, driven by carControl""" + + if not self.initialized_prev: + # Initialize CarInterface, once controls are ready + # TODO: this can make us miss at least a few cycles when doing an ECU knockout + self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + # signal pandad to switch to car safety mode + self.params.put_bool_nonblocking("ControlsReady", True) + + if self.sm.all_alive(['carControl']): + # send car controls over can + now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) + self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos) + self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) + + self.CC_prev = CC + + def step(self): + CS = self.state_update() + + self.update_events(CS) + + self.state_publish(CS) + + initialized = (not any(e.name == EventName.controlsInitializing for e in self.sm['onroadEvents']) and + self.sm.seen['onroadEvents']) + if not self.CP.passive and initialized: + self.controls_update(CS, self.sm['carControl']) + + self.initialized_prev = initialized + self.CS_prev = CS.as_reader() + + def card_thread(self): + while True: + self.step() + self.rk.monitor_time() + + +def main(): + config_realtime_process(4, Priority.CTRL_HIGH) + car = Car() + car.card_thread() + + +if __name__ == "__main__": + main() diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 050eb41b1a..85f53f68eb 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -3,9 +3,10 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_meas_steer_torque_limits from openpilot.selfdrive.car.chrysler import chryslercan from openpilot.selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags +from openpilot.selfdrive.car.interfaces import CarControllerBase -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -77,7 +78,7 @@ class CarController: self.frame += 1 - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index eb1cf7e7d5..91b922c596 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -21,10 +21,16 @@ class CarState(CarStateBase): else: self.shifter_values = can_define.dv["GEAR"]["PRNDL"] + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_cam): ret = car.CarState.new_message() + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"] + # lock info ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"], cp.vl["BCM_1"]["DOOR_OPEN_FR"], diff --git a/selfdrive/car/chrysler/fingerprints.py b/selfdrive/car/chrysler/fingerprints.py index 1df514e79b..5072aad5c0 100644 --- a/selfdrive/car/chrysler/fingerprints.py +++ b/selfdrive/car/chrysler/fingerprints.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car.chrysler.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.PACIFICA_2017_HYBRID: { + CAR.CHRYSLER_PACIFICA_2017_HYBRID: { (Ecu.combinationMeter, 0x742, None): [ b'68239262AH', b'68239262AI', @@ -33,11 +33,12 @@ FW_VERSIONS = { b'05190226AK', ], }, - CAR.PACIFICA_2018: { + CAR.CHRYSLER_PACIFICA_2018: { (Ecu.combinationMeter, 0x742, None): [ b'68227902AF', b'68227902AG', b'68227902AH', + b'68227905AG', b'68360252AC', ], (Ecu.srs, 0x744, None): [ @@ -67,10 +68,12 @@ FW_VERSIONS = { (Ecu.engine, 0x7e0, None): [ b'68267018AO ', b'68267020AJ ', + b'68303534AG ', b'68303534AJ ', b'68340762AD ', b'68340764AD ', b'68352652AE ', + b'68352654AE ', b'68366851AH ', b'68366853AE ', b'68372861AF ', @@ -79,6 +82,7 @@ FW_VERSIONS = { b'68277370AJ', b'68277370AM', b'68277372AD', + b'68277372AE', b'68277372AN', b'68277374AA', b'68277374AB', @@ -88,19 +92,22 @@ FW_VERSIONS = { b'68380571AB', ], }, - CAR.PACIFICA_2020: { + CAR.CHRYSLER_PACIFICA_2020: { (Ecu.combinationMeter, 0x742, None): [ b'68405327AC', b'68436233AB', b'68436233AC', + b'68436234AB', b'68436250AE', b'68529067AA', b'68594993AB', + b'68594994AB', ], (Ecu.srs, 0x744, None): [ b'68405565AB', b'68405565AC', b'68444299AC', + b'68480707AC', b'68480708AC', b'68526663AB', ], @@ -119,15 +126,18 @@ FW_VERSIONS = { b'68540436AC', b'68540436AD', b'68598670AB', + b'68598670AC', ], (Ecu.eps, 0x75a, None): [ b'68416742AA', b'68460393AA', b'68460393AB', b'68494461AB', + b'68494461AC', b'68524936AA', b'68524936AB', b'68525338AB', + b'68594337AB', b'68594340AB', ], (Ecu.engine, 0x7e0, None): [ @@ -135,30 +145,41 @@ FW_VERSIONS = { b'68413871AE ', b'68413871AH ', b'68413871AI ', + b'68413873AH ', b'68413873AI ', b'68443120AE ', b'68443123AC ', b'68443125AC ', + b'68496647AI ', + b'68496647AJ ', + b'68496650AH ', + b'68496650AI ', + b'68496652AH ', b'68526752AD ', b'68526752AE ', b'68526754AE ', b'68536264AE ', + b'68700304AB ', b'68700306AB ', ], (Ecu.transmission, 0x7e1, None): [ b'68414271AC', b'68414271AD', b'68414275AC', + b'68414275AD', b'68443154AB', b'68443155AC', b'68443158AB', b'68501050AD', + b'68501051AD', + b'68501055AD', b'68527221AB', b'68527223AB', b'68586231AD', + b'68586233AD', ], }, - CAR.PACIFICA_2018_HYBRID: { + CAR.CHRYSLER_PACIFICA_2018_HYBRID: { (Ecu.combinationMeter, 0x742, None): [ b'68358439AE', b'68358439AG', @@ -185,7 +206,7 @@ FW_VERSIONS = { b'05190226AM', ], }, - CAR.PACIFICA_2019_HYBRID: { + CAR.CHRYSLER_PACIFICA_2019_HYBRID: { (Ecu.combinationMeter, 0x742, None): [ b'68405292AC', b'68434956AC', @@ -250,27 +271,34 @@ FW_VERSIONS = { }, CAR.JEEP_GRAND_CHEROKEE: { (Ecu.combinationMeter, 0x742, None): [ + b'68243549AG', b'68302211AC', b'68302212AD', + b'68302223AC', b'68302246AC', b'68331511AC', b'68331574AC', b'68331687AC', + b'68331690AC', b'68340272AD', ], (Ecu.srs, 0x744, None): [ + b'68309533AA', b'68316742AB', b'68355363AB', ], (Ecu.abs, 0x747, None): [ + b'68252642AG', b'68306178AD', b'68336276AB', ], (Ecu.fwdRadar, 0x753, None): [ b'04672627AB', + b'68251506AF', b'68332015AB', ], (Ecu.eps, 0x75a, None): [ + b'68276201AG', b'68321644AB', b'68321644AC', b'68321646AC', @@ -278,6 +306,7 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'05035920AE ', + b'68252272AG ', b'68284455AI ', b'68284456AI ', b'68284477AF ', @@ -288,6 +317,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'05035517AH', + b'68253222AF', b'68311218AC', b'68311223AF', b'68311223AG', @@ -304,6 +334,7 @@ FW_VERSIONS = { b'68402708AB', b'68402971AD', b'68454144AD', + b'68454145AB', b'68454152AB', b'68454156AB', b'68516650AB', @@ -359,7 +390,7 @@ FW_VERSIONS = { b'68503664AC', ], }, - CAR.RAM_1500: { + CAR.RAM_1500_5TH_GEN: { (Ecu.combinationMeter, 0x742, None): [ b'68294051AG', b'68294051AI', @@ -376,10 +407,13 @@ FW_VERSIONS = { b'68434859AC', b'68434860AC', b'68453483AC', + b'68453483AD', b'68453487AD', b'68453491AC', + b'68453491AD', b'68453499AD', b'68453503AC', + b'68453503AD', b'68453505AC', b'68453505AD', b'68453511AC', @@ -401,9 +435,14 @@ FW_VERSIONS = { b'68527383AD', b'68527387AE', b'68527403AC', + b'68527403AD', b'68546047AF', b'68631938AA', + b'68631939AA', + b'68631940AA', + b'68631940AB', b'68631942AA', + b'68631943AB', ], (Ecu.srs, 0x744, None): [ b'68428609AB', @@ -415,8 +454,10 @@ FW_VERSIONS = { b'68615034AA', ], (Ecu.abs, 0x747, None): [ + b'68292406AG', b'68292406AH', b'68432418AB', + b'68432418AC', b'68432418AD', b'68436004AD', b'68436004AE', @@ -455,6 +496,7 @@ FW_VERSIONS = { b'68440789AC', b'68466110AA', b'68466110AB', + b'68466113AA', b'68469901AA', b'68469907AA', b'68522583AA', @@ -466,6 +508,7 @@ FW_VERSIONS = { b'68552790AA', b'68552791AB', b'68552794AA', + b'68552794AD', b'68585106AB', b'68585107AB', b'68585108AB', @@ -474,9 +517,12 @@ FW_VERSIONS = { ], (Ecu.engine, 0x7e0, None): [ b'05035699AG ', + b'05035841AC ', + b'05035841AD ', b'05036026AB ', b'05036065AE ', b'05036066AE ', + b'05036193AA ', b'05149368AA ', b'05149591AD ', b'05149591AE ', @@ -493,6 +539,8 @@ FW_VERSIONS = { b'68378701AI ', b'68378702AI ', b'68378710AL ', + b'68378742AI ', + b'68378742AK ', b'68378748AL ', b'68378758AM ', b'68448163AJ', @@ -506,27 +554,34 @@ FW_VERSIONS = { b'68455145AE ', b'68455146AC ', b'68467915AC ', + b'68467916AC ', b'68467936AC ', b'68500630AD', b'68500630AE', + b'68500631AE', b'68502719AC ', b'68502722AC ', + b'68502733AC ', b'68502734AF ', b'68502740AF ', b'68502741AF ', b'68502742AC ', + b'68502742AF ', b'68539650AD', b'68539650AF', b'68539651AD', b'68586101AA ', b'68586105AB ', + b'68629919AC ', b'68629922AC ', + b'68629925AC ', b'68629926AC ', ], (Ecu.transmission, 0x7e1, None): [ b'05035706AD', b'05035842AB', b'05036069AA', + b'05036181AA', b'05149536AC', b'05149537AC', b'05149543AC', @@ -536,6 +591,8 @@ FW_VERSIONS = { b'68360081AM', b'68360085AJ', b'68360085AL', + b'68360086AH', + b'68360086AK', b'68384328AD', b'68384332AD', b'68445531AC', @@ -548,15 +605,18 @@ FW_VERSIONS = { b'68484467AC', b'68484471AC', b'68502994AD', + b'68502996AD', b'68520867AE', b'68520867AF', b'68520870AC', b'68540431AB', b'68540433AB', + b'68551676AA', + b'68629935AB', b'68629936AC', ], }, - CAR.RAM_HD: { + CAR.RAM_HD_5TH_GEN: { (Ecu.combinationMeter, 0x742, None): [ b'68361606AH', b'68437735AC', diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 32a4f5dfcf..217a1a756c 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -1,10 +1,12 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type + class CarInterface(CarInterfaceBase): @staticmethod @@ -24,21 +26,17 @@ class CarInterface(CarInterfaceBase): elif candidate in RAM_DT: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_CHRYSLER_RAM_DT - ret.minSteerSpeed = 3.8 # m/s CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) if candidate not in RAM_CARS: # Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed. - new_eps_platform = candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, CAR.DODGE_DURANGO) + new_eps_platform = candidate in (CAR.CHRYSLER_PACIFICA_2019_HYBRID, CAR.CHRYSLER_PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, CAR.DODGE_DURANGO) new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw) if new_eps_platform or new_eps_firmware: ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value # Chrysler - if candidate in (CAR.PACIFICA_2017_HYBRID, CAR.PACIFICA_2018, CAR.PACIFICA_2018_HYBRID, CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.DODGE_DURANGO): - ret.mass = 2242. - ret.wheelbase = 3.089 - ret.steerRatio = 16.2 # Pacifica Hybrid 2017 - + if candidate in (CAR.CHRYSLER_PACIFICA_2017_HYBRID, CAR.CHRYSLER_PACIFICA_2018, CAR.CHRYSLER_PACIFICA_2018_HYBRID, \ + CAR.CHRYSLER_PACIFICA_2019_HYBRID, CAR.CHRYSLER_PACIFICA_2020, CAR.DODGE_DURANGO): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]] @@ -46,9 +44,6 @@ class CarInterface(CarInterfaceBase): # Jeep elif candidate in (CAR.JEEP_GRAND_CHEROKEE, CAR.JEEP_GRAND_CHEROKEE_2019): - ret.mass = 1778 - ret.wheelbase = 2.71 - ret.steerRatio = 16.7 ret.steerActuatorDelay = 0.2 ret.lateralTuning.init('pid') @@ -57,22 +52,15 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00006 # Ram - elif candidate == CAR.RAM_1500: + elif candidate == CAR.RAM_1500_5TH_GEN: ret.steerActuatorDelay = 0.2 ret.wheelbase = 3.88 - ret.steerRatio = 16.3 - ret.mass = 2493. - ret.minSteerSpeed = 14.5 # Older EPS FW allow steer to zero if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw): ret.minSteerSpeed = 0. - elif candidate == CAR.RAM_HD: + elif candidate == CAR.RAM_HD_5TH_GEN: ret.steerActuatorDelay = 0.2 - ret.wheelbase = 3.785 - ret.steerRatio = 15.61 - ret.mass = 3405. - ret.minSteerSpeed = 16 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, 1.0, False) else: @@ -91,6 +79,8 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + # events events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low]) @@ -105,6 +95,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 94ff1f1d0f..42ea94cf86 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -1,38 +1,92 @@ -from enum import IntFlag, StrEnum +from enum import IntFlag from dataclasses import dataclass, field -from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu class ChryslerFlags(IntFlag): + # Detected flags HIGHER_MIN_STEERING_SPEED = 1 +@dataclass +class ChryslerCarDocs(CarDocs): + package: str = "Adaptive Cruise Control (ACC)" + car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca])) + + +@dataclass +class ChryslerPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion')) + + +@dataclass(frozen=True) +class ChryslerCarSpecs(CarSpecs): + minSteerSpeed: float = 3.8 # m/s -class CAR(StrEnum): + +class CAR(Platforms): # Chrysler - PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" - PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" - PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" - PACIFICA_2018 = "CHRYSLER PACIFICA 2018" - PACIFICA_2020 = "CHRYSLER PACIFICA 2020" + CHRYSLER_PACIFICA_2017_HYBRID = ChryslerPlatformConfig( + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2017")], + ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2), + ) + CHRYSLER_PACIFICA_2018_HYBRID = ChryslerPlatformConfig( + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2018")], + CHRYSLER_PACIFICA_2017_HYBRID.specs, + ) + CHRYSLER_PACIFICA_2019_HYBRID = ChryslerPlatformConfig( + [ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-23")], + CHRYSLER_PACIFICA_2017_HYBRID.specs, + ) + CHRYSLER_PACIFICA_2018 = ChryslerPlatformConfig( + [ChryslerCarDocs("Chrysler Pacifica 2017-18")], + CHRYSLER_PACIFICA_2017_HYBRID.specs, + ) + CHRYSLER_PACIFICA_2020 = ChryslerPlatformConfig( + [ + ChryslerCarDocs("Chrysler Pacifica 2019-20"), + ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"), + ], + CHRYSLER_PACIFICA_2017_HYBRID.specs, + ) # Dodge - DODGE_DURANGO = "DODGE DURANGO 2021" + DODGE_DURANGO = ChryslerPlatformConfig( + [ChryslerCarDocs("Dodge Durango 2020-21")], + CHRYSLER_PACIFICA_2017_HYBRID.specs, + ) # Jeep - JEEP_GRAND_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # includes 2017 Trailhawk - JEEP_GRAND_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" # includes 2020 Trailhawk + JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk + [ChryslerCarDocs("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk")], + ChryslerCarSpecs(mass=1778., wheelbase=2.71, steerRatio=16.7), + ) + + JEEP_GRAND_CHEROKEE_2019 = ChryslerPlatformConfig( # includes 2020 Trailhawk + [ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4")], + JEEP_GRAND_CHEROKEE.specs, + ) # Ram - RAM_1500 = "RAM 1500 5TH GEN" - RAM_HD = "RAM HD 5TH GEN" + RAM_1500_5TH_GEN = ChryslerPlatformConfig( + [ChryslerCarDocs("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram]))], + ChryslerCarSpecs(mass=2493., wheelbase=3.88, steerRatio=16.3, minSteerSpeed=14.5), + dbc_dict('chrysler_ram_dt_generated', None), + ) + RAM_HD_5TH_GEN = ChryslerPlatformConfig( + [ + ChryslerCarDocs("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])), + ChryslerCarDocs("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])), + ], + ChryslerCarSpecs(mass=3405., wheelbase=3.785, steerRatio=15.61, minSteerSpeed=16.), + dbc_dict('chrysler_ram_hd_generated', None), + ) class CarControllerParams: @@ -55,37 +109,11 @@ class CarControllerParams: STEER_THRESHOLD = 120 -RAM_DT = {CAR.RAM_1500, } -RAM_HD = {CAR.RAM_HD, } +RAM_DT = {CAR.RAM_1500_5TH_GEN, } +RAM_HD = {CAR.RAM_HD_5TH_GEN, } RAM_CARS = RAM_DT | RAM_HD -@dataclass -class ChryslerCarInfo(CarInfo): - package: str = "Adaptive Cruise Control (ACC)" - car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca])) - - -CAR_INFO: Dict[str, Optional[Union[ChryslerCarInfo, List[ChryslerCarInfo]]]] = { - CAR.PACIFICA_2017_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2017"), - CAR.PACIFICA_2018_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2018"), - CAR.PACIFICA_2019_HYBRID: ChryslerCarInfo("Chrysler Pacifica Hybrid 2019-23"), - CAR.PACIFICA_2018: ChryslerCarInfo("Chrysler Pacifica 2017-18"), - CAR.PACIFICA_2020: [ - ChryslerCarInfo("Chrysler Pacifica 2019-20"), - ChryslerCarInfo("Chrysler Pacifica 2021-23", package="All"), - ], - CAR.JEEP_GRAND_CHEROKEE: ChryslerCarInfo("Jeep Grand Cherokee 2016-18", video_link="https://www.youtube.com/watch?v=eLR9o2JkuRk"), - CAR.JEEP_GRAND_CHEROKEE_2019: ChryslerCarInfo("Jeep Grand Cherokee 2019-21", video_link="https://www.youtube.com/watch?v=jBe4lWnRSu4"), - CAR.DODGE_DURANGO: ChryslerCarInfo("Dodge Durango 2020-21"), - CAR.RAM_1500: ChryslerCarInfo("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram])), - CAR.RAM_HD: [ - ChryslerCarInfo("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])), - ChryslerCarInfo("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])), - ], -} - - CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xf132) CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ @@ -125,16 +153,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.PACIFICA_2017_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2018: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2020: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2018_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.PACIFICA_2019_HYBRID: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.DODGE_DURANGO: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.JEEP_GRAND_CHEROKEE: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.JEEP_GRAND_CHEROKEE_2019: dbc_dict('chrysler_pacifica_2017_hybrid_generated', 'chrysler_pacifica_2017_hybrid_private_fusion'), - CAR.RAM_1500: dbc_dict('chrysler_ram_dt_generated', None), - CAR.RAM_HD: dbc_dict('chrysler_ram_hd_generated', None), -} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py new file mode 100755 index 0000000000..7bf6a6ad22 --- /dev/null +++ b/selfdrive/car/docs.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 +import argparse +from collections import defaultdict +import jinja2 +import os +from enum import Enum +from natsort import natsorted + +from cereal import car +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car import gen_empty_fingerprint +from openpilot.selfdrive.car.docs_definitions import CarDocs, Column, CommonFootnote, PartType +from openpilot.selfdrive.car.car_helpers import interfaces, get_interface_attr +from openpilot.selfdrive.car.values import PLATFORMS + + +def get_all_footnotes() -> dict[Enum, int]: + all_footnotes = list(CommonFootnote) + for footnotes in get_interface_attr("Footnote", ignore_none=True).values(): + all_footnotes.extend(footnotes) + return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} + + +CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") +CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") + + +def get_all_car_docs() -> list[CarDocs]: + all_car_docs: list[CarDocs] = [] + footnotes = get_all_footnotes() + for model, platform in PLATFORMS.items(): + car_docs = platform.config.car_docs + # If available, uses experimental longitudinal limits for the docs + CP = interfaces[model][0].get_params(platform, fingerprint=gen_empty_fingerprint(), + car_fw=[car.CarParams.CarFw(ecu="unknown")], experimental_long=True, docs=True) + + if CP.dashcamOnly or not len(car_docs): + continue + + # A platform can include multiple car models + for _car_docs in car_docs: + if not hasattr(_car_docs, "row"): + _car_docs.init_make(CP) + _car_docs.init(CP, footnotes) + all_car_docs.append(_car_docs) + + # Sort cars by make and model + year + sorted_cars: list[CarDocs] = natsorted(all_car_docs, key=lambda car: car.name.lower()) + return sorted_cars + + +def group_by_make(all_car_docs: list[CarDocs]) -> dict[str, list[CarDocs]]: + sorted_car_docs = defaultdict(list) + for car_docs in all_car_docs: + sorted_car_docs[car_docs.make].append(car_docs) + return dict(sorted_car_docs) + + +def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str) -> str: + with open(template_fn) as f: + template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True) + + footnotes = [fn.value.text for fn in get_all_footnotes()] + cars_md: str = template.render(all_car_docs=all_car_docs, PartType=PartType, + group_by_make=group_by_make, footnotes=footnotes, + Column=Column) + return cars_md + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("--template", default=CARS_MD_TEMPLATE, help="Override default template filename") + parser.add_argument("--out", default=CARS_MD_OUT, help="Override default generated filename") + args = parser.parse_args() + + with open(args.out, 'w') as f: + f.write(generate_cars_md(get_all_car_docs(), args.template)) + print(f"Generated and written to {args.out}") diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index f2ce743607..971338e9b5 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -3,7 +3,6 @@ from collections import namedtuple import copy from dataclasses import dataclass, field from enum import Enum -from typing import Dict, List, Optional, Tuple, Union from cereal import car from openpilot.common.conversions import Conversions as CV @@ -35,7 +34,7 @@ class Star(Enum): @dataclass class BasePart: name: str - parts: List[Enum] = field(default_factory=list) + parts: list[Enum] = field(default_factory=list) def all_parts(self): # Recursively get all parts @@ -76,7 +75,7 @@ class Accessory(EnumBase): @dataclass class BaseCarHarness(BasePart): - parts: List[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft]) + parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft]) has_connector: bool = True # without are hidden on the harness connector page @@ -119,7 +118,8 @@ class CarHarness(EnumBase): nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Cable.rj45_cable_7ft, Cable.long_obdc_cable, Cable.usbc_coupler]) mazda = BaseCarHarness("Mazda connector") ford_q3 = BaseCarHarness("Ford Q3 connector") - ford_q4 = BaseCarHarness("Ford Q4 connector") + ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power_v2, Cable.rj45_cable_7ft, Cable.long_obdc_cable, + Cable.usbc_coupler]) class Device(EnumBase): @@ -149,18 +149,18 @@ class PartType(Enum): tool = Tool -DEFAULT_CAR_PARTS: List[EnumBase] = [Device.threex] +DEFAULT_CAR_PARTS: list[EnumBase] = [Device.threex] @dataclass class CarParts: - parts: List[EnumBase] = field(default_factory=list) + parts: list[EnumBase] = field(default_factory=list) def __call__(self): return copy.deepcopy(self) @classmethod - def common(cls, add: Optional[List[EnumBase]] = None, remove: Optional[List[EnumBase]] = None): + def common(cls, add: list[EnumBase] = None, remove: list[EnumBase] = None): p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])] return cls(p) @@ -186,7 +186,7 @@ class CommonFootnote(Enum): Column.LONGITUDINAL) -def get_footnotes(footnotes: List[Enum], column: Column) -> List[Enum]: +def get_footnotes(footnotes: list[Enum], column: Column) -> list[Enum]: # Returns applicable footnotes given current column return [fn for fn in footnotes if fn.value.column == column] @@ -209,7 +209,7 @@ def get_year_list(years): return years_list -def split_name(name: str) -> Tuple[str, str, str]: +def split_name(name: str) -> tuple[str, str, str]: make, model = name.split(" ", 1) years = "" match = re.search(MODEL_YEARS_RE, model) @@ -220,7 +220,7 @@ def split_name(name: str) -> Tuple[str, str, str]: @dataclass -class CarInfo: +class CarDocs: # make + model + model years name: str @@ -233,13 +233,13 @@ class CarInfo: # the minimum compatibility requirements for this model, regardless # of market. can be a package, trim, or list of features - requirements: Optional[str] = None + requirements: str | None = None - video_link: Optional[str] = None - footnotes: List[Enum] = field(default_factory=list) - min_steer_speed: Optional[float] = None - min_enable_speed: Optional[float] = None - auto_resume: Optional[bool] = None + video_link: str | None = None + footnotes: list[Enum] = field(default_factory=list) + min_steer_speed: float | None = None + min_enable_speed: float | None = None + auto_resume: bool | None = None # all the parts needed for the supported car car_parts: CarParts = field(default_factory=CarParts) @@ -248,7 +248,7 @@ class CarInfo: self.make, self.model, self.years = split_name(self.name) self.year_list = get_year_list(self.years) - def init(self, CP: car.CarParams, all_footnotes: Dict[Enum, int]): + def init(self, CP: car.CarParams, all_footnotes: dict[Enum, int]): self.car_name = CP.carName self.car_fingerprint = CP.carFingerprint @@ -266,7 +266,7 @@ class CarInfo: # min steer & enable speed columns # TODO: set all the min steer speeds in carParams and remove this if self.min_steer_speed is not None: - assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams" + assert CP.minSteerSpeed < 0.5, f"{CP.carFingerprint}: Minimum steer speed set in both CarDocs and CarParams" else: self.min_steer_speed = CP.minSteerSpeed @@ -275,7 +275,7 @@ class CarInfo: self.min_enable_speed = CP.minEnableSpeed if self.auto_resume is None: - self.auto_resume = CP.autoResumeSng + self.auto_resume = CP.autoResumeSng and self.min_enable_speed <= 0 # hardware column hardware_col = "None" @@ -293,7 +293,7 @@ class CarInfo: if len(tools_docs): hardware_col += f'
    Tools{display_func(tools_docs)}
    ' - self.row: Dict[Enum, Union[str, Star]] = { + self.row: dict[Enum, str | Star] = { Column.MAKE: self.make, Column.MODEL: self.model, Column.PACKAGE: self.package, @@ -317,7 +317,7 @@ class CarInfo: return self def init_make(self, CP: car.CarParams): - """CarInfo subclasses can add make-specific logic for harness selection, footnotes, etc.""" + """CarDocs subclasses can add make-specific logic for harness selection, footnotes, etc.""" def get_detail_sentence(self, CP): if not CP.notCar: @@ -340,19 +340,19 @@ class CarInfo: # experimental mode exp_link = "Experimental mode" - if CP.openpilotLongitudinalControl or CP.experimentalLongitudinalAvailable: + if CP.openpilotLongitudinalControl and not CP.experimentalLongitudinalAvailable: sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}." return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc) else: - if CP.carFingerprint == "COMMA BODY": + if CP.carFingerprint == "COMMA_BODY": return "The body is a robotics dev kit that can run openpilot. Learn more." else: raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}") def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str: - item: Union[str, Star] = self.row[column] + item: str | Star = self.row[column] if isinstance(item, Star): item = star_icon.format(item.value) elif column == Column.MODEL and len(self.years): diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 13f7926def..e7a9fbcf2c 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -1,13 +1,12 @@ #!/usr/bin/env python3 import capnp import time -from typing import Optional, Set import cereal.messaging as messaging from panda.python.uds import SERVICE_TYPE from openpilot.selfdrive.car import make_can_msg from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType -from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp +from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.common.swaglog import cloudlog @@ -20,7 +19,7 @@ def make_tester_present_msg(addr, bus, subaddr=None): return make_can_msg(addr, bytes(dat), bus) -def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool: +def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: int = None) -> bool: # ISO-TP messages are always padded to 8 bytes # tester present response is always a single frame dat_offset = 1 if subaddr is not None else 0 @@ -34,16 +33,16 @@ def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subadd return False -def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[EcuAddrBusType]: +def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]: addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] - queries: Set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} + queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} responses = queries return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) -def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[EcuAddrBusType], - responses: Set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> Set[EcuAddrBusType]: - ecu_responses: Set[EcuAddrBusType] = set() # set((addr, subaddr, bus),) +def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: set[EcuAddrBusType], + responses: set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> set[EcuAddrBusType]: + ecu_responses: set[EcuAddrBusType] = set() # set((addr, subaddr, bus),) try: msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries] diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py index 6a7c3c75be..1128a31c29 100644 --- a/selfdrive/car/fingerprints.py +++ b/selfdrive/car/fingerprints.py @@ -1,4 +1,17 @@ from openpilot.selfdrive.car.interfaces import get_interface_attr +from openpilot.selfdrive.car.body.values import CAR as BODY +from openpilot.selfdrive.car.chrysler.values import CAR as CHRYSLER +from openpilot.selfdrive.car.ford.values import CAR as FORD +from openpilot.selfdrive.car.gm.values import CAR as GM +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI +from openpilot.selfdrive.car.mazda.values import CAR as MAZDA +from openpilot.selfdrive.car.mock.values import CAR as MOCK +from openpilot.selfdrive.car.nissan.values import CAR as NISSAN +from openpilot.selfdrive.car.subaru.values import CAR as SUBARU +from openpilot.selfdrive.car.tesla.values import CAR as TESLA +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.volkswagen.values import CAR as VW FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True) _FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True) @@ -44,3 +57,291 @@ def all_known_cars(): def all_legacy_fingerprint_cars(): """Returns a list of all known car strings, FPv1 only.""" return list(_FINGERPRINTS.keys()) + + +# A dict that maps old platform strings to their latest representations +MIGRATION = { + "ACURA ILX 2016 ACURAWATCH PLUS": HONDA.ACURA_ILX, + "ACURA RDX 2018 ACURAWATCH PLUS": HONDA.ACURA_RDX, + "ACURA RDX 2020 TECH": HONDA.ACURA_RDX_3G, + "AUDI A3": VW.AUDI_A3_MK3, + "HONDA ACCORD 2018 HYBRID TOURING": HONDA.HONDA_ACCORD, + "HONDA ACCORD 1.5T 2018": HONDA.HONDA_ACCORD, + "HONDA ACCORD 2018 LX 1.5T": HONDA.HONDA_ACCORD, + "HONDA ACCORD 2018 SPORT 2T": HONDA.HONDA_ACCORD, + "HONDA ACCORD 2T 2018": HONDA.HONDA_ACCORD, + "HONDA ACCORD HYBRID 2018": HONDA.HONDA_ACCORD, + "HONDA CIVIC 2016 TOURING": HONDA.HONDA_CIVIC, + "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.HONDA_CIVIC_BOSCH, + "HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.HONDA_CIVIC_BOSCH_DIESEL, + "HONDA CR-V 2016 EXECUTIVE": HONDA.HONDA_CRV_EU, + "HONDA CR-V 2016 TOURING": HONDA.HONDA_CRV, + "HONDA CR-V 2017 EX": HONDA.HONDA_CRV_5G, + "HONDA CR-V 2019 HYBRID": HONDA.HONDA_CRV_HYBRID, + "HONDA FIT 2018 EX": HONDA.HONDA_FIT, + "HONDA HRV 2019 TOURING": HONDA.HONDA_HRV, + "HONDA INSIGHT 2019 TOURING": HONDA.HONDA_INSIGHT, + "HONDA ODYSSEY 2018 EX-L": HONDA.HONDA_ODYSSEY, + "HONDA ODYSSEY 2019 EXCLUSIVE CHN": HONDA.HONDA_ODYSSEY_CHN, + "HONDA PILOT 2017 TOURING": HONDA.HONDA_PILOT, + "HONDA PILOT 2019 ELITE": HONDA.HONDA_PILOT, + "HONDA PILOT 2019": HONDA.HONDA_PILOT, + "HONDA PASSPORT 2021": HONDA.HONDA_PILOT, + "HONDA RIDGELINE 2017 BLACK EDITION": HONDA.HONDA_RIDGELINE, + "HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.HYUNDAI_ELANTRA, + "HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.HYUNDAI_SANTA_FE, + "HYUNDAI TUCSON DIESEL 2019": HYUNDAI.HYUNDAI_TUCSON, + "KIA OPTIMA 2016": HYUNDAI.KIA_OPTIMA_G4, + "KIA OPTIMA 2019": HYUNDAI.KIA_OPTIMA_G4_FL, + "KIA OPTIMA SX 2019 & 2016": HYUNDAI.KIA_OPTIMA_G4_FL, + "LEXUS CT 200H 2018": TOYOTA.LEXUS_CTH, + "LEXUS ES 300H 2018": TOYOTA.LEXUS_ES, + "LEXUS ES 300H 2019": TOYOTA.LEXUS_ES_TSS2, + "LEXUS IS300 2018": TOYOTA.LEXUS_IS, + "LEXUS NX300 2018": TOYOTA.LEXUS_NX, + "LEXUS NX300H 2018": TOYOTA.LEXUS_NX, + "LEXUS RX 350 2016": TOYOTA.LEXUS_RX, + "LEXUS RX350 2020": TOYOTA.LEXUS_RX_TSS2, + "LEXUS RX450 HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, + "TOYOTA SIENNA XLE 2018": TOYOTA.TOYOTA_SIENNA, + "TOYOTA C-HR HYBRID 2018": TOYOTA.TOYOTA_CHR, + "TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2, + "TOYOTA RAV4 HYBRID 2019": TOYOTA.TOYOTA_RAV4_TSS2, + "LEXUS ES HYBRID 2019": TOYOTA.LEXUS_ES_TSS2, + "LEXUS NX HYBRID 2018": TOYOTA.LEXUS_NX, + "LEXUS NX HYBRID 2020": TOYOTA.LEXUS_NX_TSS2, + "LEXUS RX HYBRID 2020": TOYOTA.LEXUS_RX_TSS2, + "TOYOTA ALPHARD HYBRID 2021": TOYOTA.TOYOTA_ALPHARD_TSS2, + "TOYOTA AVALON HYBRID 2019": TOYOTA.TOYOTA_AVALON_2019, + "TOYOTA AVALON HYBRID 2022": TOYOTA.TOYOTA_AVALON_TSS2, + "TOYOTA CAMRY HYBRID 2018": TOYOTA.TOYOTA_CAMRY, + "TOYOTA CAMRY HYBRID 2021": TOYOTA.TOYOTA_CAMRY_TSS2, + "TOYOTA C-HR HYBRID 2022": TOYOTA.TOYOTA_CHR_TSS2, + "TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2, + "TOYOTA RAV4 HYBRID 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022, + "TOYOTA RAV4 HYBRID 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023, + "TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.TOYOTA_HIGHLANDER, + "LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES, + "LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX, + "HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, + "KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, + "KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, + "CADILLAC ESCALADE ESV PLATINUM 2019": GM.CADILLAC_ESCALADE_ESV_2019, + + # Removal of platform_str, see https://github.com/commaai/openpilot/pull/31868/ + "COMMA BODY": BODY.COMMA_BODY, + "CHRYSLER PACIFICA HYBRID 2017": CHRYSLER.CHRYSLER_PACIFICA_2017_HYBRID, + "CHRYSLER PACIFICA HYBRID 2018": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID, + "CHRYSLER PACIFICA HYBRID 2019": CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID, + "CHRYSLER PACIFICA 2018": CHRYSLER.CHRYSLER_PACIFICA_2018, + "CHRYSLER PACIFICA 2020": CHRYSLER.CHRYSLER_PACIFICA_2020, + "DODGE DURANGO 2021": CHRYSLER.DODGE_DURANGO, + "JEEP GRAND CHEROKEE V6 2018": CHRYSLER.JEEP_GRAND_CHEROKEE, + "JEEP GRAND CHEROKEE 2019": CHRYSLER.JEEP_GRAND_CHEROKEE_2019, + "RAM 1500 5TH GEN": CHRYSLER.RAM_1500_5TH_GEN, + "RAM HD 5TH GEN": CHRYSLER.RAM_HD_5TH_GEN, + "FORD BRONCO SPORT 1ST GEN": FORD.FORD_BRONCO_SPORT_MK1, + "FORD ESCAPE 4TH GEN": FORD.FORD_ESCAPE_MK4, + "FORD EXPLORER 6TH GEN": FORD.FORD_EXPLORER_MK6, + "FORD F-150 14TH GEN": FORD.FORD_F_150_MK14, + "FORD F-150 LIGHTNING 1ST GEN": FORD.FORD_F_150_LIGHTNING_MK1, + "FORD FOCUS 4TH GEN": FORD.FORD_FOCUS_MK4, + "FORD MAVERICK 1ST GEN": FORD.FORD_MAVERICK_MK1, + "FORD MUSTANG MACH-E 1ST GEN": FORD.FORD_MUSTANG_MACH_E_MK1, + "HOLDEN ASTRA RS-V BK 2017": GM.HOLDEN_ASTRA, + "CHEVROLET VOLT PREMIER 2017": GM.CHEVROLET_VOLT, + "CADILLAC ATS Premium Performance 2018": GM.CADILLAC_ATS, + "CHEVROLET MALIBU PREMIER 2017": GM.CHEVROLET_MALIBU, + "GMC ACADIA DENALI 2018": GM.GMC_ACADIA, + "BUICK LACROSSE 2017": GM.BUICK_LACROSSE, + "BUICK REGAL ESSENCE 2018": GM.BUICK_REGAL, + "CADILLAC ESCALADE 2017": GM.CADILLAC_ESCALADE, + "CADILLAC ESCALADE ESV 2016": GM.CADILLAC_ESCALADE_ESV, + "CADILLAC ESCALADE ESV 2019": GM.CADILLAC_ESCALADE_ESV_2019, + "CHEVROLET BOLT EUV 2022": GM.CHEVROLET_BOLT_EUV, + "CHEVROLET SILVERADO 1500 2020": GM.CHEVROLET_SILVERADO, + "CHEVROLET EQUINOX 2019": GM.CHEVROLET_EQUINOX, + "CHEVROLET TRAILBLAZER 2021": GM.CHEVROLET_TRAILBLAZER, + "HONDA ACCORD 2018": HONDA.HONDA_ACCORD, + "HONDA CIVIC (BOSCH) 2019": HONDA.HONDA_CIVIC_BOSCH, + "HONDA CIVIC SEDAN 1.6 DIESEL 2019": HONDA.HONDA_CIVIC_BOSCH_DIESEL, + "HONDA CIVIC 2022": HONDA.HONDA_CIVIC_2022, + "HONDA CR-V 2017": HONDA.HONDA_CRV_5G, + "HONDA CR-V HYBRID 2019": HONDA.HONDA_CRV_HYBRID, + "HONDA HR-V 2023": HONDA.HONDA_HRV_3G, + "ACURA RDX 2020": HONDA.ACURA_RDX_3G, + "HONDA INSIGHT 2019": HONDA.HONDA_INSIGHT, + "HONDA E 2020": HONDA.HONDA_E, + "ACURA ILX 2016": HONDA.ACURA_ILX, + "HONDA CR-V 2016": HONDA.HONDA_CRV, + "HONDA CR-V EU 2016": HONDA.HONDA_CRV_EU, + "HONDA FIT 2018": HONDA.HONDA_FIT, + "HONDA FREED 2020": HONDA.HONDA_FREED, + "HONDA HRV 2019": HONDA.HONDA_HRV, + "HONDA ODYSSEY 2018": HONDA.HONDA_ODYSSEY, + "HONDA ODYSSEY CHN 2019": HONDA.HONDA_ODYSSEY_CHN, + "ACURA RDX 2018": HONDA.ACURA_RDX, + "HONDA PILOT 2017": HONDA.HONDA_PILOT, + "HONDA RIDGELINE 2017": HONDA.HONDA_RIDGELINE, + "HONDA CIVIC 2016": HONDA.HONDA_CIVIC, + "HYUNDAI AZERA 6TH GEN": HYUNDAI.HYUNDAI_AZERA_6TH_GEN, + "HYUNDAI AZERA HYBRID 6TH GEN": HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN, + "HYUNDAI ELANTRA 2017": HYUNDAI.HYUNDAI_ELANTRA, + "HYUNDAI I30 N LINE 2019 & GT 2018 DCT": HYUNDAI.HYUNDAI_ELANTRA_GT_I30, + "HYUNDAI ELANTRA 2021": HYUNDAI.HYUNDAI_ELANTRA_2021, + "HYUNDAI ELANTRA HYBRID 2021": HYUNDAI.HYUNDAI_ELANTRA_HEV_2021, + "HYUNDAI GENESIS 2015-2016": HYUNDAI.HYUNDAI_GENESIS, + "HYUNDAI IONIQ HYBRID 2017-2019": HYUNDAI.HYUNDAI_IONIQ, + "HYUNDAI IONIQ HYBRID 2020-2022": HYUNDAI.HYUNDAI_IONIQ_HEV_2022, + "HYUNDAI IONIQ ELECTRIC LIMITED 2019": HYUNDAI.HYUNDAI_IONIQ_EV_LTD, + "HYUNDAI IONIQ ELECTRIC 2020": HYUNDAI.HYUNDAI_IONIQ_EV_2020, + "HYUNDAI IONIQ PLUG-IN HYBRID 2019": HYUNDAI.HYUNDAI_IONIQ_PHEV_2019, + "HYUNDAI IONIQ PHEV 2020": HYUNDAI.HYUNDAI_IONIQ_PHEV, + "HYUNDAI KONA 2020": HYUNDAI.HYUNDAI_KONA, + "HYUNDAI KONA ELECTRIC 2019": HYUNDAI.HYUNDAI_KONA_EV, + "HYUNDAI KONA ELECTRIC 2022": HYUNDAI.HYUNDAI_KONA_EV_2022, + "HYUNDAI KONA ELECTRIC 2ND GEN": HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN, + "HYUNDAI KONA HYBRID 2020": HYUNDAI.HYUNDAI_KONA_HEV, + "HYUNDAI SANTA FE 2019": HYUNDAI.HYUNDAI_SANTA_FE, + "HYUNDAI SANTA FE 2022": HYUNDAI.HYUNDAI_SANTA_FE_2022, + "HYUNDAI SANTA FE HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022, + "HYUNDAI SANTA FE PlUG-IN HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022, + "HYUNDAI SONATA 2020": HYUNDAI.HYUNDAI_SONATA, + "HYUNDAI SONATA 2019": HYUNDAI.HYUNDAI_SONATA_LF, + "HYUNDAI STARIA 4TH GEN": HYUNDAI.HYUNDAI_STARIA_4TH_GEN, + "HYUNDAI TUCSON 2019": HYUNDAI.HYUNDAI_TUCSON, + "HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE, + "HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER, + "HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID, + "HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5, + "HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6, + "HYUNDAI TUCSON 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN, + "HYUNDAI SANTA CRUZ 1ST GEN": HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, + "HYUNDAI CUSTIN 1ST GEN": HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN, + "KIA FORTE E 2018 & GT 2021": HYUNDAI.KIA_FORTE, + "KIA K5 2021": HYUNDAI.KIA_K5_2021, + "KIA K5 HYBRID 2020": HYUNDAI.KIA_K5_HEV_2020, + "KIA K8 HYBRID 1ST GEN": HYUNDAI.KIA_K8_HEV_1ST_GEN, + "KIA NIRO EV 2020": HYUNDAI.KIA_NIRO_EV, + "KIA NIRO EV 2ND GEN": HYUNDAI.KIA_NIRO_EV_2ND_GEN, + "KIA NIRO HYBRID 2019": HYUNDAI.KIA_NIRO_PHEV, + "KIA NIRO PLUG-IN HYBRID 2022": HYUNDAI.KIA_NIRO_PHEV_2022, + "KIA NIRO HYBRID 2021": HYUNDAI.KIA_NIRO_HEV_2021, + "KIA NIRO HYBRID 2ND GEN": HYUNDAI.KIA_NIRO_HEV_2ND_GEN, + "KIA OPTIMA 4TH GEN": HYUNDAI.KIA_OPTIMA_G4, + "KIA OPTIMA 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_G4_FL, + "KIA OPTIMA HYBRID 2017 & SPORTS 2019": HYUNDAI.KIA_OPTIMA_H, + "KIA OPTIMA HYBRID 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_H_G4_FL, + "KIA SELTOS 2021": HYUNDAI.KIA_SELTOS, + "KIA SPORTAGE 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN, + "KIA SORENTO GT LINE 2018": HYUNDAI.KIA_SORENTO, + "KIA SORENTO 4TH GEN": HYUNDAI.KIA_SORENTO_4TH_GEN, + "KIA SORENTO HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, + "KIA STINGER GT2 2018": HYUNDAI.KIA_STINGER, + "KIA STINGER 2022": HYUNDAI.KIA_STINGER_2022, + "KIA CEED INTRO ED 2019": HYUNDAI.KIA_CEED, + "KIA EV6 2022": HYUNDAI.KIA_EV6, + "KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN, + "GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN, + "GENESIS G70 2018": HYUNDAI.GENESIS_G70, + "GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020, + "GENESIS GV70 1ST GEN": HYUNDAI.GENESIS_GV70_1ST_GEN, + "GENESIS G80 2017": HYUNDAI.GENESIS_G80, + "GENESIS G90 2017": HYUNDAI.GENESIS_G90, + "GENESIS GV80 2023": HYUNDAI.GENESIS_GV80, + "MAZDA CX-5": MAZDA.MAZDA_CX5, + "MAZDA CX-9": MAZDA.MAZDA_CX9, + "MAZDA 3": MAZDA.MAZDA_3, + "MAZDA 6": MAZDA.MAZDA_6, + "MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021, + "MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022, + "NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL, + "NISSAN LEAF 2018": NISSAN.NISSAN_LEAF, + "NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC, + "NISSAN ROGUE 2019": NISSAN.NISSAN_ROGUE, + "NISSAN ALTIMA 2020": NISSAN.NISSAN_ALTIMA, + "SUBARU ASCENT LIMITED 2019": SUBARU.SUBARU_ASCENT, + "SUBARU OUTBACK 6TH GEN": SUBARU.SUBARU_OUTBACK, + "SUBARU LEGACY 7TH GEN": SUBARU.SUBARU_LEGACY, + "SUBARU IMPREZA LIMITED 2019": SUBARU.SUBARU_IMPREZA, + "SUBARU IMPREZA SPORT 2020": SUBARU.SUBARU_IMPREZA_2020, + "SUBARU CROSSTREK HYBRID 2020": SUBARU.SUBARU_CROSSTREK_HYBRID, + "SUBARU FORESTER 2019": SUBARU.SUBARU_FORESTER, + "SUBARU FORESTER HYBRID 2020": SUBARU.SUBARU_FORESTER_HYBRID, + "SUBARU FORESTER 2017 - 2018": SUBARU.SUBARU_FORESTER_PREGLOBAL, + "SUBARU LEGACY 2015 - 2018": SUBARU.SUBARU_LEGACY_PREGLOBAL, + "SUBARU OUTBACK 2015 - 2017": SUBARU.SUBARU_OUTBACK_PREGLOBAL, + "SUBARU OUTBACK 2018 - 2019": SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018, + "SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022, + "SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023, + "SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023, + 'TESLA AP1 MODEL S': TESLA.TESLA_AP1_MODELS, + 'TESLA AP2 MODEL S': TESLA.TESLA_AP2_MODELS, + 'TESLA MODEL S RAVEN': TESLA.TESLA_MODELS_RAVEN, + "TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2, + "TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON, + "TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019, + "TOYOTA AVALON 2022": TOYOTA.TOYOTA_AVALON_TSS2, + "TOYOTA CAMRY 2018": TOYOTA.TOYOTA_CAMRY, + "TOYOTA CAMRY 2021": TOYOTA.TOYOTA_CAMRY_TSS2, + "TOYOTA C-HR 2018": TOYOTA.TOYOTA_CHR, + "TOYOTA C-HR 2021": TOYOTA.TOYOTA_CHR_TSS2, + "TOYOTA COROLLA 2017": TOYOTA.TOYOTA_COROLLA, + "TOYOTA COROLLA TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2, + "TOYOTA HIGHLANDER 2017": TOYOTA.TOYOTA_HIGHLANDER, + "TOYOTA HIGHLANDER 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2, + "TOYOTA PRIUS 2017": TOYOTA.TOYOTA_PRIUS, + "TOYOTA PRIUS v 2017": TOYOTA.TOYOTA_PRIUS_V, + "TOYOTA PRIUS TSS2 2021": TOYOTA.TOYOTA_PRIUS_TSS2, + "TOYOTA RAV4 2017": TOYOTA.TOYOTA_RAV4, + "TOYOTA RAV4 HYBRID 2017": TOYOTA.TOYOTA_RAV4H, + "TOYOTA RAV4 2019": TOYOTA.TOYOTA_RAV4_TSS2, + "TOYOTA RAV4 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022, + "TOYOTA RAV4 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023, + "TOYOTA MIRAI 2021": TOYOTA.TOYOTA_MIRAI, + "TOYOTA SIENNA 2018": TOYOTA.TOYOTA_SIENNA, + "LEXUS CT HYBRID 2018": TOYOTA.LEXUS_CTH, + "LEXUS ES 2018": TOYOTA.LEXUS_ES, + "LEXUS ES 2019": TOYOTA.LEXUS_ES_TSS2, + "LEXUS IS 2018": TOYOTA.LEXUS_IS, + "LEXUS IS 2023": TOYOTA.LEXUS_IS_TSS2, + "LEXUS NX 2018": TOYOTA.LEXUS_NX, + "LEXUS NX 2020": TOYOTA.LEXUS_NX_TSS2, + "LEXUS LC 2024": TOYOTA.LEXUS_LC_TSS2, + "LEXUS RC 2020": TOYOTA.LEXUS_RC, + "LEXUS RX 2016": TOYOTA.LEXUS_RX, + "LEXUS RX 2020": TOYOTA.LEXUS_RX_TSS2, + "LEXUS GS F 2016": TOYOTA.LEXUS_GS_F, + "VOLKSWAGEN ARTEON 1ST GEN": VW.VOLKSWAGEN_ARTEON_MK1, + "VOLKSWAGEN ATLAS 1ST GEN": VW.VOLKSWAGEN_ATLAS_MK1, + "VOLKSWAGEN CADDY 3RD GEN": VW.VOLKSWAGEN_CADDY_MK3, + "VOLKSWAGEN CRAFTER 2ND GEN": VW.VOLKSWAGEN_CRAFTER_MK2, + "VOLKSWAGEN GOLF 7TH GEN": VW.VOLKSWAGEN_GOLF_MK7, + "VOLKSWAGEN JETTA 7TH GEN": VW.VOLKSWAGEN_JETTA_MK7, + "VOLKSWAGEN PASSAT 8TH GEN": VW.VOLKSWAGEN_PASSAT_MK8, + "VOLKSWAGEN PASSAT NMS": VW.VOLKSWAGEN_PASSAT_NMS, + "VOLKSWAGEN POLO 6TH GEN": VW.VOLKSWAGEN_POLO_MK6, + "VOLKSWAGEN SHARAN 2ND GEN": VW.VOLKSWAGEN_SHARAN_MK2, + "VOLKSWAGEN TAOS 1ST GEN": VW.VOLKSWAGEN_TAOS_MK1, + "VOLKSWAGEN T-CROSS 1ST GEN": VW.VOLKSWAGEN_TCROSS_MK1, + "VOLKSWAGEN TIGUAN 2ND GEN": VW.VOLKSWAGEN_TIGUAN_MK2, + "VOLKSWAGEN TOURAN 2ND GEN": VW.VOLKSWAGEN_TOURAN_MK2, + "VOLKSWAGEN TRANSPORTER T6.1": VW.VOLKSWAGEN_TRANSPORTER_T61, + "VOLKSWAGEN T-ROC 1ST GEN": VW.VOLKSWAGEN_TROC_MK1, + "AUDI A3 3RD GEN": VW.AUDI_A3_MK3, + "AUDI Q2 1ST GEN": VW.AUDI_Q2_MK1, + "AUDI Q3 2ND GEN": VW.AUDI_Q3_MK2, + "SEAT ATECA 1ST GEN": VW.SEAT_ATECA_MK1, + "SEAT LEON 3RD GEN": VW.SEAT_ATECA_MK1, + "SEAT_LEON_MK3": VW.SEAT_ATECA_MK1, + "SKODA FABIA 4TH GEN": VW.SKODA_FABIA_MK4, + "SKODA KAMIQ 1ST GEN": VW.SKODA_KAMIQ_MK1, + "SKODA KAROQ 1ST GEN": VW.SKODA_KAROQ_MK1, + "SKODA KODIAQ 1ST GEN": VW.SKODA_KODIAQ_MK1, + "SKODA OCTAVIA 3RD GEN": VW.SKODA_OCTAVIA_MK3, + "SKODA SCALA 1ST GEN": VW.SKODA_KAMIQ_MK1, + "SKODA_SCALA_MK1": VW.SKODA_KAMIQ_MK1, + "SKODA SUPERB 3RD GEN": VW.SKODA_SUPERB_MK3, + + "mock": MOCK.MOCK, +} diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 45516d6035..7be3b2ebe9 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,9 +1,10 @@ from cereal import car -from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker +from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams +from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -22,7 +23,7 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c return clip(apply_curvature, -CarControllerParams.CURVATURE_MAX, CarControllerParams.CURVATURE_MAX) -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.VM = VM @@ -34,6 +35,7 @@ class CarController: self.main_on_last = False self.lkas_enabled_last = False self.steer_alert_last = False + self.lead_distance_bars_last = None def update(self, CC, CS, now_nanos): can_sends = [] @@ -69,10 +71,10 @@ class CarController: self.apply_curvature_last = apply_curvature - if self.CP.carFingerprint in CANFD_CAR: + if self.CP.flags & FordFlags.CANFD: # TODO: extended mode mode = 1 if CC.latActive else 0 - counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF + counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10 can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -apply_curvature, 0., counter)) else: can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -apply_curvature, 0.)) @@ -97,17 +99,21 @@ class CarController: # send lkas ui msg at 1Hz or if ui state changes if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values)) + # send acc ui msg at 5Hz or if ui state changes + if hud_control.leadDistanceBars != self.lead_distance_bars_last: + send_ui = True if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive, - fcw_alert, CS.out.cruiseState.standstill, hud_control, - CS.acc_tja_status_stock_values)) + fcw_alert, CS.out.cruiseState.standstill, hud_control, + CS.acc_tja_status_stock_values)) self.main_on_last = main_on self.lkas_enabled_last = CC.latActive self.steer_alert_last = steer_alert + self.lead_distance_bars_last = hud_control.leadDistanceBars - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.curvature = self.apply_curvature_last self.frame += 1 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index ef56d23d79..78f48ec5c4 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,10 +1,10 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.interfaces import CarStateBase +from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC +from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags +from openpilot.selfdrive.car.interfaces import CarStateBase GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -18,17 +18,13 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnRng_D_RqGsm"] self.vehicle_sensors_valid = False - self.unsupported_platform = False + + self.prev_distance_button = 0 + self.distance_button = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() - # Ford Q3 hybrid variants experience a bug where a message from the PCM sends invalid checksums, - # this must be root-caused before enabling support. Ford Q4 hybrids do not have this problem. - # TrnAin_Tq_Actl and its quality flag are only set on ICE platform variants - self.unsupported_platform = (cp.vl["VehicleOperatingModes"]["TrnAinTq_D_Qf"] == 0 and - self.CP.carFingerprint not in CANFD_CAR) - # Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement # The vehicle usually recovers out of this state within a minute of normal driving self.vehicle_sensors_valid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] == 3 @@ -56,12 +52,13 @@ class CarState(CarStateBase): ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode - if self.CP.carFingerprint in CANFD_CAR: + if self.CP.flags & FordFlags.CANFD: # this signal is always 0 on non-CAN FD cars ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3) # cruise state - ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS + is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False + ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS) ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0 @@ -90,6 +87,8 @@ class CarState(CarStateBase): ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 # TODO: block this going to the camera otherwise it will enable stock TJA ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"] # lock info ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], @@ -98,7 +97,7 @@ class CarState(CarStateBase): # blindspot sensors if self.CP.enableBsm: - cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp + cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 @@ -129,10 +128,14 @@ class CarState(CarStateBase): ("RCMStatusMessage2_FD1", 10), ] - if CP.carFingerprint in CANFD_CAR: + if CP.flags & FordFlags.CANFD: messages += [ ("Lane_Assist_Data3_FD1", 33), ] + else: + messages += [ + ("INSTRUMENT_PANEL", 1), + ] if CP.transmissionType == TransmissionType.automatic: messages += [ @@ -144,7 +147,7 @@ class CarState(CarStateBase): ("BCM_Lamp_Stat_FD1", 1), ] - if CP.enableBsm and CP.carFingerprint not in CANFD_CAR: + if CP.enableBsm and not (CP.flags & FordFlags.CANFD): messages += [ ("Side_Detect_L_Stat", 5), ("Side_Detect_R_Stat", 5), @@ -162,7 +165,7 @@ class CarState(CarStateBase): ("IPMA_Data", 1), ] - if CP.enableBsm and CP.carFingerprint in CANFD_CAR: + if CP.enableBsm and CP.flags & FordFlags.CANFD: messages += [ ("Side_Detect_L_Stat", 5), ("Side_Detect_R_Stat", 5), diff --git a/selfdrive/car/ford/fingerprints.py b/selfdrive/car/ford/fingerprints.py index a5d465849a..2201072fa3 100644 --- a/selfdrive/car/ford/fingerprints.py +++ b/selfdrive/car/ford/fingerprints.py @@ -4,23 +4,26 @@ from openpilot.selfdrive.car.ford.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.BRONCO_SPORT_MK1: { + CAR.FORD_BRONCO_SPORT_MK1: { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LX6C-2D053-RF\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', ], (Ecu.fwdCamera, 0x706, None): [ b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.ESCAPE_MK4: { + CAR.FORD_ESCAPE_MK4: { (Ecu.eps, 0x730, None): [ b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -43,7 +46,7 @@ FW_VERSIONS = { b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.EXPLORER_MK6: { + CAR.FORD_EXPLORER_MK6: { (Ecu.eps, 0x730, None): [ b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -58,6 +61,7 @@ FW_VERSIONS = { b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-2D053-BJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ @@ -71,7 +75,7 @@ FW_VERSIONS = { b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.F_150_MK14: { + CAR.FORD_F_150_MK14: { (Ecu.eps, 0x730, None): [ b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -82,10 +86,11 @@ FW_VERSIONS = { b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x706, None): [ + b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.F_150_LIGHTNING_MK1: { + CAR.FORD_F_150_LIGHTNING_MK1: { (Ecu.abs, 0x760, None): [ b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -96,7 +101,7 @@ FW_VERSIONS = { b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.MUSTANG_MACH_E_MK1: { + CAR.FORD_MUSTANG_MACH_E_MK1: { (Ecu.eps, 0x730, None): [ b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -111,7 +116,7 @@ FW_VERSIONS = { b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.FOCUS_MK4: { + CAR.FORD_FOCUS_MK4: { (Ecu.eps, 0x730, None): [ b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -125,14 +130,17 @@ FW_VERSIONS = { b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.MAVERICK_MK1: { + CAR.FORD_MAVERICK_MK1: { (Ecu.eps, 0x730, None): [ + b'NZ6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ + b'NZ6C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -141,4 +149,18 @@ FW_VERSIONS = { b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, + CAR.FORD_RANGER_MK2: { + (Ecu.eps, 0x730, None): [ + b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.abs, 0x760, None): [ + b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, } diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index c5ef0900f3..2cfd61a191 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -212,7 +212,7 @@ def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw "AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator "AccStopMde_B_Dsply": 1 if standstill else 0, "AccWarn_D_Dsply": 0, # ACC warning - "AccTGap_D_Dsply": 4, # Fixed time gap in UI + "AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap }) # Forwards FCW alert from IPMA diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index cc013fb54b..2ef5d427e6 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,11 +1,12 @@ from cereal import car from panda import Panda from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.ford.fordcan import CanBus -from openpilot.selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu +from openpilot.selfdrive.car.ford.values import Ecu, FordFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter @@ -14,7 +15,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "ford" - ret.dashcamOnly = candidate in CANFD_CAR + ret.dashcamOnly = bool(ret.flags & FordFlags.CANFD) ret.radarUnavailable = True ret.steerControlType = car.CarParams.SteerControlType.angle @@ -36,53 +37,20 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL ret.openpilotLongitudinalControl = True - if candidate in CANFD_CAR: + if ret.flags & FordFlags.CANFD: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD - - if candidate == CAR.BRONCO_SPORT_MK1: - ret.wheelbase = 2.67 - ret.steerRatio = 17.7 - ret.mass = 1625 - - elif candidate == CAR.ESCAPE_MK4: - ret.wheelbase = 2.71 - ret.steerRatio = 16.7 - ret.mass = 1750 - - elif candidate == CAR.EXPLORER_MK6: - ret.wheelbase = 3.025 - ret.steerRatio = 16.8 - 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 - - elif candidate == CAR.F_150_LIGHTNING_MK1: - # required trim only on SuperCrew - ret.wheelbase = 3.70 - ret.steerRatio = 16.9 - ret.mass = 2948 - - elif candidate == CAR.MUSTANG_MACH_E_MK1: - ret.wheelbase = 2.984 - ret.steerRatio = 17.0 # guess - ret.mass = 2200 - - elif candidate == CAR.FOCUS_MK4: - ret.wheelbase = 2.7 - ret.steerRatio = 15.0 - ret.mass = 1350 - - elif candidate == CAR.MAVERICK_MK1: - ret.wheelbase = 3.076 - ret.steerRatio = 17.0 - ret.mass = 1650 - else: - raise ValueError(f"Unsupported car: {candidate}") + # Lock out if the car does not have needed lateral and longitudinal control APIs. + # Note that we also check CAN for adaptive cruise, but no known signal for LCA exists + pscm_config = next((fw for fw in car_fw if fw.ecu == Ecu.eps and b'\x22\xDE\x01' in fw.request), None) + if pscm_config: + if len(pscm_config.fwVersion) != 24: + ret.dashcamOnly = True + else: + config_tja = pscm_config.fwVersion[7] # Traffic Jam Assist + config_lca = pscm_config.fwVersion[8] # Lane Centering Assist + if config_tja != 0xFF or config_lca != 0xFF: + ret.dashcamOnly = True # Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1 found_ecus = [fw.ecu for fw in car_fw] @@ -106,15 +74,12 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + events = self.create_common_events(ret, extra_gears=[GearShifter.manumatic]) if not self.CS.vehicle_sensors_valid: events.add(car.CarEvent.EventName.vehicleSensorsInvalid) - if self.CS.unsupported_platform: - events.add(car.CarEvent.EventName.startupNoControl) ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/ford/tests/__init__.py b/selfdrive/car/ford/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/ford/tests/print_platform_codes.py b/selfdrive/car/ford/tests/print_platform_codes.py new file mode 100755 index 0000000000..670199980a --- /dev/null +++ b/selfdrive/car/ford/tests/print_platform_codes.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 +from collections import defaultdict + +from cereal import car +from openpilot.selfdrive.car.ford.values import get_platform_codes +from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +if __name__ == "__main__": + cars_for_code: defaultdict = defaultdict(lambda: defaultdict(set)) + + for car_model, ecus in FW_VERSIONS.items(): + print(car_model) + for ecu in sorted(ecus, key=lambda x: int(x[0])): + platform_codes = get_platform_codes(ecus[ecu]) + for code in platform_codes: + cars_for_code[ecu][code].add(car_model) + + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + print(f' Codes: {sorted(platform_codes)}') + print() + + print('\nCar models vs. platform codes:') + for ecu, codes in cars_for_code.items(): + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + for code, cars in codes.items(): + print(f' {code!r}: {sorted(map(str, cars))}') diff --git a/selfdrive/car/ford/tests/test_ford.py b/selfdrive/car/ford/tests/test_ford.py new file mode 100644 index 0000000000..b1a19017d4 --- /dev/null +++ b/selfdrive/car/ford/tests/test_ford.py @@ -0,0 +1,143 @@ +import random +from collections.abc import Iterable + +import capnp +from hypothesis import settings, given, strategies as st +from parameterized import parameterized + +from cereal import car +from openpilot.selfdrive.car.fw_versions import build_fw_dict +from openpilot.selfdrive.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes +from openpilot.selfdrive.car.ford.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu + + +ECU_ADDRESSES = { + Ecu.eps: 0x730, # Power Steering Control Module (PSCM) + Ecu.abs: 0x760, # Anti-Lock Brake System (ABS) + Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM) + Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA) + Ecu.engine: 0x7E0, # Powertrain Control Module (PCM) + Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM) + Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM) +} + + +ECU_PART_NUMBER = { + Ecu.eps: [ + b"14D003", + ], + Ecu.abs: [ + b"2D053", + ], + Ecu.fwdRadar: [ + b"14D049", + ], + Ecu.fwdCamera: [ + b"14F397", # Ford Q3 + b"14H102", # Ford Q4 + ], +} + + +class TestFordFW: + def test_fw_query_config(self): + for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus: + assert ecu in ECU_ADDRESSES, "Unknown ECU" + assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch" + assert subaddr is None, "Unexpected ECU subaddress" + + @parameterized.expand(FW_VERSIONS.items()) + def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[capnp.lib.capnp._EnumModule, int, int | None], Iterable[bytes]]): + for (ecu, addr, subaddr), fws in fw_versions.items(): + assert ecu in ECU_PART_NUMBER, "Unexpected ECU" + assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch" + assert subaddr is None, "Unexpected ECU subaddress" + + for fw in fws: + assert len(fw) == 24, "Expected ECU response to be 24 bytes" + + match = FW_PATTERN.match(fw) + assert match is not None, f"Unable to parse FW: {fw!r}" + if match: + part_number = match.group("part_number") + assert part_number in ECU_PART_NUMBER[ecu], f"Unexpected part number for {fw!r}" + + codes = get_platform_codes([fw]) + assert 1 == len(codes), f"Unable to parse FW: {fw!r}" + + @settings(max_examples=100) + @given(data=st.data()) + def test_platform_codes_fuzzy_fw(self, data): + """Ensure function doesn't raise an exception""" + fw_strategy = st.lists(st.binary()) + fws = data.draw(fw_strategy) + get_platform_codes(fws) + + def test_platform_codes_spot_check(self): + # Asserts basic platform code parsing behavior for a few cases + results = get_platform_codes([ + b"JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00", + ]) + assert results == {(b"X6A", b"J"), (b"Z6T", b"N"), (b"J6T", b"P"), (b"B5A", b"L")} + + def test_fuzzy_match(self): + for platform, fw_by_addr in FW_VERSIONS.items(): + # Ensure there's no overlaps in platform codes + for _ in range(20): + car_fw = [] + for ecu, fw_versions in fw_by_addr.items(): + ecu_name, addr, sub_addr = ecu + fw = random.choice(fw_versions) + car_fw.append({"ecu": ecu_name, "fwVersion": fw, "address": addr, + "subAddress": 0 if sub_addr is None else sub_addr}) + + CP = car.CarParams.new_message(carFw=car_fw) + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS) + assert matches == {platform} + + def test_match_fw_fuzzy(self): + offline_fw = { + (Ecu.eps, 0x730, None): [ + b"L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + ], + (Ecu.abs, 0x760, None): [ + b"L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"L1MC-2D053-BD\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", + b"LB5T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + ], + # We consider all model year hints for ECU, even with different platform codes + (Ecu.fwdCamera, 0x706, None): [ + b"LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + b"NC5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", + ], + } + expected_fingerprint = CAR.FORD_EXPLORER_MK6 + + # ensure that we fuzzy match on all non-exact FW with changed revisions + live_fw = { + (0x730, None): {b"L1MC-14D003-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, + (0x760, None): {b"L1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, + (0x764, None): {b"LB5T-14D049-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, + (0x706, None): {b"LB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}, + } + candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw}) + assert candidates == {expected_fingerprint} + + # model year hint in between the range should match + live_fw[(0x706, None)] = {b"MB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"} + candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw,}) + assert candidates == {expected_fingerprint} + + # unseen model year hint should not match + live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"} + candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw}) + assert len(candidates) == 0, "Should not match new model year hint" diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index c080e02299..b1868bfa9b 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -1,13 +1,14 @@ -from collections import defaultdict -from dataclasses import dataclass -from enum import Enum, StrEnum -from typing import Dict, List, Union +import copy +import re +from dataclasses import dataclass, field, replace +from enum import Enum, IntFlag +import panda.python.uds as uds from cereal import car -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \ +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, dbc_dict, DbcDict, PlatformConfig, Platforms +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ Device -from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries +from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -40,18 +41,9 @@ class CarControllerParams: pass -class CAR(StrEnum): - BRONCO_SPORT_MK1 = "FORD BRONCO SPORT 1ST GEN" - ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" - EXPLORER_MK6 = "FORD EXPLORER 6TH GEN" - F_150_MK14 = "FORD F-150 14TH GEN" - FOCUS_MK4 = "FORD FOCUS 4TH GEN" - MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" - F_150_LIGHTNING_MK1 = "FORD F-150 LIGHTNING 1ST GEN" - MUSTANG_MACH_E_MK1 = "FORD MUSTANG MACH-E 1ST GEN" - - -CANFD_CAR = {CAR.F_150_MK14, CAR.F_150_LIGHTNING_MK1, CAR.MUSTANG_MACH_E_MK1} +class FordFlags(IntFlag): + # Static flags + CANFD = 1 class RADAR: @@ -59,14 +51,6 @@ class RADAR: DELPHI_MRR = 'FORD_CADS' -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("ford_lincoln_base_pt", RADAR.DELPHI_MRR)) - -# F-150 radar is not yet supported -DBC[CAR.F_150_MK14] = dbc_dict("ford_lincoln_base_pt", None) -DBC[CAR.F_150_LIGHTNING_MK1] = dbc_dict("ford_lincoln_base_pt", None) -DBC[CAR.MUSTANG_MACH_E_MK1] = dbc_dict("ford_lincoln_base_pt", None) - - class Footnote(Enum): FOCUS = CarFootnote( "Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " + @@ -76,36 +60,186 @@ class Footnote(Enum): @dataclass -class FordCarInfo(CarInfo): +class FordCarDocs(CarDocs): package: str = "Co-Pilot360 Assist+" + hybrid: bool = False + plug_in_hybrid: bool = False def init_make(self, CP: car.CarParams): - harness = CarHarness.ford_q4 if CP.carFingerprint in CANFD_CAR else CarHarness.ford_q3 - if CP.carFingerprint in (CAR.BRONCO_SPORT_MK1, CAR.MAVERICK_MK1, CAR.F_150_MK14): + harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3 + if CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MAVERICK_MK1, CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1): self.car_parts = CarParts([Device.threex_angled_mount, harness]) else: self.car_parts = CarParts([Device.threex, harness]) -CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.BRONCO_SPORT_MK1: FordCarInfo("Ford Bronco Sport 2021-22"), - CAR.ESCAPE_MK4: [ - FordCarInfo("Ford Escape 2020-22"), - FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"), - ], - CAR.EXPLORER_MK6: [ - FordCarInfo("Ford Explorer 2020-23"), - FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"), - ], - CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"), - CAR.F_150_LIGHTNING_MK1: FordCarInfo("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0"), - CAR.MUSTANG_MACH_E_MK1: FordCarInfo("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0"), - CAR.FOCUS_MK4: FordCarInfo("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS]), - CAR.MAVERICK_MK1: [ - FordCarInfo("Ford Maverick 2022", "LARIAT Luxury"), - FordCarInfo("Ford Maverick 2023", "Co-Pilot360 Assist"), - ], -} +@dataclass +class FordPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', RADAR.DELPHI_MRR)) + + def init(self): + for car_docs in list(self.car_docs): + if car_docs.hybrid: + name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}" + self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) + if car_docs.plug_in_hybrid: + name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}" + self.car_docs.append(replace(copy.deepcopy(car_docs), name=name)) + + +@dataclass +class FordCANFDPlatformConfig(FordPlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('ford_lincoln_base_pt', None)) + + def init(self): + super().init() + self.flags |= FordFlags.CANFD + + +class CAR(Platforms): + FORD_BRONCO_SPORT_MK1 = FordPlatformConfig( + [FordCarDocs("Ford Bronco Sport 2021-23")], + CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7), + ) + FORD_ESCAPE_MK4 = FordPlatformConfig( + [ + FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True), + FordCarDocs("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True), + ], + CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7), + ) + FORD_EXPLORER_MK6 = FordPlatformConfig( + [ + FordCarDocs("Ford Explorer 2020-23", hybrid=True), # Hybrid: Limited and Platinum only + FordCarDocs("Lincoln Aviator 2020-23", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only + ], + CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8), + ) + FORD_F_150_MK14 = FordCANFDPlatformConfig( + [FordCarDocs("Ford F-150 2022-23", "Co-Pilot360 Active 2.0", hybrid=True)], + CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0), + ) + FORD_F_150_LIGHTNING_MK1 = FordCANFDPlatformConfig( + [FordCarDocs("Ford F-150 Lightning 2021-23", "Co-Pilot360 Active 2.0")], + CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9), + ) + FORD_FOCUS_MK4 = FordPlatformConfig( + [FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only + CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0), + ) + FORD_MAVERICK_MK1 = FordPlatformConfig( + [ + FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True), + FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True), + ], + CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0), + ) + FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig( + [FordCarDocs("Ford Mustang Mach-E 2021-23", "Co-Pilot360 Active 2.0")], + CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio + ) + FORD_RANGER_MK2 = FordCANFDPlatformConfig( + [FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering")], + CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0), + ) + + +# FW response contains a combined software and part number +# A-Z except no I, O or W +# e.g. NZ6A-14C204-AAA +# 1222-333333-444 +# 1 = Model year hint (approximates model year/generation) +# 2 = Platform hint +# 3 = Part number +# 4 = Software version +FW_ALPHABET = b'A-HJ-NP-VX-Z' +FW_PATTERN = re.compile(b'^(?P[' + FW_ALPHABET + b'])' + + b'(?P[0-9' + FW_ALPHABET + b']{3})-' + + b'(?P[0-9' + FW_ALPHABET + b']{5,6})-' + + b'(?P[' + FW_ALPHABET + b']{2,})\x00*$') + + +def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes]]: + codes = set() + for fw in fw_versions: + match = FW_PATTERN.match(fw) + if match is not None: + codes.add((match.group('platform_hint'), match.group('model_year_hint'))) + + return codes + + +def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]: + candidates: set[str] = set() + + for candidate, fws in offline_fw_versions.items(): + # Keep track of ECUs which pass all checks (platform hint, within model year hint range) + valid_found_ecus = set() + valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS} + for ecu, expected_versions in fws.items(): + addr = ecu[1:] + # Only check ECUs expected to have platform codes + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + # Expected platform codes & model year hints + codes = get_platform_codes(expected_versions) + expected_platform_codes = {code for code, _ in codes} + expected_model_year_hints = {model_year_hint for _, model_year_hint in codes} + + # Found platform codes & model year hints + codes = get_platform_codes(live_fw_versions.get(addr, set())) + found_platform_codes = {code for code, _ in codes} + found_model_year_hints = {model_year_hint for _, model_year_hint in codes} + + # Check platform code matches for any found versions + if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes): + break + + # Check any model year hint within range in the database. Note that some models have more than one + # platform code per ECU which we don't consider as separate ranges + if not any(min(expected_model_year_hints) <= found_model_year_hint <= max(expected_model_year_hints) for + found_model_year_hint in found_model_year_hints): + break + + valid_found_ecus.add(addr) + + # If all live ECUs pass all checks for candidate, add it as a match + if valid_expected_ecus.issubset(valid_found_ecus): + candidates.add(candidate) + + return candidates + + +# All of these ECUs must be present and are expected to have platform codes we can match +PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps) + +DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00 + +ASBUILT_BLOCKS: list[tuple[int, list]] = [ + (1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]), + (2, [Ecu.abs, Ecu.debug, Ecu.eps]), + (3, [Ecu.abs, Ecu.debug, Ecu.eps]), + (4, [Ecu.debug, Ecu.fwdCamera]), + (5, [Ecu.debug]), + (6, [Ecu.debug]), + (7, [Ecu.debug]), + (8, [Ecu.debug]), + (9, [Ecu.debug]), + (16, [Ecu.debug, Ecu.fwdCamera]), + (18, [Ecu.fwdCamera]), + (20, [Ecu.fwdCamera]), + (21, [Ecu.fwdCamera]), +] + + +def ford_asbuilt_block_request(block_id: int): + return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) + + +def ford_asbuilt_block_response(block_id: int): + return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -114,13 +248,32 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], + logging=True, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire], bus=0, auxiliary=True, ), + *[Request( + [StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)], + [StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)], + whitelist_ecus=ecus, + bus=0, + logging=True, + ) for block_id, ecus in ASBUILT_BLOCKS], ], extra_ecus=[ - # We are unlikely to get a response from the PCM from behind the gateway - (Ecu.engine, 0x7e0, None), - (Ecu.shiftByWire, 0x732, None), + (Ecu.engine, 0x7e0, None), # Powertrain Control Module + # Note: We are unlikely to get a response from behind the gateway + (Ecu.shiftByWire, 0x732, None), # Gear Shift Module + (Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module ], + # Custom fuzzy fingerprinting function using platform and model year hints + match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index 36e6794d2c..bb2827571c 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -3,16 +3,16 @@ import capnp import copy from dataclasses import dataclass, field import struct -from typing import Callable, Dict, List, Optional, Set, Tuple +from collections.abc import Callable import panda.python.uds as uds -AddrType = Tuple[int, Optional[int]] -EcuAddrBusType = Tuple[int, Optional[int], int] -EcuAddrSubAddr = Tuple[int, int, Optional[int]] +AddrType = tuple[int, int | None] +EcuAddrBusType = tuple[int, int | None, int] +EcuAddrSubAddr = tuple[int, int, int | None] -LiveFwVersions = Dict[AddrType, Set[bytes]] -OfflineFwVersions = Dict[str, Dict[EcuAddrSubAddr, List[bytes]]] +LiveFwVersions = dict[AddrType, set[bytes]] +OfflineFwVersions = dict[str, dict[EcuAddrSubAddr, list[bytes]]] # A global list of addresses we will only ever consider for VIN responses # engine, hybrid controller, Ford abs, Hyundai CAN FD cluster, 29-bit engine, PGM-FI @@ -47,6 +47,11 @@ class StdQueries: MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ @@ -71,30 +76,30 @@ class StdQueries: @dataclass class Request: - request: List[bytes] - response: List[bytes] - whitelist_ecus: List[int] = field(default_factory=list) + request: list[bytes] + response: list[bytes] + whitelist_ecus: list[int] = field(default_factory=list) rx_offset: int = 0x8 bus: int = 1 # Whether this query should be run on the first auxiliary panda (CAN FD cars for example) auxiliary: bool = False # FW responses from these queries will not be used for fingerprinting logging: bool = False - # boardd toggles OBD multiplexing on/off as needed + # pandad toggles OBD multiplexing on/off as needed obd_multiplexing: bool = True @dataclass class FwQueryConfig: - requests: List[Request] + requests: list[Request] # TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus # Overrides and removes from essential ecus for specific models and ecus (exact matching) - non_essential_ecus: Dict[capnp.lib.capnp._EnumModule, List[str]] = field(default_factory=dict) + non_essential_ecus: dict[capnp.lib.capnp._EnumModule, list[str]] = field(default_factory=dict) # Ecus added for data collection, not to be fingerprinted on - extra_ecus: List[Tuple[capnp.lib.capnp._EnumModule, int, Optional[int]]] = field(default_factory=list) - # Function a brand can implement to provide better fuzzy matching. Takes in FW versions, + extra_ecus: list[tuple[capnp.lib.capnp._EnumModule, int, int | None]] = field(default_factory=list) + # Function a brand can implement to provide better fuzzy matching. Takes in FW versions and VIN, # returns set of candidates. Only will match if one candidate is returned - match_fw_to_car_fuzzy: Optional[Callable[[LiveFwVersions, OfflineFwVersions], Set[str]]] = None + match_fw_to_car_fuzzy: Callable[[LiveFwVersions, str, OfflineFwVersions], set[str]] | None = None def __post_init__(self): for i in range(len(self.requests)): diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 6c02e69503..4bb6413d01 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,18 +1,20 @@ #!/usr/bin/env python3 from collections import defaultdict -from typing import Any, DefaultDict, Dict, Iterator, List, Optional, Set, TypeVar +from collections.abc import Iterator +from typing import Any, Protocol, TypeVar + from tqdm import tqdm import capnp import panda.python.uds as uds from cereal import car from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs -from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig -from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions +from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery -from openpilot.common.swaglog import cloudlog Ecu = car.CarParams.Ecu ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] @@ -27,19 +29,18 @@ REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for T = TypeVar('T') -def chunks(l: List[T], n: int = 128) -> Iterator[List[T]]: +def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]: for i in range(0, len(l), n): yield l[i:i + n] -def is_brand(brand: str, filter_brand: Optional[str]) -> bool: +def is_brand(brand: str, filter_brand: str | None) -> bool: """Returns if brand matches filter_brand or no brand filter is specified""" return filter_brand is None or brand == filter_brand -def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder], - filter_brand: Optional[str] = None) -> Dict[AddrType, Set[bytes]]: - fw_versions_dict: DefaultDict[AddrType, Set[bytes]] = defaultdict(set) +def build_fw_dict(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], filter_brand: 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 @@ -47,7 +48,12 @@ def build_fw_dict(fw_versions: List[capnp.lib.capnp._DynamicStructBuilder], return dict(fw_versions_dict) -def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude=None): +class MatchFwToCar(Protocol): + def __call__(self, live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True) -> set[str]: + ... + + +def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, exclude: str = None) -> set[str]: """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.""" @@ -72,7 +78,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude= all_fw_versions[(addr[1], addr[2], f)].append(candidate) matched_ecus = set() - candidate = None + match: str | None = None for addr, versions in live_fw_versions.items(): ecu_key = (addr[0], addr[1]) for version in versions: @@ -81,23 +87,23 @@ def match_fw_to_car_fuzzy(live_fw_versions, match_brand=None, log=True, exclude= if len(candidates) == 1: matched_ecus.add(ecu_key) - if candidate is None: - candidate = candidates[0] + if match is None: + match = candidates[0] # We uniquely matched two different cars. No fuzzy match possible - elif candidate != candidates[0]: + elif match != candidates[0]: return set() # Note that it is possible to match to a candidate without all its ECUs being present # if there are enough matches. FIXME: parameterize this or require all ECUs to exist like exact matching - if len(matched_ecus) >= 2: + if match and len(matched_ecus) >= 2: if log: - cloudlog.error(f"Fingerprinted {candidate} using fuzzy match. {len(matched_ecus)} matching ECUs") - return {candidate} + cloudlog.error(f"Fingerprinted {match} using fuzzy match. {len(matched_ecus)} matching ECUs") + return {match} else: return set() -def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True, extra_fw_versions=None) -> Set[str]: +def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str = None, log: bool = True, extra_fw_versions: dict = None) -> 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 @@ -138,9 +144,10 @@ def match_fw_to_car_exact(live_fw_versions, match_brand=None, log=True, extra_fw return set(candidates.keys()) - invalid -def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): +def match_fw_to_car(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], vin: str, + allow_exact: bool = True, allow_fuzzy: bool = True, log: bool = True) -> tuple[bool, set[str]]: # Try exact matching first - exact_matches = [] + exact_matches: list[tuple[bool, MatchFwToCar]] = [] if allow_exact: exact_matches = [(True, match_fw_to_car_exact)] if allow_fuzzy: @@ -148,7 +155,7 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): for exact_match, match_func in exact_matches: # For each brand, attempt to fingerprint using all FW returned from its queries - matches = set() + matches: set[str] = set() for brand in VERSIONS.keys(): fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand) matches |= match_func(fw_versions_dict, match_brand=brand, log=log) @@ -156,7 +163,7 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): # If specified and no matches so far, fall back to brand's fuzzy fingerprinting function config = FW_QUERY_CONFIGS[brand] if not exact_match and not len(matches) and config.match_fw_to_car_fuzzy is not None: - matches |= config.match_fw_to_car_fuzzy(fw_versions_dict, VERSIONS[brand]) + matches |= config.match_fw_to_car_fuzzy(fw_versions_dict, vin, VERSIONS[brand]) if len(matches): return exact_match, matches @@ -164,12 +171,12 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True, log=True): return True, set() -def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: +def get_present_ecus(logcan, sendcan, num_pandas: int = 1) -> set[EcuAddrBusType]: params = Params() # queries are split by OBD multiplexing mode - queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []} - parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []} - responses = set() + queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []} + parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []} + responses: set[EcuAddrBusType] = set() for brand, config, r in REQUESTS: # Skip query if no panda available @@ -203,7 +210,7 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]: return ecu_responses -def get_brand_ecu_matches(ecu_rx_addrs: Set[EcuAddrBusType]) -> dict[str, set[AddrType]]: +def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[AddrType]]: """Returns dictionary of brands and matches with ECUs in their FW versions""" brand_addrs = {brand: {(addr, subaddr) for _, addr, subaddr in config.get_all_ecus(VERSIONS[brand])} for @@ -230,8 +237,8 @@ def set_obd_multiplexing(params: Params, obd_multiplexing: bool): cloudlog.warning("OBD multiplexing set successfully") -def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \ - List[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions_ordered(logcan, sendcan, vin: str, ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, + debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -246,15 +253,15 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand all_car_fw.extend(car_fw) # If there is a match using this brand's FW alone, finish querying early - _, matches = match_fw_to_car(car_fw, log=False) + _, matches = match_fw_to_car(car_fw, vin, log=False) if len(matches) == 1: break return all_car_fw -def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False) -> \ - List[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions(logcan, sendcan, query_brand: str = None, extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, + debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: versions = VERSIONS.copy() params = Params() @@ -345,7 +352,7 @@ if __name__ == "__main__": pandaStates_sock = messaging.sub_sock('pandaStates') sendcan = messaging.pub_sock('sendcan') - # Set up params for boardd + # Set up params for pandad params = Params() params.remove("FirmwareQueryDone") params.put_bool("IsOnroad", False) @@ -366,14 +373,15 @@ if __name__ == "__main__": t = time.time() print("Getting vin...") - vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1), retry=10, debug=args.debug) + set_obd_multiplexing(params, True) + vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1), debug=args.debug) print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}') print(f"Getting VIN took {time.time() - t:.3f} s") print() t = time.time() fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True) - _, candidates = match_fw_to_car(fw_vers) + _, candidates = match_fw_to_car(fw_vers, vin) print() print("Found FW versions") @@ -381,7 +389,8 @@ if __name__ == "__main__": padding = max([len(fw.brand) for fw in fw_vers] or [0]) for version in fw_vers: subaddr = None if version.subAddress == 0 else hex(version.subAddress) - print(f" Brand: {version.brand:{padding}}, bus: {version.bus} - (Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]") + print(f" Brand: {version.brand:{padding}}, bus: {version.bus}, OBD: {version.obdMultiplexing} - " + + f"(Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}]") print("}") print() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 6c7e8007f2..b204d3b80f 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -6,6 +6,7 @@ from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons +from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert NetworkLocation = car.CarParams.NetworkLocation @@ -17,7 +18,7 @@ CAMERA_CANCEL_DELAY_FRAMES = 10 MIN_STEER_MSG_INTERVAL_MS = 15 -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.start_time = 0. @@ -117,7 +118,7 @@ class CarController: # Send dashboard UI commands (ACC status) send_fcw = hud_alert == VisualAlert.fcw can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled, - hud_v_cruise * CV.MS_TO_KPH, hud_control.leadVisible, send_fcw)) + hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) @@ -154,7 +155,7 @@ class CarController: if self.frame % 10 == 0: can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last new_actuators.gas = self.apply_gas diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index c3d061de78..a1129c59c8 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -26,11 +26,16 @@ class CarState(CarStateBase): self.cam_lka_steering_cmd_counter = 0 self.buttons_counter = 0 + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, pt_cp, cam_cp, loopback_cp): ret = car.CarState.new_message() self.prev_cruise_buttons = self.cruise_buttons + self.prev_distance_button = self.distance_button self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"] + self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"] self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"] self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"]) # This is to avoid a fault where you engage while still moving backwards after shifting to D. diff --git a/selfdrive/car/gm/fingerprints.py b/selfdrive/car/gm/fingerprints.py index 73a205a250..3752fbb8d3 100644 --- a/selfdrive/car/gm/fingerprints.py +++ b/selfdrive/car/gm/fingerprints.py @@ -9,7 +9,7 @@ FINGERPRINTS = { CAR.HOLDEN_ASTRA: [{ 190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7 }], - CAR.VOLT: [{ + CAR.CHEVROLET_VOLT: [{ 170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8 }, { @@ -27,31 +27,31 @@ FINGERPRINTS = { CAR.CADILLAC_ATS: [{ 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 }], - CAR.MALIBU: [{ + CAR.CHEVROLET_MALIBU: [{ 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8 }], - CAR.ACADIA: [{ + CAR.GMC_ACADIA: [{ 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7 }, { 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8 }], - CAR.ESCALADE: [{ + CAR.CADILLAC_ESCALADE: [{ 170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 4, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1609: 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, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8 }], - CAR.ESCALADE_ESV: [{ + CAR.CADILLAC_ESCALADE_ESV: [{ 309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8 }], - CAR.ESCALADE_ESV_2019: [{ + CAR.CADILLAC_ESCALADE_ESV_2019: [{ 715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7 }], - CAR.BOLT_EUV: [{ + CAR.CHEVROLET_BOLT_EUV: [{ 189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7 }], - CAR.SILVERADO: [{ + CAR.CHEVROLET_SILVERADO: [{ 190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 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, 1611: 8, 1930: 7 }], - CAR.EQUINOX: [{ + CAR.CHEVROLET_EQUINOX: [{ 190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 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, 1033: 7, 1034: 7, 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, 1611: 8, 1930: 7 }, { diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index bd1e29ce3b..e833e77636 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -76,7 +76,7 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s mode = 0x1 # TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake - if enabled and CP.carFingerprint in (CAR.BOLT_EUV,): + if enabled and CP.carFingerprint in (CAR.CHEVROLET_BOLT_EUV,): mode = 0x9 if apply_brake > 0: @@ -102,17 +102,17 @@ def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_s return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values) -def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, lead_car_in_sight, fcw): +def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw): target_speed = min(target_speed_kph, 255) values = { "ACCAlwaysOne": 1, "ACCResumeButton": 0, "ACCSpeedSetpoint": target_speed, - "ACCGapLevel": 3 * enabled, # 3 "far", 0 "inactive" + "ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive" "ACCCmdActive": enabled, "ACCAlwaysOne2": 1, - "ACCLeadCar": lead_car_in_sight, + "ACCLeadCar": hud_control.leadVisible, "FCWAlert": 0x3 if fcw else 0 } diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 05caa28510..358bc9e5ba 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -22,9 +22,9 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D NON_LINEAR_TORQUE_PARAMS = { - CAR.BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], - CAR.ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772], - CAR.SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122] + CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178], + CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772], + CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122] } NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/neural_ff_weights.json') @@ -43,7 +43,7 @@ class CarInterface(CarInterfaceBase): return 0.10006696 * sigmoid * (v_ego + 3.12485927) def get_steer_feedforward_function(self): - if self.CP.carFingerprint == CAR.VOLT: + if self.CP.carFingerprint == CAR.CHEVROLET_VOLT: return self.get_steer_feedforward_volt else: return CarInterfaceBase.get_steer_feedforward_default @@ -74,7 +74,7 @@ class CarInterface(CarInterfaceBase): return float(self.neural_ff_model.predict(inputs)) + friction def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType: - if self.CP.carFingerprint == CAR.BOLT_EUV: + if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_EUV: self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint) return self.torque_from_lateral_accel_neural elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS: @@ -137,7 +137,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/car/tests/routes.py, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} or \ + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \ (ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable) # Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below. @@ -145,19 +145,12 @@ 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 - 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. - ret.wheelbase = 2.69 - ret.steerRatio = 17.7 # Stock 15.7, LiveParameters - ret.tireStiffnessFactor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters - ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh - + if candidate == CAR.CHEVROLET_VOLT: ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] ret.lateralTuning.pid.kiBP = [0.] @@ -165,64 +158,22 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt() ret.steerActuatorDelay = 0.2 - elif candidate == CAR.MALIBU: - 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. - ret.wheelbase = 2.662 - # Remaining parameters copied from Volt for now - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 15.7 - - elif candidate == CAR.ACADIA: + elif candidate == CAR.GMC_ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm - 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 ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) elif candidate == CAR.BUICK_LACROSSE: - 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 # (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. - ret.wheelbase = 2.78 - ret.steerRatio = 15.3 - ret.centerToFront = ret.wheelbase * 0.5 - - elif candidate == CAR.ESCALADE: + elif candidate == CAR.CADILLAC_ESCALADE: ret.minEnableSpeed = -1. # engage speed is decided by pcm - 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 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate in (CAR.ESCALADE_ESV, CAR.ESCALADE_ESV_2019): + elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019): ret.minEnableSpeed = -1. # engage speed is decided by pcm - ret.mass = 2739. - ret.wheelbase = 3.302 - ret.steerRatio = 17.3 - ret.centerToFront = ret.wheelbase * 0.5 - ret.tireStiffnessFactor = 1.0 - if candidate == CAR.ESCALADE_ESV: + if candidate == CAR.CADILLAC_ESCALADE_ESV: 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 @@ -230,21 +181,11 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.BOLT_EUV: - ret.mass = 1669. - ret.wheelbase = 2.63779 - ret.steerRatio = 16.8 - ret.centerToFront = ret.wheelbase * 0.4 - ret.tireStiffnessFactor = 1.0 + elif candidate == CAR.CHEVROLET_BOLT_EUV: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.SILVERADO: - ret.mass = 2450. - ret.wheelbase = 3.75 - ret.steerRatio = 16.3 - ret.centerToFront = ret.wheelbase * 0.5 - ret.tireStiffnessFactor = 1.0 + elif candidate == CAR.CHEVROLET_SILVERADO: # 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 @@ -252,19 +193,10 @@ class CarInterface(CarInterfaceBase): ret.minEnableSpeed = -1. CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.EQUINOX: - ret.mass = 3500. * CV.LB_TO_KG - ret.wheelbase = 2.72 - ret.steerRatio = 14.4 - ret.centerToFront = ret.wheelbase * 0.4 + elif candidate == CAR.CHEVROLET_EQUINOX: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - elif candidate == CAR.TRAILBLAZER: - ret.mass = 1345. - ret.wheelbase = 2.64 - ret.steerRatio = 16.8 - ret.centerToFront = ret.wheelbase * 0.4 - ret.tireStiffnessFactor = 1.0 + elif candidate == CAR.CHEVROLET_TRAILBLAZER: ret.steerActuatorDelay = 0.2 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) @@ -276,8 +208,12 @@ class CarInterface(CarInterfaceBase): # Don't add event if transitioning from INIT, unless it's to an actual button if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT: - ret.buttonEvents = create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, - unpressed_btn=CruiseButtons.UNPRESS) + ret.buttonEvents = [ + *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT, + unpressed_btn=CruiseButtons.UNPRESS), + *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, + {1: ButtonType.gapAdjustCruise}) + ] # The ECM allows enabling on falling edge of set, but only rising edge of resume events = self.create_common_events(ret, extra_gears=[GearShifter.sport, GearShifter.low, @@ -301,6 +237,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/gm/tests/__init__.py b/selfdrive/car/gm/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/gm/tests/test_gm.py b/selfdrive/car/gm/tests/test_gm.py new file mode 100644 index 0000000000..a0a4a94ffa --- /dev/null +++ b/selfdrive/car/gm/tests/test_gm.py @@ -0,0 +1,20 @@ +from parameterized import parameterized + +from openpilot.selfdrive.car.gm.fingerprints import FINGERPRINTS +from openpilot.selfdrive.car.gm.values import CAMERA_ACC_CAR, GM_RX_OFFSET + +CAMERA_DIAGNOSTIC_ADDRESS = 0x24b + + +class TestGMFingerprint: + @parameterized.expand(FINGERPRINTS.items()) + def test_can_fingerprints(self, car_model, fingerprints): + assert len(fingerprints) > 0 + + assert all(len(finger) for finger in fingerprints) + + # The camera can sometimes be communicating on startup + if car_model in CAMERA_ACC_CAR: + for finger in fingerprints: + for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET): + assert finger.get(required_addr) == 8, required_addr diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 8188ad4e6e..53a4621d27 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -1,11 +1,8 @@ -from collections import defaultdict -from dataclasses import dataclass -from enum import Enum, StrEnum -from typing import Dict, List, Union +from dataclasses import dataclass, field from cereal import car -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import dbc_dict, PlatformConfig, DbcDict, Platforms, CarSpecs +from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -62,32 +59,8 @@ class CarControllerParams: self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.] -class CAR(StrEnum): - HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017" - VOLT = "CHEVROLET VOLT PREMIER 2017" - CADILLAC_ATS = "CADILLAC ATS Premium Performance 2018" - MALIBU = "CHEVROLET MALIBU PREMIER 2017" - ACADIA = "GMC ACADIA DENALI 2018" - BUICK_LACROSSE = "BUICK LACROSSE 2017" - BUICK_REGAL = "BUICK REGAL ESSENCE 2018" - ESCALADE = "CADILLAC ESCALADE 2017" - ESCALADE_ESV = "CADILLAC ESCALADE ESV 2016" - ESCALADE_ESV_2019 = "CADILLAC ESCALADE ESV 2019" - BOLT_EUV = "CHEVROLET BOLT EUV 2022" - SILVERADO = "CHEVROLET SILVERADO 1500 2020" - EQUINOX = "CHEVROLET EQUINOX 2019" - TRAILBLAZER = "CHEVROLET TRAILBLAZER 2021" - - -class Footnote(Enum): - OBD_II = CarFootnote( - 'Requires a community built ASCM harness. ' + - 'NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).', - Column.MODEL) - - @dataclass -class GMCarInfo(CarInfo): +class GMCarDocs(CarDocs): package: str = "Adaptive Cruise Control (ACC)" def init_make(self, CP: car.CarParams): @@ -95,31 +68,88 @@ class GMCarInfo(CarInfo): self.car_parts = CarParts.common([CarHarness.gm]) else: self.car_parts = CarParts.common([CarHarness.obd_ii]) - self.footnotes.append(Footnote.OBD_II) - - -CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = { - CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017"), - CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ"), - CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"), - CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017"), - CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo"), - CAR.BUICK_LACROSSE: GMCarInfo("Buick LaCrosse 2017-19", "Driver Confidence Package 2"), - CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"), - CAR.ESCALADE: GMCarInfo("Cadillac Escalade 2017", "Driver Assist Package"), - CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS"), - CAR.ESCALADE_ESV_2019: GMCarInfo("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS"), - CAR.BOLT_EUV: [ - GMCarInfo("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), - GMCarInfo("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), - ], - CAR.SILVERADO: [ - GMCarInfo("Chevrolet Silverado 1500 2020-21", "Safety Package II"), - GMCarInfo("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), - ], - CAR.EQUINOX: GMCarInfo("Chevrolet Equinox 2019-22"), - CAR.TRAILBLAZER: GMCarInfo("Chevrolet Trailblazer 2021-22"), -} + + +@dataclass(frozen=True, kw_only=True) +class GMCarSpecs(CarSpecs): + tireStiffnessFactor: float = 0.444 # not optimized yet + + +@dataclass +class GMPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) + + +@dataclass +class GMASCMPlatformConfig(GMPlatformConfig): + def init(self): + # ASCM is supported, but due to a janky install and hardware configuration, we are not showing in the car docs + self.car_docs = [] + + +class CAR(Platforms): + HOLDEN_ASTRA = GMASCMPlatformConfig( + [GMCarDocs("Holden Astra 2017")], + GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4), + ) + CHEVROLET_VOLT = GMASCMPlatformConfig( + [GMCarDocs("Chevrolet Volt 2017-18", min_enable_speed=0, video_link="https://youtu.be/QeMCN_4TFfQ")], + GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469), + ) + CADILLAC_ATS = GMASCMPlatformConfig( + [GMCarDocs("Cadillac ATS Premium Performance 2018")], + GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3), + ) + CHEVROLET_MALIBU = GMASCMPlatformConfig( + [GMCarDocs("Chevrolet Malibu Premier 2017")], + GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4), + ) + GMC_ACADIA = GMASCMPlatformConfig( + [GMCarDocs("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo")], + GMCarSpecs(mass=1975, wheelbase=2.86, steerRatio=14.4, centerToFrontRatio=0.4), + ) + BUICK_LACROSSE = GMASCMPlatformConfig( + [GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")], + GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4), + ) + BUICK_REGAL = GMASCMPlatformConfig( + [GMCarDocs("Buick Regal Essence 2018")], + GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4), + ) + CADILLAC_ESCALADE = GMASCMPlatformConfig( + [GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")], + GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3), + ) + CADILLAC_ESCALADE_ESV = GMASCMPlatformConfig( + [GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")], + GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0), + ) + CADILLAC_ESCALADE_ESV_2019 = GMASCMPlatformConfig( + [GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")], + CADILLAC_ESCALADE_ESV.specs, + ) + CHEVROLET_BOLT_EUV = GMPlatformConfig( + [ + GMCarDocs("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim without Super Cruise Package", video_link="https://youtu.be/xvwzGMUA210"), + GMCarDocs("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"), + ], + GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) + CHEVROLET_SILVERADO = GMPlatformConfig( + [ + GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"), + GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video_link="https://youtu.be/5HbNoBLzRwE"), + ], + GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0), + ) + CHEVROLET_EQUINOX = GMPlatformConfig( + [GMCarDocs("Chevrolet Equinox 2019-22")], + GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4), + ) + CHEVROLET_TRAILBLAZER = GMPlatformConfig( + [GMCarDocs("Chevrolet Trailblazer 2021-22")], + GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0), + ) class CruiseButtons: @@ -148,22 +178,35 @@ class CanBus: # In a Data Module, an identifier is a string used to recognize an object, # either by itself or together with the identifiers of parent objects. # Each returns a 4 byte hex representation of the decimal part number. `b"\x02\x8c\xf0'"` -> 42790951 +GM_BOOT_SOFTWARE_PART_NUMER_REQUEST = b'\x1a\xc0' # likely does not contain anything useful GM_SOFTWARE_MODULE_1_REQUEST = b'\x1a\xc1' GM_SOFTWARE_MODULE_2_REQUEST = b'\x1a\xc2' GM_SOFTWARE_MODULE_3_REQUEST = b'\x1a\xc3' + +# Part number of XML data file that is used to configure ECU +GM_XML_DATA_FILE_PART_NUMBER = b'\x1a\x9c' +GM_XML_CONFIG_COMPAT_ID = b'\x1a\x9b' # used to know if XML file is compatible with the ECU software/hardware + # This DID is for identifying the part number that reflects the mix of hardware, # software, and calibrations in the ECU when it first arrives at the vehicle assembly plant. # If there's an Alpha Code, it's associated with this part number and stored in the DID $DB. GM_END_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcb' +GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdb' GM_BASE_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcc' +GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdc' GM_FW_RESPONSE = b'\x5a' GM_FW_REQUESTS = [ + GM_BOOT_SOFTWARE_PART_NUMER_REQUEST, GM_SOFTWARE_MODULE_1_REQUEST, GM_SOFTWARE_MODULE_2_REQUEST, GM_SOFTWARE_MODULE_3_REQUEST, + GM_XML_DATA_FILE_PART_NUMBER, + GM_XML_CONFIG_COMPAT_ID, GM_END_MODEL_PART_NUMBER_REQUEST, + GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, GM_BASE_MODEL_PART_NUMBER_REQUEST, + GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST, ] GM_RX_OFFSET = 0x400 @@ -181,11 +224,11 @@ FW_QUERY_CONFIG = FwQueryConfig( extra_ecus=[(Ecu.fwdCamera, 0x24b, None)], ) -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis')) - -EV_CAR = {CAR.VOLT, CAR.BOLT_EUV} +EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_BOLT_EUV} # We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness) -CAMERA_ACC_CAR = {CAR.BOLT_EUV, CAR.SILVERADO, CAR.EQUINOX, CAR.TRAILBLAZER} +CAMERA_ACC_CAR = {CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_TRAILBLAZER} STEER_THRESHOLD = 1.0 + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 056b47c4b3..fe023ea17d 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -4,9 +4,9 @@ from cereal import car from openpilot.common.numpy_fast import clip, interp from openpilot.common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from openpilot.selfdrive.car import create_gas_interceptor_command from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -95,7 +95,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "lead_visible", - "lanes_visible", "fcw", "acc_alert", "steer_required"]) + "lanes_visible", "fcw", "acc_alert", "steer_required", "lead_distance_bars"]) def rate_limit_steer(new_steer, last_steer): @@ -104,11 +104,12 @@ def rate_limit_steer(new_steer, last_steer): return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA) -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) + self.CAN = hondacan.CanBus(CP) self.frame = 0 self.braking = False @@ -167,7 +168,7 @@ class CarController: can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1)) # Send steering command. - can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint, + can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_steer, CC.latActive, self.CP.carFingerprint, CS.CP.openpilotLongitudinalControl)) # wind brake from air resistance decel at high speed @@ -181,7 +182,7 @@ class CarController: 0.5] # The Honda ODYSSEY seems to have different PCM_ACCEL # msgs, is it other cars too? - if self.CP.enableGasInterceptor or not CC.longActive: + if not CC.longActive: pcm_speed = 0.0 pcm_accel = int(0.0) elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL: @@ -201,12 +202,12 @@ class CarController: if not self.CP.openpilotLongitudinalControl: if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message - can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint)) + can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CAN, self.CP.carFingerprint)) # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint)) + can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint)) elif CC.cruiseControl.resume: - can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) + can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, self.CP.carFingerprint)) else: # Send gas and brake commands. @@ -219,7 +220,7 @@ class CarController: stopping = actuators.longControlState == LongCtrlState.stopping self.stopping_counter = self.stopping_counter + 1 if stopping else 0 - can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas, + can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas, self.stopping_counter, self.CP.carFingerprint)) else: apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0) @@ -227,38 +228,23 @@ class CarController: pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) pcm_override = True - can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, + can_sends.append(hondacan.create_brake_command(self.packer, self.CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw_display, self.CP.carFingerprint, CS.stock_brake)) self.apply_brake_last = apply_brake self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX - if self.CP.enableGasInterceptor: - # way too aggressive at low speed without this - gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0]) - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. - # This prevents unexpected pedal range rescaling - # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected - # when you do enable. - if CC.longActive: - self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.) - else: - self.gas = 0.0 - can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2)) - # Send dashboard UI commands. if self.frame % 10 == 0: hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible, - hud_control.lanesVisible, fcw_display, acc_alert, steer_required) - can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) + hud_control.lanesVisible, fcw_display, acc_alert, steer_required, hud_control.leadDistanceBars) + can_sends.extend(hondacan.create_ui_commands(self.packer, self.CAN, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud)) if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH: self.speed = pcm_speed + self.gas = pcm_accel / self.params.NIDEC_GAS_MAX - if not self.CP.enableGasInterceptor: - self.gas = pcm_accel / self.params.NIDEC_GAS_MAX - - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.speed = self.speed new_actuators.accel = self.accel new_actuators.gas = self.gas diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 9025f72397..c98d1a72d3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -5,7 +5,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus +from openpilot.selfdrive.car.honda.hondacan import CanBus, get_cruise_speed_conversion from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ HondaFlags @@ -28,7 +28,7 @@ def get_can_messages(CP, gearbox_msg): ("STEER_MOTOR_TORQUE", 0), # TODO: not on every car ] - if CP.carFingerprint == CAR.ODYSSEY_CHN: + if CP.carFingerprint == CAR.HONDA_ODYSSEY_CHN: messages += [ ("SCM_FEEDBACK", 25), ("SCM_BUTTONS", 50), @@ -39,7 +39,7 @@ def get_can_messages(CP, gearbox_msg): ("SCM_BUTTONS", 25), ] - if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): + if CP.carFingerprint in (CAR.HONDA_CRV_HYBRID, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): messages.append((gearbox_msg, 50)) else: messages.append((gearbox_msg, 100)) @@ -47,7 +47,7 @@ def get_can_messages(CP, gearbox_msg): if CP.flags & HondaFlags.BOSCH_ALT_BRAKE: messages.append(("BRAKE_MODULE", 50)) - if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): + if CP.carFingerprint in (HONDA_BOSCH | {CAR.HONDA_CIVIC, CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN}): messages.append(("EPB_STATUS", 50)) if CP.carFingerprint in HONDA_BOSCH: @@ -58,24 +58,20 @@ def get_can_messages(CP, gearbox_msg): ("ACC_CONTROL", 50), ] else: # Nidec signals - if CP.carFingerprint == CAR.ODYSSEY_CHN: + if CP.carFingerprint == CAR.HONDA_ODYSSEY_CHN: messages.append(("CRUISE_PARAMS", 10)) else: messages.append(("CRUISE_PARAMS", 50)) # TODO: clean this up - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, - CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): + if CP.carFingerprint in (CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CRV_HYBRID, CAR.HONDA_INSIGHT, + CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.HONDA_CIVIC_2022, CAR.HONDA_HRV_3G): pass - elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): + elif CP.carFingerprint in (CAR.HONDA_ODYSSEY_CHN, CAR.HONDA_FREED, CAR.HONDA_HRV): pass else: messages.append(("DOORS_STATUS", 3)) - # add gas interceptor reading if we are using it - if CP.enableGasInterceptor: - messages.append(("GAS_SENSOR", 50)) - if CP.carFingerprint in HONDA_BOSCH_RADARLESS: messages.append(("CRUISE_FAULT_STATUS", 50)) elif CP.openpilotLongitudinalControl: @@ -89,7 +85,7 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.gearbox_msg = "GEARBOX" - if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt: + if CP.carFingerprint == CAR.HONDA_ACCORD and CP.transmissionType == TransmissionType.cvt: self.gearbox_msg = "GEARBOX_15T" self.main_on_sig_msg = "SCM_FEEDBACK" @@ -129,10 +125,10 @@ class CarState(CarStateBase): # panda checks if the signal is non-zero ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5 # TODO: find a common signal across all cars - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, - CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G): + if self.CP.carFingerprint in (CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CRV_HYBRID, CAR.HONDA_INSIGHT, + CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.HONDA_CIVIC_2022, CAR.HONDA_HRV_3G): ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"]) - elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV): + elif self.CP.carFingerprint in (CAR.HONDA_ODYSSEY_CHN, CAR.HONDA_FREED, CAR.HONDA_HRV): ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"]) else: ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"], @@ -185,25 +181,24 @@ class CarState(CarStateBase): ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1 # TODO: set for all cars - if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}): + if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.HONDA_CIVIC, CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN}): ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0 gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) - if self.CP.enableGasInterceptor: - # Same threshold as panda, equivalent to 1e-5 with previous DBC scaling - ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 - ret.gasPressed = ret.gas > 492 - else: - ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] - ret.gasPressed = ret.gas > 1e-5 + ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] + ret.gasPressed = ret.gas > 1e-5 ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"] ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"] ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200) if self.CP.carFingerprint in HONDA_BOSCH: + # The PCM always manages its own cruise control state, but doesn't publish it + if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS: + ret.cruiseState.nonAdaptive = cp_cam.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0 + if not self.CP.openpilotLongitudinalControl: # ACC_HUD is on camera bus on radarless cars acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"] @@ -237,7 +232,7 @@ class CarState(CarStateBase): ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models - if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE): + if self.CP.carFingerprint in (CAR.HONDA_PILOT, CAR.HONDA_RIDGELINE): if ret.brake > 0.1: ret.brakePressed = True @@ -267,7 +262,7 @@ class CarState(CarStateBase): def get_can_parser(self, CP): messages = get_can_messages(CP, self.gearbox_msg) - return CANParser(DBC[CP.carFingerprint]["pt"], messages, get_pt_bus(CP.carFingerprint)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).pt) @staticmethod def get_cam_can_parser(CP): @@ -276,9 +271,10 @@ class CarState(CarStateBase): ] if CP.carFingerprint in HONDA_BOSCH_RADARLESS: - messages.append(("LKAS_HUD", 10)) - if not CP.openpilotLongitudinalControl: - messages.append(("ACC_HUD", 10)) + messages += [ + ("ACC_HUD", 10), + ("LKAS_HUD", 10), + ] elif CP.carFingerprint not in HONDA_BOSCH: messages += [ @@ -287,7 +283,7 @@ class CarState(CarStateBase): ("BRAKE_COMMAND", 50), ] - return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).camera) @staticmethod def get_body_can_parser(CP): @@ -296,6 +292,6 @@ class CarState(CarStateBase): ("BSM_STATUS_LEFT", 3), ("BSM_STATUS_RIGHT", 3), ] - bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) + bus_body = CanBus(CP).radar # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port) return CANParser(DBC[CP.carFingerprint]["body"], messages, bus_body) return None diff --git a/selfdrive/car/honda/fingerprints.py b/selfdrive/car/honda/fingerprints.py index 359ae83b15..8a5b79b41e 100644 --- a/selfdrive/car/honda/fingerprints.py +++ b/selfdrive/car/honda/fingerprints.py @@ -10,44 +10,10 @@ Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.ACCORD: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-6A0-8720\x00\x00', - b'37805-6A0-9520\x00\x00', - b'37805-6A0-9620\x00\x00', - b'37805-6A0-9720\x00\x00', - b'37805-6A0-A540\x00\x00', - b'37805-6A0-A550\x00\x00', - b'37805-6A0-A640\x00\x00', - b'37805-6A0-A650\x00\x00', - b'37805-6A0-A740\x00\x00', - b'37805-6A0-A750\x00\x00', - b'37805-6A0-A840\x00\x00', - b'37805-6A0-A850\x00\x00', - b'37805-6A0-A930\x00\x00', - b'37805-6A0-AF30\x00\x00', - b'37805-6A0-AG30\x00\x00', - b'37805-6A0-AJ10\x00\x00', - b'37805-6A0-C540\x00\x00', - b'37805-6A0-CG20\x00\x00', - b'37805-6A1-H650\x00\x00', - b'37805-6B2-A550\x00\x00', - b'37805-6B2-A560\x00\x00', - b'37805-6B2-A650\x00\x00', - b'37805-6B2-A660\x00\x00', - b'37805-6B2-A720\x00\x00', - b'37805-6B2-A810\x00\x00', - b'37805-6B2-A820\x00\x00', - b'37805-6B2-A920\x00\x00', - b'37805-6B2-AA10\x00\x00', - b'37805-6B2-C520\x00\x00', - b'37805-6B2-C540\x00\x00', - b'37805-6B2-M520\x00\x00', - b'37805-6B2-Y810\x00\x00', - b'37805-6M4-B730\x00\x00', - ], + CAR.HONDA_ACCORD: { (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TVC-A910\x00\x00', + b'54008-TWA-A910\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', @@ -89,6 +55,12 @@ FW_VERSIONS = { b'57114-TVA-C530\x00\x00', b'57114-TVA-E520\x00\x00', b'57114-TVE-H250\x00\x00', + b'57114-TWA-A040\x00\x00', + b'57114-TWA-A050\x00\x00', + b'57114-TWA-A530\x00\x00', + b'57114-TWA-B520\x00\x00', + b'57114-TWA-C510\x00\x00', + b'57114-TWB-H030\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBX-H120\x00\x00', @@ -100,6 +72,7 @@ FW_VERSIONS = { b'39990-TVA-X030\x00\x00', b'39990-TVA-X040\x00\x00', b'39990-TVE-H130\x00\x00', + b'39990-TWB-H120\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TBX-H230\x00\x00', @@ -108,40 +81,9 @@ FW_VERSIONS = { b'77959-TVA-H230\x00\x00', b'77959-TVA-L420\x00\x00', b'77959-TVA-X330\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TBX-H310\x00\x00', - b'78109-TVA-A010\x00\x00', - b'78109-TVA-A020\x00\x00', - b'78109-TVA-A030\x00\x00', - b'78109-TVA-A110\x00\x00', - b'78109-TVA-A120\x00\x00', - b'78109-TVA-A130\x00\x00', - b'78109-TVA-A210\x00\x00', - b'78109-TVA-A220\x00\x00', - b'78109-TVA-A230\x00\x00', - b'78109-TVA-A310\x00\x00', - b'78109-TVA-C010\x00\x00', - b'78109-TVA-C130\x00\x00', - b'78109-TVA-L010\x00\x00', - b'78109-TVA-L210\x00\x00', - b'78109-TVA-R310\x00\x00', - b'78109-TVC-A010\x00\x00', - b'78109-TVC-A020\x00\x00', - b'78109-TVC-A030\x00\x00', - b'78109-TVC-A110\x00\x00', - b'78109-TVC-A130\x00\x00', - b'78109-TVC-A210\x00\x00', - b'78109-TVC-A220\x00\x00', - b'78109-TVC-A230\x00\x00', - b'78109-TVC-C010\x00\x00', - b'78109-TVC-C110\x00\x00', - b'78109-TVC-L010\x00\x00', - b'78109-TVC-L210\x00\x00', - b'78109-TVC-M510\x00\x00', - b'78109-TVC-YF10\x00\x00', - b'78109-TVE-H610\x00\x00', - b'78109-TWA-A210\x00\x00', + b'77959-TWA-A440\x00\x00', + b'77959-TWA-L420\x00\x00', + b'77959-TWB-H220\x00\x00', ], (Ecu.hud, 0x18da61f1, None): [ b'78209-TVA-A010\x00\x00', @@ -158,6 +100,9 @@ FW_VERSIONS = { b'36802-TVE-H070\x00\x00', b'36802-TWA-A070\x00\x00', b'36802-TWA-A080\x00\x00', + b'36802-TWA-A210\x00\x00', + b'36802-TWA-A330\x00\x00', + b'36802-TWB-H060\x00\x00', ], (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TBX-H130\x00\x00', @@ -166,106 +111,19 @@ FW_VERSIONS = { b'36161-TVC-A330\x00\x00', b'36161-TVE-H050\x00\x00', b'36161-TWA-A070\x00\x00', + b'36161-TWA-A330\x00\x00', + b'36161-TWB-H040\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TVA-A010\x00\x00', b'38897-TVA-A020\x00\x00', b'38897-TVA-A230\x00\x00', b'38897-TVA-A240\x00\x00', - ], - }, - CAR.ACCORDH: { - (Ecu.gateway, 0x18daeff1, None): [ b'38897-TWA-A120\x00\x00', b'38897-TWD-J020\x00\x00', ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TWA-A040\x00\x00', - b'57114-TWA-A050\x00\x00', - b'57114-TWA-A530\x00\x00', - b'57114-TWA-B520\x00\x00', - b'57114-TWA-C510\x00\x00', - b'57114-TWB-H030\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TWA-A440\x00\x00', - b'77959-TWA-L420\x00\x00', - b'77959-TWB-H220\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TWA-A010\x00\x00', - b'78109-TWA-A020\x00\x00', - b'78109-TWA-A030\x00\x00', - b'78109-TWA-A110\x00\x00', - b'78109-TWA-A120\x00\x00', - b'78109-TWA-A130\x00\x00', - b'78109-TWA-A210\x00\x00', - b'78109-TWA-A220\x00\x00', - b'78109-TWA-A230\x00\x00', - b'78109-TWA-A610\x00\x00', - b'78109-TWA-H210\x00\x00', - b'78109-TWA-L010\x00\x00', - b'78109-TWA-L210\x00\x00', - ], - (Ecu.shiftByWire, 0x18da0bf1, None): [ - b'54008-TWA-A910\x00\x00', - ], - (Ecu.hud, 0x18da61f1, None): [ - b'78209-TVA-A010\x00\x00', - b'78209-TVA-A110\x00\x00', - ], - (Ecu.fwdCamera, 0x18dab5f1, None): [ - b'36161-TWA-A070\x00\x00', - b'36161-TWA-A330\x00\x00', - b'36161-TWB-H040\x00\x00', - ], - (Ecu.fwdRadar, 0x18dab0f1, None): [ - b'36802-TWA-A070\x00\x00', - b'36802-TWA-A080\x00\x00', - b'36802-TWA-A210\x00\x00', - b'36802-TWA-A330\x00\x00', - b'36802-TWB-H060\x00\x00', - ], - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TVA-A150\x00\x00', - b'39990-TVA-A160\x00\x00', - b'39990-TVA-A340\x00\x00', - b'39990-TWB-H120\x00\x00', - ], }, - CAR.CIVIC: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-5AA-A640\x00\x00', - b'37805-5AA-A650\x00\x00', - b'37805-5AA-A670\x00\x00', - b'37805-5AA-A680\x00\x00', - b'37805-5AA-A810\x00\x00', - b'37805-5AA-C640\x00\x00', - b'37805-5AA-C680\x00\x00', - b'37805-5AA-C820\x00\x00', - b'37805-5AA-L650\x00\x00', - b'37805-5AA-L660\x00\x00', - b'37805-5AA-L680\x00\x00', - b'37805-5AA-L690\x00\x00', - b'37805-5AA-L810\x00\x00', - b'37805-5AG-Q710\x00\x00', - b'37805-5AJ-A610\x00\x00', - b'37805-5AJ-A620\x00\x00', - b'37805-5AJ-L610\x00\x00', - b'37805-5BA-A310\x00\x00', - b'37805-5BA-A510\x00\x00', - b'37805-5BA-A740\x00\x00', - b'37805-5BA-A760\x00\x00', - b'37805-5BA-A930\x00\x00', - b'37805-5BA-A960\x00\x00', - b'37805-5BA-C640\x00\x00', - b'37805-5BA-C860\x00\x00', - b'37805-5BA-L410\x00\x00', - b'37805-5BA-L760\x00\x00', - b'37805-5BA-L930\x00\x00', - b'37805-5BA-L940\x00\x00', - b'37805-5BA-L960\x00\x00', - ], + CAR.HONDA_CIVIC: { (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A040\x00\x00', b'28101-5CG-A050\x00\x00', @@ -299,25 +157,6 @@ FW_VERSIONS = { b'77959-TBG-A030\x00\x00', b'77959-TEA-Q820\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TBA-A510\x00\x00', - b'78109-TBA-A520\x00\x00', - b'78109-TBA-A530\x00\x00', - b'78109-TBA-C310\x00\x00', - b'78109-TBA-C520\x00\x00', - b'78109-TBC-A310\x00\x00', - b'78109-TBC-A320\x00\x00', - b'78109-TBC-A510\x00\x00', - b'78109-TBC-A520\x00\x00', - b'78109-TBC-A530\x00\x00', - b'78109-TBC-C510\x00\x00', - b'78109-TBC-C520\x00\x00', - b'78109-TBC-C530\x00\x00', - b'78109-TBH-A510\x00\x00', - b'78109-TBH-A530\x00\x00', - b'78109-TED-Q510\x00\x00', - b'78109-TEG-A310\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TBA-A020\x00\x00', b'36161-TBA-A030\x00\x00', @@ -333,61 +172,7 @@ FW_VERSIONS = { b'38897-TBA-A020\x00\x00', ], }, - CAR.CIVIC_BOSCH: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-5AA-A940\x00\x00', - b'37805-5AA-A950\x00\x00', - b'37805-5AA-C950\x00\x00', - b'37805-5AA-L940\x00\x00', - b'37805-5AA-L950\x00\x00', - b'37805-5AG-Z910\x00\x00', - b'37805-5AJ-A750\x00\x00', - b'37805-5AJ-L750\x00\x00', - b'37805-5AK-T530\x00\x00', - b'37805-5AN-A750\x00\x00', - b'37805-5AN-A830\x00\x00', - b'37805-5AN-A840\x00\x00', - b'37805-5AN-A930\x00\x00', - b'37805-5AN-A940\x00\x00', - b'37805-5AN-A950\x00\x00', - b'37805-5AN-AG20\x00\x00', - b'37805-5AN-AH20\x00\x00', - b'37805-5AN-AJ30\x00\x00', - b'37805-5AN-AK10\x00\x00', - b'37805-5AN-AK20\x00\x00', - b'37805-5AN-AR10\x00\x00', - b'37805-5AN-AR20\x00\x00', - b'37805-5AN-C650\x00\x00', - b'37805-5AN-CH20\x00\x00', - b'37805-5AN-E630\x00\x00', - b'37805-5AN-E720\x00\x00', - b'37805-5AN-E820\x00\x00', - b'37805-5AN-J820\x00\x00', - b'37805-5AN-L840\x00\x00', - b'37805-5AN-L930\x00\x00', - b'37805-5AN-L940\x00\x00', - b'37805-5AN-LF20\x00\x00', - b'37805-5AN-LH20\x00\x00', - b'37805-5AN-LJ20\x00\x00', - b'37805-5AN-LR20\x00\x00', - b'37805-5AN-LS20\x00\x00', - b'37805-5AW-G720\x00\x00', - b'37805-5AZ-E850\x00\x00', - b'37805-5AZ-G540\x00\x00', - b'37805-5AZ-G740\x00\x00', - b'37805-5AZ-G840\x00\x00', - b'37805-5BB-A530\x00\x00', - b'37805-5BB-A540\x00\x00', - b'37805-5BB-A620\x00\x00', - b'37805-5BB-A630\x00\x00', - b'37805-5BB-A640\x00\x00', - b'37805-5BB-C540\x00\x00', - b'37805-5BB-C630\x00\x00', - b'37805-5BB-C640\x00\x00', - b'37805-5BB-L540\x00\x00', - b'37805-5BB-L630\x00\x00', - b'37805-5BB-L640\x00\x00', - ], + CAR.HONDA_CIVIC_BOSCH: { (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', b'28101-5CG-AB10\x00\x00', @@ -420,6 +205,7 @@ FW_VERSIONS = { b'57114-TGG-G320\x00\x00', b'57114-TGG-L320\x00\x00', b'57114-TGG-L330\x00\x00', + b'57114-TGH-L130\x00\x00', b'57114-TGK-T320\x00\x00', b'57114-TGL-G330\x00\x00', ], @@ -433,6 +219,7 @@ FW_VERSIONS = { b'39990-TGG-A020\x00\x00', b'39990-TGG-A120\x00\x00', b'39990-TGG-J510\x00\x00', + b'39990-TGH-J530\x00\x00', b'39990-TGL-E130\x00\x00', b'39990-TGN-E120\x00\x00', ], @@ -447,40 +234,7 @@ FW_VERSIONS = { b'77959-TGG-G110\x00\x00', b'77959-TGG-J320\x00\x00', b'77959-TGG-Z820\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TBA-A110\x00\x00', - b'78109-TBA-A910\x00\x00', - b'78109-TBA-C340\x00\x00', - b'78109-TBA-C910\x00\x00', - b'78109-TBC-A740\x00\x00', - b'78109-TBC-C540\x00\x00', - b'78109-TBG-A110\x00\x00', - b'78109-TBH-A710\x00\x00', - b'78109-TEG-A720\x00\x00', - b'78109-TFJ-G020\x00\x00', - b'78109-TGG-9020\x00\x00', - b'78109-TGG-A210\x00\x00', - b'78109-TGG-A220\x00\x00', - b'78109-TGG-A310\x00\x00', - b'78109-TGG-A320\x00\x00', - b'78109-TGG-A330\x00\x00', - b'78109-TGG-A610\x00\x00', - b'78109-TGG-A620\x00\x00', - b'78109-TGG-A810\x00\x00', - b'78109-TGG-A820\x00\x00', - b'78109-TGG-C010\x00\x00', - b'78109-TGG-C220\x00\x00', - b'78109-TGG-E110\x00\x00', - b'78109-TGG-G030\x00\x00', - b'78109-TGG-G230\x00\x00', - b'78109-TGG-G410\x00\x00', - b'78109-TGK-Z410\x00\x00', - b'78109-TGL-G120\x00\x00', - b'78109-TGL-G130\x00\x00', - b'78109-TGL-G210\x00\x00', - b'78109-TGL-G230\x00\x00', - b'78109-TGL-GM10\x00\x00', + b'77959-TGH-J110\x00\x00', ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', @@ -492,6 +246,7 @@ FW_VERSIONS = { b'36802-TGG-A130\x00\x00', b'36802-TGG-G040\x00\x00', b'36802-TGG-G130\x00\x00', + b'36802-TGH-A140\x00\x00', b'36802-TGK-Q120\x00\x00', b'36802-TGL-G040\x00\x00', ], @@ -506,6 +261,7 @@ FW_VERSIONS = { b'36161-TGG-G070\x00\x00', b'36161-TGG-G130\x00\x00', b'36161-TGG-G140\x00\x00', + b'36161-TGH-A140\x00\x00', b'36161-TGK-Q120\x00\x00', b'36161-TGL-G050\x00\x00', b'36161-TGL-G070\x00\x00', @@ -513,16 +269,13 @@ FW_VERSIONS = { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A020\x00\x00', b'38897-TBA-A110\x00\x00', + b'38897-TGH-A010\x00\x00', ], (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'39494-TGL-G030\x00\x00', ], }, - CAR.CIVIC_BOSCH_DIESEL: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-59N-G630\x00\x00', - b'37805-59N-G830\x00\x00', - ], + CAR.HONDA_CIVIC_BOSCH_DIESEL: { (Ecu.transmission, 0x18da1ef1, None): [ b'28101-59Y-G220\x00\x00', b'28101-59Y-G620\x00\x00', @@ -537,10 +290,6 @@ FW_VERSIONS = { b'77959-TFK-G210\x00\x00', b'77959-TGN-G220\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TFK-G020\x00\x00', - b'78109-TGN-G120\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TFK-G130\x00\x00', b'36802-TGN-G130\x00\x00', @@ -556,7 +305,7 @@ FW_VERSIONS = { b'38897-TBA-A020\x00\x00', ], }, - CAR.CRV: { + CAR.HONDA_CRV: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-T1W-A230\x00\x00', b'57114-T1W-A240\x00\x00', @@ -565,52 +314,13 @@ FW_VERSIONS = { (Ecu.srs, 0x18da53f1, None): [ b'77959-T0A-A230\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-T1W-A210\x00\x00', - b'78109-T1W-C210\x00\x00', - b'78109-T1X-A210\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T1W-A830\x00\x00', b'36161-T1W-C830\x00\x00', b'36161-T1X-A830\x00\x00', ], }, - CAR.CRV_5G: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-5PA-3060\x00\x00', - b'37805-5PA-3080\x00\x00', - b'37805-5PA-3180\x00\x00', - b'37805-5PA-4050\x00\x00', - b'37805-5PA-4150\x00\x00', - b'37805-5PA-6520\x00\x00', - b'37805-5PA-6530\x00\x00', - b'37805-5PA-6630\x00\x00', - b'37805-5PA-6640\x00\x00', - b'37805-5PA-7630\x00\x00', - b'37805-5PA-9530\x00\x00', - b'37805-5PA-9630\x00\x00', - b'37805-5PA-9640\x00\x00', - b'37805-5PA-9730\x00\x00', - b'37805-5PA-9830\x00\x00', - b'37805-5PA-9840\x00\x00', - b'37805-5PA-A650\x00\x00', - b'37805-5PA-A670\x00\x00', - b'37805-5PA-A680\x00\x00', - b'37805-5PA-A850\x00\x00', - b'37805-5PA-A870\x00\x00', - b'37805-5PA-A880\x00\x00', - b'37805-5PA-A890\x00\x00', - b'37805-5PA-AB10\x00\x00', - b'37805-5PA-AD10\x00\x00', - b'37805-5PA-AF20\x00\x00', - b'37805-5PA-AH20\x00\x00', - b'37805-5PA-BF10\x00\x00', - b'37805-5PA-C680\x00\x00', - b'37805-5PD-Q630\x00\x00', - b'37805-5PF-F730\x00\x00', - b'37805-5PF-M630\x00\x00', - ], + CAR.HONDA_CRV_5G: { (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5RG-A020\x00\x00', b'28101-5RG-A030\x00\x00', @@ -649,25 +359,6 @@ FW_VERSIONS = { b'46114-TLA-A930\x00\x00', b'46114-TMC-U020\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TLA-A020\x00\x00', - b'78109-TLA-A110\x00\x00', - b'78109-TLA-A120\x00\x00', - b'78109-TLA-A210\x00\x00', - b'78109-TLA-A220\x00\x00', - b'78109-TLA-C020\x00\x00', - b'78109-TLA-C110\x00\x00', - b'78109-TLA-C210\x00\x00', - b'78109-TLA-C310\x00\x00', - b'78109-TLB-A020\x00\x00', - b'78109-TLB-A110\x00\x00', - b'78109-TLB-A120\x00\x00', - b'78109-TLB-A210\x00\x00', - b'78109-TLB-A220\x00\x00', - b'78109-TMC-Q210\x00\x00', - b'78109-TMM-F210\x00\x00', - b'78109-TMM-M110\x00\x00', - ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TLA-A010\x00\x00', b'38897-TLA-A110\x00\x00', @@ -704,11 +395,7 @@ FW_VERSIONS = { b'77959-TMM-F040\x00\x00', ], }, - CAR.CRV_EU: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-R5Z-G740\x00\x00', - b'37805-R5Z-G780\x00\x00', - ], + CAR.HONDA_CRV_EU: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-T1V-G920\x00\x00', ], @@ -722,15 +409,11 @@ FW_VERSIONS = { b'28101-5LH-E120\x00\x00', b'28103-5LH-E100\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-T1B-3050\x00\x00', - b'78109-T1V-G020\x00\x00', - ], (Ecu.srs, 0x18da53f1, None): [ b'77959-T1G-G940\x00\x00', ], }, - CAR.CRV_HYBRID: { + CAR.HONDA_CRV_HYBRID: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-TMB-H030\x00\x00', b'57114-TPA-G020\x00\x00', @@ -756,12 +439,6 @@ FW_VERSIONS = { b'36161-TPG-A030\x00\x00', b'36161-TPG-A040\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TMB-H220\x00\x00', - b'78109-TPA-G520\x00\x00', - b'78109-TPG-A110\x00\x00', - b'78109-TPG-A210\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TMB-H040\x00\x00', b'36802-TPA-E040\x00\x00', @@ -775,7 +452,7 @@ FW_VERSIONS = { b'77959-TLA-H240\x00\x00', ], }, - CAR.FIT: { + CAR.HONDA_FIT: { (Ecu.vsa, 0x18da28f1, None): [ b'57114-T5R-L020\x00\x00', b'57114-T5R-L220\x00\x00', @@ -787,12 +464,6 @@ FW_VERSIONS = { (Ecu.gateway, 0x18daeff1, None): [ b'38897-T5A-J010\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-T5A-A210\x00\x00', - b'78109-T5A-A410\x00\x00', - b'78109-T5A-A420\x00\x00', - b'78109-T5A-A910\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T5R-A040\x00\x00', b'36161-T5R-A240\x00\x00', @@ -802,7 +473,7 @@ FW_VERSIONS = { b'77959-T5R-A230\x00\x00', ], }, - CAR.FREED: { + CAR.HONDA_FREED: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TDK-J010\x00\x00', ], @@ -814,37 +485,17 @@ FW_VERSIONS = { b'57114-TDK-J120\x00\x00', b'57114-TDK-J330\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TDK-J310\x00\x00', - b'78109-TDK-J320\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-TDK-J070\x00\x00', b'36161-TDK-J080\x00\x00', b'36161-TDK-J530\x00\x00', ], }, - CAR.ODYSSEY: { + CAR.HONDA_ODYSSEY: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-THR-A010\x00\x00', b'38897-THR-A020\x00\x00', ], - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-5MR-3050\x00\x00', - b'37805-5MR-3250\x00\x00', - b'37805-5MR-4070\x00\x00', - b'37805-5MR-4080\x00\x00', - b'37805-5MR-4180\x00\x00', - b'37805-5MR-A240\x00\x00', - b'37805-5MR-A250\x00\x00', - b'37805-5MR-A310\x00\x00', - b'37805-5MR-A740\x00\x00', - b'37805-5MR-A750\x00\x00', - b'37805-5MR-A840\x00\x00', - b'37805-5MR-C620\x00\x00', - b'37805-5MR-D530\x00\x00', - b'37805-5MR-K730\x00\x00', - ], (Ecu.eps, 0x18da30f1, None): [ b'39990-THR-A020\x00\x00', b'39990-THR-A030\x00\x00', @@ -889,53 +540,17 @@ FW_VERSIONS = { b'57114-THR-A040\x00\x00', b'57114-THR-A110\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-THR-A220\x00\x00', - b'78109-THR-A230\x00\x00', - b'78109-THR-A420\x00\x00', - b'78109-THR-A430\x00\x00', - b'78109-THR-A720\x00\x00', - b'78109-THR-A730\x00\x00', - b'78109-THR-A820\x00\x00', - b'78109-THR-A830\x00\x00', - b'78109-THR-AB20\x00\x00', - b'78109-THR-AB30\x00\x00', - b'78109-THR-AB40\x00\x00', - b'78109-THR-AC20\x00\x00', - b'78109-THR-AC30\x00\x00', - b'78109-THR-AC40\x00\x00', - b'78109-THR-AC50\x00\x00', - b'78109-THR-AD30\x00\x00', - b'78109-THR-AE20\x00\x00', - b'78109-THR-AE30\x00\x00', - b'78109-THR-AE40\x00\x00', - b'78109-THR-AK10\x00\x00', - b'78109-THR-AL10\x00\x00', - b'78109-THR-AN10\x00\x00', - b'78109-THR-C220\x00\x00', - b'78109-THR-C320\x00\x00', - b'78109-THR-C330\x00\x00', - b'78109-THR-CE20\x00\x00', - b'78109-THR-CL10\x00\x00', - b'78109-THR-DA20\x00\x00', - b'78109-THR-DA30\x00\x00', - b'78109-THR-DA40\x00\x00', - b'78109-THR-K120\x00\x00', - ], (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-THR-A020\x00\x00', ], }, - CAR.ODYSSEY_CHN: { + CAR.HONDA_ODYSSEY_CHN: { (Ecu.eps, 0x18da30f1, None): [ b'39990-T6D-H220\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-T6A-J010\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-T6A-F310\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T6A-P040\x00\x00', ], @@ -943,12 +558,13 @@ FW_VERSIONS = { b'77959-T6A-P110\x00\x00', ], }, - CAR.PILOT: { + CAR.HONDA_PILOT: { (Ecu.shiftByWire, 0x18da0bf1, None): [ b'54008-TG7-A520\x00\x00', b'54008-TG7-A530\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ + b'28101-5EY-A040\x00\x00', b'28101-5EY-A050\x00\x00', b'28101-5EY-A100\x00\x00', b'28101-5EY-A430\x00\x00', @@ -964,36 +580,6 @@ FW_VERSIONS = { b'28101-5EZ-A700\x00\x00', b'28103-5EY-A110\x00\x00', ], - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-RLV-4060\x00\x00', - b'37805-RLV-4070\x00\x00', - b'37805-RLV-5140\x00\x00', - b'37805-RLV-5230\x00\x00', - b'37805-RLV-A830\x00\x00', - b'37805-RLV-A840\x00\x00', - b'37805-RLV-B210\x00\x00', - b'37805-RLV-B220\x00\x00', - b'37805-RLV-B420\x00\x00', - b'37805-RLV-B430\x00\x00', - b'37805-RLV-B620\x00\x00', - b'37805-RLV-B710\x00\x00', - b'37805-RLV-B720\x00\x00', - b'37805-RLV-C430\x00\x00', - b'37805-RLV-C510\x00\x00', - b'37805-RLV-C520\x00\x00', - b'37805-RLV-C530\x00\x00', - b'37805-RLV-C910\x00\x00', - b'37805-RLV-F120\x00\x00', - b'37805-RLV-L080\x00\x00', - b'37805-RLV-L090\x00\x00', - b'37805-RLV-L150\x00\x00', - b'37805-RLV-L160\x00\x00', - b'37805-RLV-L180\x00\x00', - b'37805-RLV-L350\x00\x00', - b'37805-RLV-L410\x00\x00', - b'37805-RLV-L430\x00\x00', - b'37805-RLV-L850\x00\x00', - ], (Ecu.gateway, 0x18daeff1, None): [ b'38897-TG7-A030\x00\x00', b'38897-TG7-A040\x00\x00', @@ -1039,43 +625,6 @@ FW_VERSIONS = { b'77959-TGS-A010\x00\x00', b'77959-TGS-A110\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TG7-A040\x00\x00', - b'78109-TG7-A050\x00\x00', - b'78109-TG7-A420\x00\x00', - b'78109-TG7-A520\x00\x00', - b'78109-TG7-A720\x00\x00', - b'78109-TG7-AJ10\x00\x00', - b'78109-TG7-AJ20\x00\x00', - b'78109-TG7-AK10\x00\x00', - b'78109-TG7-AK20\x00\x00', - b'78109-TG7-AM20\x00\x00', - b'78109-TG7-AP10\x00\x00', - b'78109-TG7-AP20\x00\x00', - b'78109-TG7-AS20\x00\x00', - b'78109-TG7-AT20\x00\x00', - b'78109-TG7-AU20\x00\x00', - b'78109-TG7-AX20\x00\x00', - b'78109-TG7-D020\x00\x00', - b'78109-TG7-DJ10\x00\x00', - b'78109-TG7-YK20\x00\x00', - b'78109-TG8-A420\x00\x00', - b'78109-TG8-A520\x00\x00', - b'78109-TG8-AJ10\x00\x00', - b'78109-TG8-AJ20\x00\x00', - b'78109-TG8-AK20\x00\x00', - b'78109-TG8-AS20\x00\x00', - b'78109-TGS-AB10\x00\x00', - b'78109-TGS-AC10\x00\x00', - b'78109-TGS-AD10\x00\x00', - b'78109-TGS-AJ20\x00\x00', - b'78109-TGS-AK20\x00\x00', - b'78109-TGS-AP20\x00\x00', - b'78109-TGS-AT20\x00\x00', - b'78109-TGS-AX20\x00\x00', - b'78109-TGT-AJ20\x00\x00', - b'78109-TGT-AK30\x00\x00', - ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A130\x00\x00', b'57114-TG7-A140\x00\x00', @@ -1084,6 +633,7 @@ FW_VERSIONS = { b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', b'57114-TG8-A140\x00\x00', + b'57114-TG8-A230\x00\x00', b'57114-TG8-A240\x00\x00', b'57114-TG8-A630\x00\x00', b'57114-TG8-A730\x00\x00', @@ -1105,32 +655,8 @@ FW_VERSIONS = { b'77959-TX4-C010\x00\x00', b'77959-TX4-C020\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TX4-A210\x00\x00', - b'78109-TX4-A310\x00\x00', - b'78109-TX5-A210\x00\x00', - b'78109-TX5-A310\x00\x00', - ], }, CAR.ACURA_RDX_3G: { - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-5YF-A130\x00\x00', - b'37805-5YF-A230\x00\x00', - b'37805-5YF-A320\x00\x00', - b'37805-5YF-A330\x00\x00', - b'37805-5YF-A420\x00\x00', - b'37805-5YF-A430\x00\x00', - b'37805-5YF-A750\x00\x00', - b'37805-5YF-A760\x00\x00', - b'37805-5YF-A850\x00\x00', - b'37805-5YF-A870\x00\x00', - b'37805-5YF-AD20\x00\x00', - b'37805-5YF-C210\x00\x00', - b'37805-5YF-C220\x00\x00', - b'37805-5YF-C410\x00\x00', - b'37805-5YF-C420\x00\x00', - b'37805-5YF-C430\x00\x00', - ], (Ecu.vsa, 0x18da28f1, None): [ b'57114-TJB-A030\x00\x00', b'57114-TJB-A040\x00\x00', @@ -1160,24 +686,6 @@ FW_VERSIONS = { b'28102-5YL-A700\x00\x00', b'28102-5YL-A711\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TJB-A140\x00\x00', - b'78109-TJB-A240\x00\x00', - b'78109-TJB-A420\x00\x00', - b'78109-TJB-AB10\x00\x00', - b'78109-TJB-AD10\x00\x00', - b'78109-TJB-AF10\x00\x00', - b'78109-TJB-AQ20\x00\x00', - b'78109-TJB-AR10\x00\x00', - b'78109-TJB-AS10\x00\x00', - b'78109-TJB-AU10\x00\x00', - b'78109-TJB-AW10\x00\x00', - b'78109-TJC-A240\x00\x00', - b'78109-TJC-A420\x00\x00', - b'78109-TJC-AA10\x00\x00', - b'78109-TJC-AD10\x00\x00', - b'78109-TJC-AF10\x00\x00', - ], (Ecu.srs, 0x18da53f1, None): [ b'77959-TJB-A040\x00\x00', b'77959-TJB-A120\x00\x00', @@ -1202,7 +710,7 @@ FW_VERSIONS = { b'39990-TJB-A130\x00\x00', ], }, - CAR.RIDGELINE: { + CAR.HONDA_RIDGELINE: { (Ecu.eps, 0x18da30f1, None): [ b'39990-T6Z-A020\x00\x00', b'39990-T6Z-A030\x00\x00', @@ -1222,18 +730,6 @@ FW_VERSIONS = { b'38897-T6Z-A010\x00\x00', b'38897-T6Z-A110\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78108-T6Z-AF10\x00\x00', - b'78109-T6Z-A420\x00\x00', - b'78109-T6Z-A510\x00\x00', - b'78109-T6Z-A710\x00\x00', - b'78109-T6Z-A810\x00\x00', - b'78109-T6Z-A910\x00\x00', - b'78109-T6Z-AA10\x00\x00', - b'78109-T6Z-C620\x00\x00', - b'78109-T6Z-C910\x00\x00', - b'78109-TJZ-A510\x00\x00', - ], (Ecu.srs, 0x18da53f1, None): [ b'77959-T6Z-A020\x00\x00', ], @@ -1245,7 +741,7 @@ FW_VERSIONS = { b'57114-TJZ-A520\x00\x00', ], }, - CAR.INSIGHT: { + CAR.HONDA_INSIGHT: { (Ecu.eps, 0x18da30f1, None): [ b'39990-TXM-A040\x00\x00', ], @@ -1270,15 +766,8 @@ FW_VERSIONS = { (Ecu.gateway, 0x18daeff1, None): [ b'38897-TXM-A020\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TXM-A010\x00\x00', - b'78109-TXM-A020\x00\x00', - b'78109-TXM-A030\x00\x00', - b'78109-TXM-A110\x00\x00', - b'78109-TXM-C010\x00\x00', - ], }, - CAR.HRV: { + CAR.HONDA_HRV: { (Ecu.gateway, 0x18daeff1, None): [ b'38897-T7A-A010\x00\x00', b'38897-T7A-A110\x00\x00', @@ -1295,36 +784,32 @@ FW_VERSIONS = { (Ecu.srs, 0x18da53f1, None): [ b'77959-T7A-A230\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-THW-A110\x00\x00', - b'78109-THX-A110\x00\x00', - b'78109-THX-A120\x00\x00', - b'78109-THX-A210\x00\x00', - b'78109-THX-A220\x00\x00', - b'78109-THX-C220\x00\x00', - ], }, - CAR.HRV_3G: { + CAR.HONDA_HRV_3G: { (Ecu.eps, 0x18da30f1, None): [ + b'39990-3M0-G110\x00\x00', b'39990-3W0-A030\x00\x00', ], (Ecu.gateway, 0x18daeff1, None): [ + b'38897-3M0-M110\x00\x00', b'38897-3W1-A010\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ + b'77959-3M0-K840\x00\x00', b'77959-3V0-A820\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78108-3V1-A220\x00\x00', + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'8S102-3M6-P030\x00\x00', + b'8S102-3W0-A060\x00\x00', + b'8S102-3W0-AB10\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ + b'57114-3M6-M010\x00\x00', b'57114-3W0-A040\x00\x00', ], (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6EH-A010\x00\x00', - ], - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-6CT-A710\x00\x00', + b'28101-6JC-M310\x00\x00', ], (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'46114-3W0-A020\x00\x00', @@ -1342,11 +827,6 @@ FW_VERSIONS = { b'77959-TX6-A230\x00\x00', b'77959-TX6-C210\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-T3R-A120\x00\x00', - b'78109-T3R-A410\x00\x00', - b'78109-TV9-A510\x00\x00', - ], }, CAR.HONDA_E: { (Ecu.eps, 0x18da30f1, None): [ @@ -1361,9 +841,6 @@ FW_VERSIONS = { (Ecu.srs, 0x18da53f1, None): [ b'77959-TYF-G430\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78108-TYF-G610\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TYF-E030\x00\x00', ], @@ -1374,7 +851,7 @@ FW_VERSIONS = { b'57114-TYF-E030\x00\x00', ], }, - CAR.CIVIC_2022: { + CAR.HONDA_CIVIC_2022: { (Ecu.eps, 0x18da30f1, None): [ b'39990-T24-T120\x00\x00', b'39990-T39-A130\x00\x00', @@ -1395,15 +872,6 @@ FW_VERSIONS = { b'77959-T47-A940\x00\x00', b'77959-T47-A950\x00\x00', ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78108-T21-A220\x00\x00', - b'78108-T21-A230\x00\x00', - b'78108-T21-A620\x00\x00', - b'78108-T21-A740\x00\x00', - b'78108-T21-MB10\x00\x00', - b'78108-T22-A020\x00\x00', - b'78108-T23-A110\x00\x00', - ], (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36161-T20-A060\x00\x00', b'36161-T20-A070\x00\x00', @@ -1423,14 +891,5 @@ FW_VERSIONS = { b'28101-65H-A120\x00\x00', b'28101-65J-N010\x00\x00', ], - (Ecu.programmedFuelInjection, 0x18da10f1, None): [ - b'37805-64A-A540\x00\x00', - b'37805-64A-A620\x00\x00', - b'37805-64D-P510\x00\x00', - b'37805-64L-A540\x00\x00', - b'37805-64S-A540\x00\x00', - b'37805-64S-A720\x00\x00', - b'37805-64S-AA10\x00\x00', - ], }, } diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index a8cbad78ce..1be496d951 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,4 +1,5 @@ from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CanBusBase from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams # CAN bus layout with relay @@ -8,15 +9,34 @@ from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_ # 3 = F-CAN A - OBDII port -def get_pt_bus(car_fingerprint): - return 1 if car_fingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) else 0 +class CanBus(CanBusBase): + def __init__(self, CP=None, fingerprint=None) -> None: + # use fingerprint if specified + super().__init__(CP if fingerprint is None else None, fingerprint) + if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS): + self._pt, self._radar = self.offset + 1, self.offset + else: + self._pt, self._radar = self.offset, self.offset + 1 + + @property + def pt(self) -> int: + return self._pt + + @property + def radar(self) -> int: + return self._radar -def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False): + @property + def camera(self) -> int: + return self.offset + 2 + + +def get_lkas_cmd_bus(CAN, car_fingerprint, radar_disabled=False): no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS if radar_disabled or no_radar: # when radar is disabled, steering commands are sent directly to powertrain bus - return get_pt_bus(car_fingerprint) + return CAN.pt # normally steering commands are sent to radar, which forwards them to powertrain bus return 0 @@ -26,7 +46,7 @@ def get_cruise_speed_conversion(car_fingerprint: str, is_metric: bool) -> float: return CV.MPH_TO_MS if car_fingerprint in HONDA_BOSCH_RADARLESS and not is_metric else CV.KPH_TO_MS -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): +def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -47,13 +67,11 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ "AEB_REQ_2": 0, "AEB_STATUS": 0, } - bus = get_pt_bus(car_fingerprint) - return packer.make_can_msg("BRAKE_COMMAND", bus, values) + return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values) -def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint): +def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint): commands = [] - bus = get_pt_bus(car_fingerprint) min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0] control_on = 5 if enabled else 0 @@ -90,43 +108,43 @@ def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, c "SET_TO_75": 0x75, "SET_TO_30": 0x30, } - commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values)) + commands.append(packer.make_can_msg("ACC_CONTROL_ON", CAN.pt, acc_control_on_values)) - commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values)) + commands.append(packer.make_can_msg("ACC_CONTROL", CAN.pt, acc_control_values)) return commands -def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled): +def create_steering_control(packer, CAN, apply_steer, lkas_active, car_fingerprint, radar_disabled): values = { "STEER_TORQUE": apply_steer if lkas_active else 0, "STEER_TORQUE_REQUEST": lkas_active, } - bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled) + bus = get_lkas_cmd_bus(CAN, car_fingerprint, radar_disabled) return packer.make_can_msg("STEERING_CONTROL", bus, values) -def create_bosch_supplemental_1(packer, car_fingerprint): +def create_bosch_supplemental_1(packer, CAN, car_fingerprint): # non-active params values = { "SET_ME_X04": 0x04, "SET_ME_X80": 0x80, "SET_ME_X10": 0x10, } - bus = get_lkas_cmd_bus(car_fingerprint) + bus = get_lkas_cmd_bus(CAN, car_fingerprint) return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values) -def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): +def create_ui_commands(packer, CAN, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud): commands = [] - bus_pt = get_pt_bus(CP.carFingerprint) radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl - bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled) + bus_lkas = get_lkas_cmd_bus(CAN, CP.carFingerprint, radar_disabled) if CP.openpilotLongitudinalControl: acc_hud_values = { 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': 1 if enabled else 0, - 'HUD_DISTANCE': 0, # max distance setting on display + # only moves the lead car without ACC_ON + 'HUD_DISTANCE': (hud.lead_distance_bars + 1) % 4, # wraps to 0 at 4 bars 'IMPERIAL_UNIT': int(not is_metric), 'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0, 'SET_ME_X01_2': 1, @@ -137,6 +155,8 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values['FCM_OFF'] = 1 acc_hud_values['FCM_OFF_2'] = 1 else: + # Shows the distance bars, TODO: stock camera shows updates temporarily while disabled + acc_hud_values['ACC_ON'] = int(enabled) acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH acc_hud_values['PCM_GAS'] = hud.pcm_accel acc_hud_values['SET_ME_X01'] = 1 @@ -144,7 +164,7 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2'] acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM'] acc_hud_values['ICONS'] = acc_hud['ICONS'] - commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values)) + commands.append(packer.make_can_msg("ACC_HUD", CAN.pt, acc_hud_values)) lkas_hud_values = { 'SET_ME_X41': 0x41, @@ -173,19 +193,19 @@ def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, 'CMBS_OFF': 0x01, 'SET_TO_1': 0x01, } - commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values)) + commands.append(packer.make_can_msg('RADAR_HUD', CAN.pt, radar_hud_values)) - if CP.carFingerprint == CAR.CIVIC_BOSCH: - commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {})) + if CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH: + commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", CAN.pt, {})) return commands -def spam_buttons_command(packer, button_val, car_fingerprint): +def spam_buttons_command(packer, CAN, button_val, car_fingerprint): values = { 'CRUISE_BUTTONS': button_val, 'CRUISE_SETTING': 0, } # send buttons to camera on radarless cars - bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint) + bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt return packer.make_can_msg("SCM_BUTTONS", bus, values) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 153fa1e635..2026c385c2 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -3,9 +3,9 @@ from cereal import car from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.common.numpy_fast import interp -from openpilot.selfdrive.car.honda.hondacan import get_pt_bus -from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \ - HONDA_BOSCH_RADARLESS +from openpilot.selfdrive.car.honda.hondacan import CanBus +from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CruiseSettings, HondaFlags, CAR, HONDA_BOSCH, \ + HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.disable_ecu import disable_ecu @@ -16,6 +16,7 @@ EventName = car.CarEvent.EventName TransmissionType = car.CarParams.TransmissionType BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise, CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel} +SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.altButton1} class CarInterface(CarInterfaceBase): @@ -23,8 +24,6 @@ class CarInterface(CarInterfaceBase): def get_pid_accel_limits(CP, current_speed, cruise_speed): if CP.carFingerprint in HONDA_BOSCH: return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX - elif CP.enableGasInterceptor: - return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX else: # NIDECs don't allow acceleration near cruise_speed, # so limit limits of pid to prevent windup @@ -36,6 +35,8 @@ class CarInterface(CarInterfaceBase): def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "honda" + CAN = CanBus(ret, fingerprint) + if candidate in HONDA_BOSCH: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)] ret.radarUnavailable = True @@ -47,20 +48,19 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = not ret.openpilotLongitudinalControl else: ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)] - ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = True - ret.pcmCruise = not ret.enableGasInterceptor + ret.pcmCruise = True - if candidate == CAR.CRV_5G: - ret.enableBsm = 0x12f8bfa7 in fingerprint[0] + if candidate == CAR.HONDA_CRV_5G: + ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar] # Detect Bosch cars with new HUD msgs if any(0x33DA in f for f in fingerprint.values()): ret.flags |= HondaFlags.BOSCH_EXT_HUD.value - # Accord 1.5T CVT has different gearbox message - if candidate == CAR.ACCORD and 0x191 in fingerprint[1]: + # Accord ICE 1.5T CVT has different gearbox message + if candidate == CAR.HONDA_ACCORD and 0x191 in fingerprint[CAN.pt]: ret.transmissionType = TransmissionType.cvt # Certain Hondas have an extra steering sensor at the bottom of the steering rack, @@ -89,11 +89,7 @@ class CarInterface(CarInterfaceBase): if fw.ecu == "eps" and b"," in fw.fwVersion: eps_modified = True - if candidate == CAR.CIVIC: - 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 candidate == CAR.HONDA_CIVIC: if eps_modified: # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 @@ -107,21 +103,12 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] - elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022): - ret.mass = 1326. - ret.wheelbase = 2.70 - ret.centerToFront = ret.wheelbase * 0.4 - ret.steerRatio = 15.38 # 10.93 is end-to-end spec + elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL, CAR.HONDA_CIVIC_2022): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # 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 in (CAR.ACCORD, CAR.ACCORDH): - 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 + elif candidate == CAR.HONDA_ACCORD: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - ret.tireStiffnessFactor = 0.8467 if eps_modified: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] @@ -129,29 +116,15 @@ 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 - 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 - 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 - ret.wheelbase = 2.62 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.89 # as spec + elif candidate in (CAR.HONDA_CRV, CAR.HONDA_CRV_EU): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end - 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 - ret.wheelbase = 2.66 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 # 12.3 is spec end-to-end + elif candidate == CAR.HONDA_CRV_5G: if eps_modified: # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 @@ -161,123 +134,69 @@ 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]] - ret.tireStiffnessFactor = 0.677 ret.wheelSpeedFactor = 1.025 - elif candidate == CAR.CRV_HYBRID: - 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 + elif candidate == CAR.HONDA_CRV_HYBRID: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - 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 - ret.wheelbase = 2.53 - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 13.06 + elif candidate == CAR.HONDA_FIT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - 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 - ret.wheelbase = 2.74 - # the remaining parameters were copied from FIT - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 13.06 + elif candidate == CAR.HONDA_FREED: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] - 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 - ret.wheelbase = 2.61 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.2 + elif candidate in (CAR.HONDA_HRV, CAR.HONDA_HRV_3G): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] - ret.tireStiffnessFactor = 0.5 - if candidate == CAR.HRV: + if candidate == CAR.HONDA_HRV: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]] ret.wheelSpeedFactor = 1.025 else: 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 - ret.wheelbase = 2.68 - ret.centerToFront = ret.wheelbase * 0.38 - ret.steerRatio = 15.0 # as spec - 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 - 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]] - ret.tireStiffnessFactor = 0.677 - - elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): - ret.mass = 1900. - ret.wheelbase = 3.00 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 14.35 # as spec - ret.tireStiffnessFactor = 0.82 + + elif candidate in (CAR.HONDA_ODYSSEY, CAR.HONDA_ODYSSEY_CHN): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] - if candidate == CAR.ODYSSEY_CHN: + if candidate == CAR.HONDA_ODYSSEY_CHN: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end else: 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 # average weight - ret.wheelbase = 2.86 - ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 16.0 # as spec + elif candidate == CAR.HONDA_PILOT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - 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 - ret.wheelbase = 3.18 - ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.59 # as spec + elif candidate == CAR.HONDA_RIDGELINE: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - 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 - ret.wheelbase = 2.7 - ret.centerToFront = ret.wheelbase * 0.39 - ret.steerRatio = 15.0 # 12.58 is spec end-to-end + elif candidate == CAR.HONDA_INSIGHT: ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - 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 - 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 - ret.tireStiffnessFactor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning else: raise ValueError(f"unsupported car {candidate}") # These cars use alternate user brake msg (0x1BE) - if 0x1BE in fingerprint[get_pt_bus(candidate)] and candidate in HONDA_BOSCH: + # TODO: Only detect feature for Accord/Accord Hybrid, not all Bosch DBCs have BRAKE_MODULE + if 0x1BE in fingerprint[CAN.pt] and candidate in (CAR.HONDA_ACCORD, CAR.HONDA_HRV_3G): ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value + + if ret.flags & HondaFlags.BOSCH_ALT_BRAKE: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE # These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON) @@ -287,16 +206,13 @@ class CarInterface(CarInterfaceBase): if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG - if ret.enableGasInterceptor and candidate not in HONDA_BOSCH: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR - if candidate in HONDA_BOSCH_RADARLESS: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not # conflict with PCM acc - ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor + ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC}) ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS ret.steerActuatorDelay = 0.1 @@ -315,7 +231,7 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = [ *create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT), - *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}), + *create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, SETTINGS_BUTTONS_DICT), ] # events @@ -341,8 +257,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - # pass in a car.CarControl - # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/honda/tests/__init__.py b/selfdrive/car/honda/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/honda/tests/test_honda.py b/selfdrive/car/honda/tests/test_honda.py new file mode 100644 index 0000000000..b8ad7872b2 --- /dev/null +++ b/selfdrive/car/honda/tests/test_honda.py @@ -0,0 +1,14 @@ +import re + +from openpilot.selfdrive.car.honda.fingerprints import FW_VERSIONS + +HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}-[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$" + + +class TestHondaFingerprint: + def test_fw_version_format(self): + # Asserts all FW versions follow an expected format + for fw_by_ecu in FW_VERSIONS.values(): + for fws in fw_by_ecu.values(): + for fw in fws: + assert re.match(HONDA_FW_VERSION_RE, fw) is not None, fw diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 517d2550a4..c3005c667c 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -1,12 +1,11 @@ from dataclasses import dataclass -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Optional, Union +from enum import Enum, IntFlag from cereal import car from openpilot.common.conversions import Conversions as CV from panda.python import uds -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -47,10 +46,19 @@ class CarControllerParams: class HondaFlags(IntFlag): + # Detected flags # Bosch models with alternate set of LKAS_HUD messages BOSCH_EXT_HUD = 1 BOSCH_ALT_BRAKE = 2 + # Static flags + BOSCH = 4 + BOSCH_RADARLESS = 8 + + NIDEC = 16 + NIDEC_ALT_PCM_ACCEL = 32 + NIDEC_ALT_SCM_MESSAGES = 64 + # Car button codes class CruiseButtons: @@ -60,6 +68,11 @@ class CruiseButtons: MAIN = 1 +class CruiseSettings: + DISTANCE = 3 + LKAS = 1 + + # See dbc files for info on values VISUAL_HUD = { VisualAlert.none: 0, @@ -73,91 +86,184 @@ VISUAL_HUD = { } -class CAR(StrEnum): - ACCORD = "HONDA ACCORD 2018" - ACCORDH = "HONDA ACCORD HYBRID 2018" - CIVIC = "HONDA CIVIC 2016" - CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019" - CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019" - CIVIC_2022 = "HONDA CIVIC 2022" - ACURA_ILX = "ACURA ILX 2016" - CRV = "HONDA CR-V 2016" - CRV_5G = "HONDA CR-V 2017" - CRV_EU = "HONDA CR-V EU 2016" - CRV_HYBRID = "HONDA CR-V HYBRID 2019" - FIT = "HONDA FIT 2018" - FREED = "HONDA FREED 2020" - HRV = "HONDA HRV 2019" - HRV_3G = "HONDA HR-V 2023" - ODYSSEY = "HONDA ODYSSEY 2018" - ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019" - ACURA_RDX = "ACURA RDX 2018" - ACURA_RDX_3G = "ACURA RDX 2020" - PILOT = "HONDA PILOT 2017" - RIDGELINE = "HONDA RIDGELINE 2017" - INSIGHT = "HONDA INSIGHT 2019" - HONDA_E = "HONDA E 2020" - - -class Footnote(Enum): - CIVIC_DIESEL = CarFootnote( - "2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.", - Column.FSR_STEERING) - - @dataclass -class HondaCarInfo(CarInfo): +class HondaCarDocs(CarDocs): package: str = "Honda Sensing" def init_make(self, CP: car.CarParams): - if CP.carFingerprint in HONDA_BOSCH: - self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a]) + if CP.flags & HondaFlags.BOSCH: + self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.flags & HondaFlags.BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a]) else: self.car_parts = CarParts.common([CarHarness.nidec]) -CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = { - CAR.ACCORD: [ - HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), - HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), - ], - CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"), - CAR.CIVIC_BOSCH: [ - HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", - footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS), - HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), - ], - CAR.CIVIC_BOSCH_DIESEL: None, # same platform - CAR.CIVIC_2022: [ - HondaCarInfo("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), - HondaCarInfo("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), - ], - CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS), - CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring - CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-20", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"), - CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"), - CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey - CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.PILOT: [ - HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), - HondaCarInfo("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), - ], - CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-24", min_steer_speed=12. * CV.MPH_TO_MS), - CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), - CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS), -} +class Footnote(Enum): + CIVIC_DIESEL = CarFootnote( + "2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.", + Column.FSR_STEERING) + -HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ +class HondaBoschPlatformConfig(PlatformConfig): + def init(self): + self.flags |= HondaFlags.BOSCH + + +class HondaNidecPlatformConfig(PlatformConfig): + def init(self): + self.flags |= HondaFlags.NIDEC + + +class CAR(Platforms): + # Bosch Cars + HONDA_ACCORD = HondaBoschPlatformConfig( + [ + HondaCarDocs("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarDocs("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS), + HondaCarDocs("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS), + ], + # steerRatio: 11.82 is spec end-to-end + CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467), + dbc_dict('honda_accord_2018_can_generated', None), + ) + HONDA_CIVIC_BOSCH = HondaBoschPlatformConfig( + [ + HondaCarDocs("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", + footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS), + HondaCarDocs("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS), + ], + CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec + dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + ) + HONDA_CIVIC_BOSCH_DIESEL = HondaBoschPlatformConfig( + [], # don't show in docs + HONDA_CIVIC_BOSCH.specs, + dbc_dict('honda_accord_2018_can_generated', None), + ) + HONDA_CIVIC_2022 = HondaBoschPlatformConfig( + [ + HondaCarDocs("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), + HondaCarDocs("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"), + ], + HONDA_CIVIC_BOSCH.specs, + dbc_dict('honda_civic_ex_2022_can_generated', None), + flags=HondaFlags.BOSCH_RADARLESS, + ) + HONDA_CRV_5G = HondaBoschPlatformConfig( + [HondaCarDocs("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)], + # steerRatio: 12.3 is spec end-to-end + CarSpecs(mass=3410 * CV.LB_TO_KG, wheelbase=2.66, steerRatio=16.0, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), + dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), + flags=HondaFlags.BOSCH_ALT_BRAKE, + ) + HONDA_CRV_HYBRID = HondaBoschPlatformConfig( + [HondaCarDocs("Honda CR-V Hybrid 2017-21", min_steer_speed=12. * CV.MPH_TO_MS)], + # mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end + CarSpecs(mass=1667, wheelbase=2.66, steerRatio=16, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), + dbc_dict('honda_accord_2018_can_generated', None), + ) + HONDA_HRV_3G = HondaBoschPlatformConfig( + [HondaCarDocs("Honda HR-V 2023", "All")], + CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.61, steerRatio=15.2, centerToFrontRatio=0.41, tireStiffnessFactor=0.5), + dbc_dict('honda_civic_ex_2022_can_generated', None), + flags=HondaFlags.BOSCH_RADARLESS, + ) + ACURA_RDX_3G = HondaBoschPlatformConfig( + [HondaCarDocs("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec + dbc_dict('acura_rdx_2020_can_generated', None), + flags=HondaFlags.BOSCH_ALT_BRAKE, + ) + HONDA_INSIGHT = HondaBoschPlatformConfig( + [HondaCarDocs("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=2987 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.0, centerToFrontRatio=0.39, tireStiffnessFactor=0.82), # as spec + dbc_dict('honda_insight_ex_2019_can_generated', None), + ) + HONDA_E = HondaBoschPlatformConfig( + [HondaCarDocs("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS)], + CarSpecs(mass=3338.8 * CV.LB_TO_KG, wheelbase=2.5, centerToFrontRatio=0.5, steerRatio=16.71, tireStiffnessFactor=0.82), + dbc_dict('acura_rdx_2020_can_generated', None), + ) + + # Nidec Cars + ACURA_ILX = HondaNidecPlatformConfig( + [HondaCarDocs("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS)], + CarSpecs(mass=3095 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=18.61, centerToFrontRatio=0.37, tireStiffnessFactor=0.72), # 15.3 is spec end-to-end + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_CRV = HondaNidecPlatformConfig( + [HondaCarDocs("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3572 * CV.LB_TO_KG, wheelbase=2.62, steerRatio=16.89, centerToFrontRatio=0.41, tireStiffnessFactor=0.444), # as spec + dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_CRV_EU = HondaNidecPlatformConfig( + [], # Euro version of CRV Touring, don't show in docs + HONDA_CRV.specs, + dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_FIT = HondaNidecPlatformConfig( + [HondaCarDocs("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=2644 * CV.LB_TO_KG, wheelbase=2.53, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_FREED = HondaNidecPlatformConfig( + [HondaCarDocs("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3086. * CV.LB_TO_KG, wheelbase=2.74, steerRatio=13.06, centerToFrontRatio=0.39, tireStiffnessFactor=0.75), # mostly copied from FIT + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_HRV = HondaNidecPlatformConfig( + [HondaCarDocs("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS)], + HONDA_HRV_3G.specs, + dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_ODYSSEY = HondaNidecPlatformConfig( + [HondaCarDocs("Honda Odyssey 2018-20")], + CarSpecs(mass=1900, wheelbase=3.0, steerRatio=14.35, centerToFrontRatio=0.41, tireStiffnessFactor=0.82), + dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_PCM_ACCEL, + ) + HONDA_ODYSSEY_CHN = HondaNidecPlatformConfig( + [], # Chinese version of Odyssey, don't show in docs + HONDA_ODYSSEY.specs, + dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + ACURA_RDX = HondaNidecPlatformConfig( + [HondaCarDocs("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=3925 * CV.LB_TO_KG, wheelbase=2.68, steerRatio=15.0, centerToFrontRatio=0.38, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_PILOT = HondaNidecPlatformConfig( + [ + HondaCarDocs("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS), + HondaCarDocs("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS), + ], + CarSpecs(mass=4278 * CV.LB_TO_KG, wheelbase=2.86, centerToFrontRatio=0.428, steerRatio=16.0, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_RIDGELINE = HondaNidecPlatformConfig( + [HondaCarDocs("Honda Ridgeline 2017-24", min_steer_speed=12. * CV.MPH_TO_MS)], + CarSpecs(mass=4515 * CV.LB_TO_KG, wheelbase=3.18, centerToFrontRatio=0.41, steerRatio=15.59, tireStiffnessFactor=0.444), # as spec + dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + flags=HondaFlags.NIDEC_ALT_SCM_MESSAGES, + ) + HONDA_CIVIC = HondaNidecPlatformConfig( + [HondaCarDocs("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE")], + CarSpecs(mass=1326, wheelbase=2.70, centerToFrontRatio=0.4, steerRatio=15.38), # 10.93 is end-to-end spec + dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), + ) + + +HONDA_ALT_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xF112) -HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ +HONDA_ALT_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(0xF112) FW_QUERY_CONFIG = FwQueryConfig( @@ -170,17 +276,10 @@ FW_QUERY_CONFIG = FwQueryConfig( ), # Data collection requests: - # Attempt to get the radarless Civic 2022+ camera FW + # Log manufacturer-specific identifier for current ECUs Request( - [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], - [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], - bus=0, - logging=True - ), - # Log extra identifiers for current ECUs - Request( - [HONDA_VERSION_REQUEST], - [HONDA_VERSION_RESPONSE], + [HONDA_ALT_VERSION_REQUEST], + [HONDA_ALT_VERSION_RESPONSE], bus=1, logging=True, ), @@ -189,7 +288,6 @@ FW_QUERY_CONFIG = FwQueryConfig( [StdQueries.UDS_VERSION_REQUEST], [StdQueries.UDS_VERSION_RESPONSE], bus=0, - logging=True, ), # Bosch PT bus Request( @@ -201,56 +299,33 @@ FW_QUERY_CONFIG = FwQueryConfig( ], # We lose these ECUs without the comma power on these cars. # Note that we still attempt to match with them when they are present + # This is or'd with (ALL_ECUS - ESSENTIAL_ECUS) from fw_versions.py non_essential_ecus={ - Ecu.programmedFuelInjection: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.transmission: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.vsa: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.combinationMeter: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.gateway: [CAR.CIVIC_BOSCH, CAR.CRV_5G], - Ecu.electricBrakeBooster: [CAR.CIVIC_BOSCH, CAR.CRV_5G], + Ecu.eps: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_2022, CAR.HONDA_E, CAR.HONDA_HRV_3G], + Ecu.vsa: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_2022, CAR.HONDA_CRV_5G, CAR.HONDA_CRV_HYBRID, + CAR.HONDA_E, CAR.HONDA_HRV_3G, CAR.HONDA_INSIGHT], }, extra_ecus=[ + (Ecu.combinationMeter, 0x18da60f1, None), + (Ecu.programmedFuelInjection, 0x18da10f1, None), # The only other ECU on PT bus accessible by camera on radarless Civic - (Ecu.unknown, 0x18DAB3F1, None), + # This is likely a manufacturer-specific sub-address implementation: the camera responds to this and 0x18dab0f1 + # Unclear what the part number refers to: 8S103 is 'Camera Set Mono', while 36160 is 'Camera Monocular - Honda' + # TODO: add query back, camera does not support querying both in parallel and 0x18dab0f1 often fails to respond + # (Ecu.unknown, 0x18DAB3F1, None), ], ) - -DBC = { - CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None), - CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None), - CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None), - CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), - CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_accord_2018_can_generated', None), - CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'), - CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None), - CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), - CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None), - CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), - CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), - CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), - CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), - CAR.CIVIC_2022: dbc_dict('honda_civic_ex_2022_can_generated', None), -} - STEER_THRESHOLD = { # default is 1200, overrides go here CAR.ACURA_RDX: 400, - CAR.CRV_EU: 400, + CAR.HONDA_CRV_EU: 400, } -HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} -HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, - CAR.PILOT, CAR.RIDGELINE} -HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G} -HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G} +HONDA_NIDEC_ALT_PCM_ACCEL = CAR.with_flags(HondaFlags.NIDEC_ALT_PCM_ACCEL) +HONDA_NIDEC_ALT_SCM_MESSAGES = CAR.with_flags(HondaFlags.NIDEC_ALT_SCM_MESSAGES) +HONDA_BOSCH = CAR.with_flags(HondaFlags.BOSCH) +HONDA_BOSCH_RADARLESS = CAR.with_flags(HondaFlags.BOSCH_RADARLESS) + + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0b5f724ab9..4038ddcca9 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -7,6 +7,7 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fau from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR +from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -42,7 +43,7 @@ def process_hud_alert(enabled, fingerprint, hud_control): return sys_warning, sys_state, left_lane_warning, right_lane_warning -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.CAN = CanBus(CP) @@ -128,13 +129,13 @@ class CarController: can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units)) + set_speed_in_units, hud_control)) self.accel_last = accel else: # button presses can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False)) else: - can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, apply_steer_req, + can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning)) @@ -147,7 +148,7 @@ class CarController: jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0 use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), - hud_control.leadVisible, set_speed_in_units, stopping, + hud_control, set_speed_in_units, stopping, CC.cruiseControl.override, use_fca)) # 20 Hz LFA MFA message @@ -162,7 +163,7 @@ class CarController: if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl: can_sends.append(hyundaican.create_frt_radar_opt(self.packer)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.accel = accel @@ -174,12 +175,12 @@ class CarController: can_sends = [] if use_clu11: if CC.cruiseControl.cancel: - can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP.carFingerprint)) + can_sends.append(hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL, self.CP)) elif CC.cruiseControl.resume: # send resume at a max freq of 10Hz if (self.frame - self.last_button_frame) * DT_CTRL > 0.1: # send 25 messages at a time to increases the likelihood of resume being accepted - can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP.carFingerprint)] * 25) + can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL, self.CP)] * 25) if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: self.last_button_frame = self.frame else: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 64a9fdf2ce..92c489cf34 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -118,6 +118,7 @@ class CarState(CarStateBase): ret.brakePressed = cp.vl["TCS13"]["DriverOverride"] == 2 # 2 includes regen braking by user on HEV/EV ret.brakeHoldActive = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY ret.parkingBrake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1 + ret.espDisabled = cp.vl["TCS11"]["TCS_PAS"] == 1 ret.accFaulted = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED if self.CP.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): @@ -207,7 +208,7 @@ class CarState(CarStateBase): # TODO: alt signal usage may be described by cp.vl['BLINKERS']['USE_ALT_LAMP'] left_blinker_sig, right_blinker_sig = "LEFT_LAMP", "RIGHT_LAMP" - if self.CP.carFingerprint == CAR.KONA_EV_2ND_GEN: + if self.CP.carFingerprint == CAR.HYUNDAI_KONA_EV_2ND_GEN: left_blinker_sig, right_blinker_sig = "LEFT_LAMP_ALT", "RIGHT_LAMP_ALT" ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(50, cp.vl["BLINKERS"][left_blinker_sig], cp.vl["BLINKERS"][right_blinker_sig]) @@ -255,6 +256,7 @@ class CarState(CarStateBase): messages = [ # address, frequency ("MDPS12", 50), + ("TCS11", 100), ("TCS13", 50), ("TCS15", 10), ("CLU11", 50), diff --git a/selfdrive/car/hyundai/fingerprints.py b/selfdrive/car/hyundai/fingerprints.py index d1fc1faabb..d115283dd5 100644 --- a/selfdrive/car/hyundai/fingerprints.py +++ b/selfdrive/car/hyundai/fingerprints.py @@ -5,22 +5,7 @@ from openpilot.selfdrive.car.hyundai.values import CAR Ecu = car.CarParams.Ecu FINGERPRINTS = { - CAR.HYUNDAI_GENESIS: [{ - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1342: 6, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1268: 8, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1437: 8, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 - }, - { - 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 5, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4 - }], - CAR.SANTA_FE: [{ + CAR.HYUNDAI_SANTA_FE: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1379: 8, 1384: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8 }, { @@ -29,40 +14,19 @@ FINGERPRINTS = { { 67: 8, 68: 8, 80: 4, 160: 8, 161: 8, 272: 8, 288: 4, 339: 8, 356: 8, 357: 8, 399: 8, 544: 8, 608: 8, 672: 8, 688: 5, 704: 1, 790: 8, 809: 8, 848: 8, 880: 8, 898: 8, 900: 8, 901: 8, 904: 8, 1056: 8, 1064: 8, 1065: 8, 1072: 8, 1075: 8, 1087: 8, 1088: 8, 1151: 8, 1200: 8, 1201: 8, 1232: 4, 1264: 8, 1265: 8, 1266: 8, 1296: 8, 1306: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1348: 8, 1349: 8, 1369: 8, 1370: 8, 1371: 8, 1407: 8, 1415: 8, 1419: 8, 1440: 8, 1442: 4, 1461: 8, 1470: 8 }], - CAR.SONATA: [{ + CAR.HYUNDAI_SONATA: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 549: 8, 550: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 8, 865: 8, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 908: 8, 909: 8, 912: 7, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1089: 5, 1096: 8, 1107: 5, 1108: 8, 1114: 8, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1170: 8, 1173: 8, 1180: 8, 1183: 8, 1184: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1268: 8, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1330: 8, 1339: 8, 1342: 6, 1343: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1379: 8, 1384: 8, 1394: 8, 1407: 8, 1419: 8, 1427: 6, 1446: 8, 1456: 4, 1460: 8, 1470: 8, 1485: 8, 1504: 3, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8 }], - CAR.SONATA_LF: [{ - 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1314: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1397: 8, 1407: 8, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2008: 8, 2009: 8, 2012: 8, 2013: 8, 2014: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], - CAR.KIA_SORENTO: [{ - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 - }], CAR.KIA_STINGER: [{ 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1379: 8, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1456: 4, 1470: 8 }], - CAR.GENESIS_G80: [{ - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1024: 2, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1191: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8 - }, - { - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 546: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1437: 8, 1456: 4, 1470: 8 - }, - { - 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1162: 8, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1193: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1437: 8, 1456: 4, 1470: 8 - }], CAR.GENESIS_G90: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 358: 6, 359: 8, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1162: 4, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1378: 4, 1384: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1434: 2, 1456: 4, 1470: 8, 1988: 8, 2000: 8, 2003: 8, 2004: 8, 2005: 8, 2008: 8, 2011: 8, 2012: 8, 2013: 8 }], - CAR.IONIQ_EV_2020: [{ - 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 - }], - CAR.IONIQ: [{ - 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8 - }], - CAR.KONA_EV: [{ + CAR.HYUNDAI_KONA_EV: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 549: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1307: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1378: 4, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 1157: 4, 1193: 8, 1379: 8, 1988: 8, 1996: 8 }], - CAR.KONA_EV_2022: [{ + CAR.HYUNDAI_KONA_EV_2022: [{ 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1069: 8, 1078: 4, 1136: 8, 1145: 8, 1151: 8, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 8, 1173: 8, 1183: 8, 1188: 8, 1191: 2, 1193: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1339: 8, 1342: 8, 1343: 8, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1446: 8, 1456: 4, 1470: 8, 1473: 8, 1485: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8 }], CAR.KIA_NIRO_EV: [{ @@ -74,13 +38,10 @@ FINGERPRINTS = { { 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 576: 8, 593: 8, 688: 5, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 909: 8, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1265: 4, 1268: 8, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1420: 8, 1425: 2, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8 }], - CAR.PALISADE: [{ - 67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 546: 8, 547: 8, 548: 8, 549: 8, 576: 8, 593: 8, 608: 8, 688: 6, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 913: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1123: 8, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1162: 8, 1164: 8, 1168: 7, 1170: 8, 1173: 8, 1180: 8, 1186: 2, 1191: 2, 1193: 8, 1210: 8, 1225: 8, 1227: 8, 1265: 4, 1280: 8, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1371: 8, 1378: 8, 1384: 8, 1407: 8, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8 - }], } FW_VERSIONS = { - CAR.AZERA_6TH_GEN: { + CAR.HYUNDAI_AZERA_6TH_GEN: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IG__ SCC F-CU- 1.00 1.00 99110-G8100 ', ], @@ -90,33 +51,22 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IG MFC AT MES LHD 1.00 1.04 99211-G8100 200511', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 U912\x00\x00\x00\x00\x00\x00SIG0M35MH0\xa4 |.', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81641KA051\x00\x00\x00\x00\x00\x00\x00\x00', - ], }, - CAR.AZERA_HEV_6TH_GEN: { + CAR.HYUNDAI_AZERA_HEV_6TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.00 99211-G8000 180903', + b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.01 99211-G8000 181109', b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.02 99211-G8100 191029', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00IG MDPS C 1.00 1.00 56310M9600\x00 4IHSC100', b'\xf1\x00IG MDPS C 1.00 1.01 56310M9350\x00 4IH8C101', + b'\xf1\x00IG MDPS C 1.00 1.02 56310M9350\x00 4IH8C102', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IGhe SCC FHCUP 1.00 1.00 99110-M9100 ', b'\xf1\x00IGhe SCC FHCUP 1.00 1.01 99110-M9000 ', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006T7N0_C2\x00\x006T7Q2051\x00\x00TIG2H24KA2\x12@\x11\xb7', - b'\xf1\x006T7N0_C2\x00\x006T7VA051\x00\x00TIGSH24KA1\xc7\x85\xe2`', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H570051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H590051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00IGhe SCC FHCUP 1.00 1.02 99110-M9000 ', ], }, CAR.HYUNDAI_GENESIS: { @@ -126,42 +76,36 @@ FW_VERSIONS = { b'\xf1\x00DH LKAS 1.5 -140425', ], }, - CAR.IONIQ: { + CAR.HYUNDAI_IONIQ: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', + b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2100 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.05 56310/G2501 4AEHC105', b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2301 4AEHC107', + b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2501 4AEHC107', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G2400 180222', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6F2051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U3H1051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H1051\x00\x00HAE0G16US2\x00\x00\x00\x00', + b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2400 180222', ], }, - CAR.IONIQ_PHEV_2019: { + CAR.HYUNDAI_IONIQ_PHEV_2019: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ', b'\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2100 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2501 4AEHC107', + b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2551 4AEHC107', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00AEP MFC AT AUS RHD 1.00 1.00 95740-G2400 180222', b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2400 180222', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\x00\x00\x00\x00', - b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PAE0G16NS1\xdbD\r\x81', - ], }, - CAR.IONIQ_PHEV: { + CAR.HYUNDAI_IONIQ_PHEV: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2200 ', b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', @@ -170,6 +114,7 @@ FW_VERSIONS = { b'\xf1\x00AEhe SCC FHCUP 1.00 1.02 99110-G2100 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2210 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2310 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2510 4APHC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2560 4APHC101', @@ -181,28 +126,18 @@ FW_VERSIONS = { b'\xf1\x00AEP MFC AT USA LHD 1.00 1.00 95740-G2700 201027', b'\xf1\x00AEP MFC AT USA LHD 1.00 1.01 95740-G2600 190819', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00', - b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL0\x00\x00\x00\x00', - b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\xad\xeb\xabt', - b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00', - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL0\x82zT\xd2', - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PAE0G16NL2\x00\x00\x00\x00', - ], }, - CAR.IONIQ_EV_2020: { + CAR.HYUNDAI_IONIQ_EV_2020: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEev SCC F-CUP 1.00 1.00 99110-G7200 ', b'\xf1\x00AEev SCC F-CUP 1.00 1.00 99110-G7500 ', b'\xf1\x00AEev SCC F-CUP 1.00 1.01 99110-G7000 ', + b'\xf1\x00AEev SCC F-CUP 1.00 1.01 99110-G7100 ', + b'\xf1\x00AEev SCC FHCUP 1.00 1.01 99110-G7100 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7310 4APEC101', + b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7510 4APEC101', b'\xf1\x00AE MDPS C 1.00 1.01 56310/G7560 4APEC101', ], (Ecu.fwdCamera, 0x7c4, None): [ @@ -211,9 +146,10 @@ FW_VERSIONS = { b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.01 95740-G2600 190819', b'\xf1\x00AEE MFC AT EUR LHD 1.00 1.03 95740-G2500 190516', b'\xf1\x00AEE MFC AT EUR RHD 1.00 1.01 95740-G2600 190819', + b'\xf1\x00AEE MFC AT USA LHD 1.00 1.01 95740-G2600 190819', ], }, - CAR.IONIQ_EV_LTD: { + CAR.HYUNDAI_IONIQ_EV_LTD: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7000 ', b'\xf1\x00AEev SCC F-CUP 1.00 1.00 96400-G7100 ', @@ -232,7 +168,7 @@ FW_VERSIONS = { b'\xf1\x00AEE MFC AT USA LHD 1.00 1.00 95740-G2400 180222', ], }, - CAR.IONIQ_HEV_2022: { + CAR.HYUNDAI_IONIQ_HEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ', b'\xf1\x00AEhe SCC FHCUP 1.00 1.00 99110-G2600 ', @@ -243,15 +179,8 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2700 201027', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HAE0G16NL2\x96\xda\xd4\xee', - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HAE0G16NL2\x00\x00\x00\x00', - ], }, - CAR.SONATA: { + CAR.HYUNDAI_SONATA: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DN8_ SCC F-CU- 1.00 1.00 99110-L0000 ', b'\xf1\x00DN8_ SCC F-CUP 1.00 1.00 99110-L0000 ', @@ -266,55 +195,26 @@ FW_VERSIONS = { b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', b'\xf1\x00DN ESC \x06 107 \x07\x03 58910-L1300', + b'\xf1\x00DN ESC \x06 107"\x08\x07 58910-L0100', b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', b'\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', b'\xf1\x00DN ESC \x07 107"\x08\x07 58910-L0100', b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100', - b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 106 \x07\x01 58910-L0100', - ], - (Ecu.engine, 0x7e0, None): [ - b'HM6M1_0a0_F00', - b'HM6M1_0a0_G20', - b'HM6M2_0a0_BD0', - b'\xf1\x81HM6M1_0a0_F00', - b'\xf1\x81HM6M1_0a0_G20', - b'\xf1\x82DNBVN5GMCCXXXDCA', - b'\xf1\x82DNBVN5GMCCXXXG2F', - b'\xf1\x82DNBWN5TMDCXXXG2E', - b'\xf1\x82DNCVN5GMCCXXXF0A', - b'\xf1\x82DNCVN5GMCCXXXG2B', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DNDWN5TMDCXXXJ1A', - b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M00', - b'\xf1\x8739110-2S041\xf1\x81HM6M1_0a0_M10', - b'\xf1\x8739110-2S042\xf1\x81HM6M1_0a0_M00', - b'\xf1\x8739110-2S278\xf1\x82DNDVD5GMCCXXXL5B', - b'\xf1\x87391162M003', - b'\xf1\x87391162M010', - b'\xf1\x87391162M013', - b'\xf1\x87391162M023', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC102', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0200\x00 4DNAC102', + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101', b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC102', + b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103', + b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103', b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP101', b'\xf1\x00DN8 MDPS R 1.00 1.02 57700-L1000 4DNDP105', - b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101', - b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101', - b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103', - b'\xf1\x8756310-L1030\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103', - b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1,00 1,01 56310L0010\x00 4DNAC101', - b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101', - b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101', - b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DN8 MFC AT KOR LHD 1.00 1.02 99211-L1000 190422', @@ -327,74 +227,8 @@ FW_VERSIONS = { b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.06 99211-L1000 210325', b'\xf1\x00DN8 MFC AT USA LHD 1.00 1.07 99211-L1000 211223', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00HT6TA260BLHT6TA800A1TDN8C20KS4\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x00HT6TA260BLHT6TA810A1TDN8M25GS0\x00\x00\x00\x00\x00\x00\xaa\x8c\xd9p', - b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92', - b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:', - b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB4\x00\x00\x00\x00\x00\x00g!l[', - b'\xf1\x00HT6WA280BLHT6WAE10A1SDN8G25NB5\x00\x00\x00\x00\x00\x00\xe0t\xa9\xba', - b'\xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', - b'\xf1\x00T02601BL T02832A1 VDN8T25XXX832NS8G\x0e\xfeE', - b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSA\xb9\x13\xf9p', - b'\xf1\x00T02601BL T02900A1 VDN8T25XXX900NSCF\xe4!Y', - b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16KB05\x95h%', - b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', - b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VDN8T25XXX730NS5\xf7_\x92\xf5', - b'\xf1\x87SAKFBA2926554GJ2VefVww\x87xwwwww\x88\x87xww\x87wTo\xfb\xffvUo\xff\x8d\x16\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SAKFBA3030524GJ2UVugww\x97yx\x88\x87\x88vw\x87gww\x87wto\xf9\xfffUo\xff\xa2\x0c\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SAKFBA3356084GJ2\x86fvgUUuWgw\x86www\x87wffvf\xb6\xcf\xfc\xffeUO\xff\x12\x19\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SAKFBA3474944GJ2ffvgwwwwg\x88\x86x\x88\x88\x98\x88ffvfeo\xfa\xff\x86fo\xff\t\xae\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SAKFBA3475714GJ2Vfvgvg\x96yx\x88\x97\x88ww\x87ww\x88\x87xs_\xfb\xffvUO\xff\x0f\xff\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA3753044GJ3UUeVff\x86hwwwwvwwgvfgfvo\xf9\xfffU_\xffC\xae\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA3862294GJ3vfvgvefVxw\x87\x87w\x88\x87xwwwwc_\xf9\xff\x87w\x9f\xff\xd5\xdc\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA3873834GJ3fefVwuwWx\x88\x97\x88w\x88\x97xww\x87wU_\xfb\xff\x86f\x8f\xffN\x04\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA4525334GJ3\x89\x99\x99\x99fevWh\x88\x86\x88fwvgw\x88\x87xfo\xfa\xffuDo\xff\xd1>\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA4626804GJ3wwww\x88\x87\x88xx\x88\x87\x88wwgw\x88\x88\x98\x88\x95_\xf9\xffuDo\xff|\xe7\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA4803224GJ3wwwwwvwg\x88\x88\x98\x88wwww\x87\x88\x88xu\x9f\xfc\xff\x87f\x8f\xff\xea\xea\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA6212564GJ3\x87wwwUTuGg\x88\x86xx\x88\x87\x88\x87\x88\x98xu?\xf9\xff\x97f\x7f\xff\xb8\n\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA6347404GJ3wwwwff\x86hx\x88\x97\x88\x88\x88\x88\x88vfgf\x88?\xfc\xff\x86Uo\xff\xec/\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA6901634GJ3UUuWVeVUww\x87wwwwwvUge\x86/\xfb\xff\xbb\x99\x7f\xff]2\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALDBA7077724GJ3\x98\x88\x88\x88ww\x97ygwvwww\x87ww\x88\x87x\x87_\xfd\xff\xba\x99o\xff\x99\x01\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SALFBA3525114GJ2wvwgvfvggw\x86wffvffw\x86g\x85_\xf9\xff\xa8wo\xffv\xcd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA3624024GJ2\x88\x88\x88\x88wv\x87hx\x88\x97\x88x\x88\x97\x88ww\x87w\x86o\xfa\xffvU\x7f\xff\xd1\xec\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA3960824GJ2wwwwff\x86hffvfffffvfwfg_\xf9\xff\xa9\x88\x8f\xffb\x99\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA4011074GJ2fgvwwv\x87hw\x88\x87xww\x87wwfgvu_\xfa\xffefo\xff\x87\xc0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA4121304GJ2x\x87xwff\x86hwwwwww\x87wwwww\x84_\xfc\xff\x98\x88\x9f\xffi\xa6\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA4195874GJ2EVugvf\x86hgwvwww\x87wgw\x86wc_\xfb\xff\x98\x88\x8f\xff\xe23\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA4625294GJ2eVefeUeVx\x88\x97\x88wwwwwwww\xa7o\xfb\xffvw\x9f\xff\xee.\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA4728774GJ2vfvg\x87vwgww\x87ww\x88\x97xww\x87w\x86_\xfb\xffeD?\xffk0\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA5129064GJ2vfvgwv\x87hx\x88\x87\x88ww\x87www\x87wd_\xfa\xffvfo\xff\x1d\x00\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA5454914GJ2\x98\x88\x88\x88\x87vwgx\x88\x87\x88xww\x87ffvf\xa7\x7f\xf9\xff\xa8w\x7f\xff\x1b\x90\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA5987784GJ2UVugDDtGx\x88\x87\x88w\x88\x87xwwwwd/\xfb\xff\x97fO\xff\xb0h\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA5987864GJ2fgvwUUuWgwvw\x87wxwwwww\x84/\xfc\xff\x97w\x7f\xff\xdf\x1d\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA6337644GJ2vgvwwv\x87hgffvwwwwwwww\x85O\xfa\xff\xa7w\x7f\xff\xc5\xfc\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA6802004GJ2UUuWUUuWgw\x86www\x87www\x87w\x96?\xf9\xff\xa9\x88\x7f\xff\x9fK\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA6892284GJ233S5\x87w\x87xx\x88\x87\x88vwwgww\x87w\x84?\xfb\xff\x98\x88\x8f\xff*\x9e\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v', - b'\xf1\x87SALFBA7005534GJ2eUuWfg\x86xxww\x87x\x88\x87\x88\x88w\x88\x87\x87O\xfc\xffuUO\xff\xa3k\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', - b'\xf1\x87SALFBA7152454GJ2gvwgFf\x86hx\x88\x87\x88vfWfffffd?\xfa\xff\xba\x88o\xff,\xcf\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB1\xe3\xc10\xa1', - b'\xf1\x87SALFBA7460044GJ2gx\x87\x88Vf\x86hx\x88\x87\x88wwwwgw\x86wd?\xfa\xff\x86U_\xff\xaf\x1f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SALFBA7485034GJ2ww\x87xww\x87xfwvgwwwwvfgf\xa5/\xfc\xff\xa9w_\xff40\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMDBA7743924GJ3wwwwww\x87xgwvw\x88\x88\x88\x88wwww\x85_\xfa\xff\x86f\x7f\xff0\x9d\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SAMDBA7817334GJ3Vgvwvfvgww\x87wwwwwwfgv\x97O\xfd\xff\x88\x88o\xff\x8e\xeb\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SAMDBA8054504GJ3gw\x87xffvgffffwwwweUVUf?\xfc\xffvU_\xff\xddl\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SAMFB41553621GC7ww\x87xUU\x85Xvwwg\x88\x88\x88\x88wwgw\x86\xaf\xfb\xffuDo\xff\xaa\x8f\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMFB42555421GC7\x88\x88\x88\x88wvwgx\x88\x87\x88wwgw\x87wxw3\x8f\xfc\xff\x98f\x8f\xffga\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMFBA7978674GJ2gw\x87xgw\x97ywwwwvUGeUUeU\x87O\xfb\xff\x98w\x8f\xfffF\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMFBA8105254GJ2wx\x87\x88Vf\x86hx\x88\x87\x88wwwwwwww\x86O\xfa\xff\x99\x88\x7f\xffZG\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMFBA9283024GJ2wwwwEUuWwwgwwwwwwwww\x87/\xfb\xff\x98w\x8f\xff<\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SAMFBA9708354GJ2wwwwVf\x86h\x88wx\x87xww\x87\x88\x88\x88\x88w/\xfa\xff\x97w\x8f\xff\x86\xa0\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - b'\xf1\x87SANDB45316691GC6\x99\x99\x99\x99\x88\x88\xa8\x8avfwfwwww\x87wxwT\x9f\xfd\xff\x88wo\xff\x1c\xfa\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB3\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SANFB45889451GC7wx\x87\x88gw\x87x\x88\x88x\x88\x87wxw\x87wxw\x87\x8f\xfc\xffeU\x8f\xff+Q\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00SDN8T16NB2\n\xdd^\xbc', - ], - }, - CAR.SONATA_LF: { + }, + CAR.HYUNDAI_SONATA_LF: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00LF__ SCC F-CUP 1.00 1.00 96401-C2200 ', ], @@ -402,44 +236,22 @@ FW_VERSIONS = { b'\xf1\x00LF ESC \t 11 \x17\x01\x13 58920-C2610', b'\xf1\x00LF ESC \x0c 11 \x17\x01\x13 58920-C2610', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81606D5051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81606D5K51\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', - ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00LFF LKAS AT USA LHD 1.00 1.01 95740-C1000 E51', b'\xf1\x00LFF LKAS AT USA LHD 1.01 1.02 95740-C1000 E52', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', - b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2\x00\x00\x00\x00', - b'\xf1\x87LAHSGN012918KF10\x98\x88x\x87\x88\x88x\x87\x88\x88\x98\x88\x87w\x88w\x88\x88\x98\x886o\xf6\xff\x98w\x7f\xff3\x00\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', - b'\xf1\x87LAJSG49645724HF0\x87x\x87\x88\x87www\x88\x99\xa8\x89\x88\x99\xa8\x89\x88\x99\xa8\x89S_\xfb\xff\x87f\x7f\xff^2\xf1\x816W3B1051\x00\x00\xf1\x006W351_C2\x00\x006W3B1051\x00\x00TLF0T20NL2H\r\xbdm', - b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\x00\x00\x00\x00', - b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24NL1\xb0\x9f\xee\xf5', - b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B4051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B4051\x00\x00TLF0G24SL2n\x8d\xbe\xd8', - ], }, - CAR.TUCSON: { + CAR.HYUNDAI_TUCSON: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TL__ FCA F-CUP 1.00 1.01 99110-D3500 ', b'\xf1\x00TL__ FCA F-CUP 1.00 1.02 99110-D3510 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x8971TLC2NAIDDIR002\xf1\x8271TLC2NAIDDIR002', - ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TL MFC AT KOR LHD 1.00 1.02 95895-D3800 180719', b'\xf1\x00TL MFC AT USA LHD 1.00 1.06 95895-D3800 190107', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87KMLDCU585233TJ20wx\x87\x88x\x88\x98\x89vfwfwwww\x87f\x9f\xff\x98\xff\x7f\xf9\xf7s\xf1\x816T6G4051\x00\x00\xf1\x006T6J0_C2\x00\x006T6G4051\x00\x00TTL4G24NH2\x00\x00\x00\x00', - b'\xf1\x87LBJXAN202299KF22\x87x\x87\x88ww\x87xx\x88\x97\x88\x87\x88\x98x\x88\x99\x98\x89\x87o\xf6\xff\x87w\x7f\xff\x12\x9a\xf1\x81U083\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U083\x00\x00\x00\x00\x00\x00TTL2V20KL1\x8fRn\x8a', - ], }, - CAR.SANTA_FE: { + CAR.HYUNDAI_SANTA_FE: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1210 ', b'\xf1\x00TM__ SCC F-CUP 1.00 1.01 99110-S2000 ', @@ -458,11 +270,6 @@ FW_VERSIONS = { b'\xf1\x00TM ESC \r 104\x19\x07\x08 58910-S2650', b'\xf1\x00TM ESC \r 105\x19\x05# 58910-S1500', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81606G1051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81606G3051\x00\x00\x00\x00\x00\x00\x00\x00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8409', b'\xf1\x00TM MDPS C 1.00 1.00 56340-S2000 8A12', @@ -473,66 +280,26 @@ FW_VERSIONS = { b'\xf1\x00TM MFC AT EUR LHD 1.00 1.01 99211-S1010 181207', b'\xf1\x00TM MFC AT USA LHD 1.00 1.00 99211-S2000 180409', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', - b'\xf1\x00bcsh8p54 U833\x00\x00\x00\x00\x00\x00TTM4V22US3_<]\xf1', - b'\xf1\x87LBJSGA7082574HG0\x87www\x98\x88\x88\x88\x99\xaa\xb9\x9afw\x86gx\x99\xa7\x89co\xf8\xffvU_\xffR\xaf\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\xa6\xe0\x91', - b'\xf1\x87LBKSGA0458404HG0vfvg\x87www\x89\x99\xa8\x99y\xaa\xa7\x9ax\x88\xa7\x88t_\xf9\xff\x86w\x8f\xff\x15x\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2T20NS1\x00\x00\x00\x00', - b'\xf1\x87LDJUEA6010814HG1\x87w\x87x\x86gvw\x88\x88\x98\x88gw\x86wx\x88\x97\x88\x85o\xf8\xff\x86f_\xff\xd37\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', - b'\xf1\x87LDJUEA6458264HG1ww\x87x\x97x\x87\x88\x88\x99\x98\x89g\x88\x86xw\x88\x97x\x86o\xf7\xffvw\x8f\xff3\x9a\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS0\xf8\x19\x92g', - b'\xf1\x87LDKUEA2045844HG1wwww\x98\x88x\x87\x88\x88\xa8\x88x\x99\x97\x89x\x88\xa7\x88U\x7f\xf8\xffvfO\xffC\x1e\xf1\x816W3E0051\x00\x00\xf1\x006W351_C2\x00\x006W3E0051\x00\x00TTM4T20NS3\x00\x00\x00\x00', - b'\xf1\x87LDKUEA9993304HG1\x87www\x97x\x87\x88\x99\x99\xa9\x99x\x99\xa7\x89w\x88\x97x\x86_\xf7\xffwwO\xffl#\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4T20NS1R\x7f\x90\n', - b'\xf1\x87LDLUEA6061564HG1\xa9\x99\x89\x98\x87wwwx\x88\x97\x88x\x99\xa7\x89x\x99\xa7\x89sO\xf9\xffvU_\xff<\xde\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', - b'\xf1\x87LDLUEA6159884HG1\x88\x87hv\x99\x99y\x97\x89\xaa\xb8\x9ax\x99\x87\x89y\x99\xb7\x99\xa7?\xf7\xff\x97wo\xff\xf3\x05\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', - b'\xf1\x87LDLUEA6852664HG1\x97wWu\x97www\x89\xaa\xc8\x9ax\x99\x97\x89x\x99\xa7\x89SO\xf7\xff\xa8\x88\x7f\xff\x03z\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', - b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS5\x00\x00\x00\x00', - b'\xf1\x87LDLUEA6898374HG1fevW\x87wwwx\x88\x97\x88h\x88\x96\x88x\x88\xa7\x88ao\xf9\xff\x98\x99\x7f\xffD\xe2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4T20NS50\xcb\xc3\xed', - b'\xf1\x87SBJWAA5842214GG0\x88\x87\x88xww\x87x\x89\x99\xa8\x99\x88\x99\x98\x89w\x88\x87xw_\xfa\xfffU_\xff\xd1\x8d\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', - b'\xf1\x87SBJWAA5890864GG0\xa9\x99\x89\x98\x98\x87\x98y\x89\x99\xa8\x99w\x88\x87xww\x87wvo\xfb\xffuD_\xff\x9f\xb5\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', - b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x00\x00\x00\x00', - b'\xf1\x87SBJWAA6562474GG0ffvgeTeFx\x88\x97\x88ww\x87www\x87w\x84o\xfa\xff\x87fO\xff\xc2 \xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS1\x98{|\xe3', - b'\xf1\x87SBJWAA7780564GG0wvwgUUeVwwwwx\x88\x87\x88wwwwd_\xfc\xff\x86f\x7f\xff\xd7*\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', - b'\xf1\x87SBJWAA8278284GG0ffvgUU\x85Xx\x88\x87\x88x\x88w\x88ww\x87w\x96o\xfd\xff\xa7U_\xff\xf2\xa0\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM2G24NS2F\x84<\xc0', - b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6\x00\x00\x00\x00', - b'\xf1\x87SBLWAA4363244GG0wvwgwv\x87hgw\x86ww\x88\x87xww\x87wdo\xfb\xff\x86f\x7f\xff3$\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS6x0\x17\xfe', - b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00', - b'\xf1\x87SBLWAA6622844GG0wwwwff\x86hwwwwx\x88\x87\x88\x88\x88\x88\x88\x98?\xfd\xff\xa9\x88\x7f\xffn\xe5\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7u\x1e{\x1c', - b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2\x00\x00\x00\x00', - b'\xf1\x87SDJXAA7656854GG1DEtWUU\x85X\x88\x88\x98\x88w\x88\x87xx\x88\x87\x88\x96o\xfb\xff\x86f\x7f\xff.\xca\xf1\x816W3C2051\x00\x00\xf1\x006W351_C2\x00\x006W3C2051\x00\x00TTM4G24NS2K\xdaV0', - b'\xf1\x87SDKXAA2443414GG1vfvgwv\x87h\x88\x88\x88\x88ww\x87wwwww\x99_\xfc\xffvD?\xffl\xd2\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM4G24NS6\x00\x00\x00\x00', - ], - }, - CAR.SANTA_FE_2022: { + }, + CAR.HYUNDAI_SANTA_FE_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TM__ SCC F-CUP 1.00 1.00 99110-S1500 ', + b'\xf1\x00TM__ SCC F-CUP 1.00 1.01 99110-S1500 ', b'\xf1\x00TM__ SCC FHCUP 1.00 1.00 99110-S1500 ', + b'\xf1\x00TM__ SCC FHCUP 1.00 1.01 99110-S1500 ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00TM ESC \x01 102!\x04\x03 58910-S2DA0', + b'\xf1\x00TM ESC \x01 104"\x10\x07 58910-S2DA0', b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', b'\xf1\x00TM ESC \x02 103"\x07\x08 58910-S2GA0', b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', + b'\xf1\x00TM ESC \x03 102!\x04\x03 58910-S2DA0', b'\xf1\x00TM ESC \x04 101 \x08\x04 58910-S2GA0', b'\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', + b'\xf1\x00TM ESC \x04 103"\x07\x08 58910-S2GA0', + b'\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', b'\xf1\x00TM ESC 103!\x030 58910-S1MA0', - b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', - b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', - b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', - b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x04 102!\x04\x05 58910-S2GA0', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81HM6M1_0a0_G20', - b'\xf1\x81HM6M1_0a0_H00', - b'\xf1\x81HM6M2_0a0_G00', - b'\xf1\x82TACVN5GMI3XXXH0A', - b'\xf1\x82TACVN5GSI3XXXH0A', - b'\xf1\x82TMBZN5TMD3XXXG2E', - b'\xf1\x82TMCFD5MMCXXXXG0A', - b'\xf1\x87 \xf1\x81 ', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_J10', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M1_0a0_L50', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82TMDWN5TMD3TXXJ1A', - b'\xf1\x8739101-2STN8\xf1\x81HM6M1_0a0_M00', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.01 56310-S1AB0 4TSDC101', @@ -547,54 +314,33 @@ FW_VERSIONS = { b'\xf1\x00TMA MFC AT USA LHD 1.00 1.01 99211-S2500 210205', b'\xf1\x00TMA MFC AT USA LHD 1.00 1.03 99211-S2500 220414', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00HT6TA290BLHT6TAF00A1STM0M25GS1\x00\x00\x00\x00\x00\x006\xd8\x97\x15', - b'\xf1\x00HT6WA280BLHT6WAD00A1STM2G25NH2\x00\x00\x00\x00\x00\x00\xf8\xc0\xc3\xaa', - b'\xf1\x00HT6WA280BLHT6WAD00A1STM4G25NH1\x00\x00\x00\x00\x00\x00\x9cl\x04\xbc', - b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', - b'\xf1\x00T02601BL T02800A1 VTMPT25XXX800NS4\xed\xaf\xed\xf5', - b'\xf1\x00T02601BL T02900A1 VTMPT25XXW900NS1c\x918\xc5', - b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', - b'\xf1\x00T02601BL T02900A1 VTMPT25XXX900NSA\xf3\xf4Uj', - b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6', - b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', - b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02900A1 \xf1\x00T02601BL T02900A1 VTMPT25XXX900NS8\xb7\xaa\xfe\xfc', - b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', - b'\xf1\x87SDMXCA9087684GN1VfvgUUeVwwgwwwwwffffU?\xfb\xff\x97\x88\x7f\xff+\xa4\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', - ], - }, - CAR.SANTA_FE_HEV_2022: { + }, + CAR.HYUNDAI_SANTA_FE_HEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', + b'\xf1\x00TMhe SCC FHCUP 1.00 1.01 99110-CL500 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLEC0 4TSHC102', b'\xf1\x00TM MDPS C 1.00 1.02 56310-GA000 4TSHA100', b'\xf1\x00TM MDPS R 1.00 1.05 57700-CL000 4TSHP105', + b'\xf1\x00TM MDPS R 1.00 1.06 57700-CL000 4TSHP106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TMA MFC AT USA LHD 1.00 1.03 99211-S2500 220414', b'\xf1\x00TMH MFC AT EUR LHD 1.00 1.06 99211-S1500 220727', + b'\xf1\x00TMH MFC AT KOR LHD 1.00 1.06 99211-S1500 220727', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.03 99211-S1500 210224', + b'\xf1\x00TMH MFC AT USA LHD 1.00 1.05 99211-S1500 220126', b'\xf1\x00TMH MFC AT USA LHD 1.00 1.06 99211-S1500 220727', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16SA3\xa3\x1b\xe14', - b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2H16UA3I\x94\xac\x8f', - b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TTM2H16SA2\x80\xd7l\xb2', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391312MTC1', - b'\xf1\x87391312MTE0', - b'\xf1\x87391312MTL0', - ], }, - CAR.SANTA_FE_PHEV_2022: { + CAR.HYUNDAI_SANTA_FE_PHEV_2022: { (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00TMhe SCC F-CUP 1.00 1.00 99110-CL500 ', + b'\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', b'\xf1\x00TMhe SCC FHCUP 1.00 1.01 99110-CL500 ', - b'\xf1\x8799110CL500\xf1\x00TMhe SCC FHCUP 1.00 1.00 99110-CL500 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56310-CLAC0 4TSHC102', @@ -603,19 +349,11 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TMP MFC AT USA LHD 1.00 1.03 99211-S1500 210224', + b'\xf1\x00TMP MFC AT USA LHD 1.00 1.05 99211-S1500 220126', b'\xf1\x00TMP MFC AT USA LHD 1.00 1.06 99211-S1500 220727', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea', - b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA0o\x88^\xbe', - b'\xf1\x8795441-3D121\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TTM2P16SA1\x0b\xc5\x0f\xea', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391312MTF0', - b'\xf1\x87391312MTF1', - ], }, - CAR.CUSTIN_1ST_GEN: { + CAR.HYUNDAI_CUSTIN_1ST_GEN: { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00KU ESC \x01 101!\x02\x03 58910-O3200', ], @@ -628,12 +366,6 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00KU2 MFC AT CHN LHD 1.00 1.02 99211-O3000 220923', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391212MEC0', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 U928\x00\x00\x00\x00\x00\x00SKU0T15KB2\x92U\xf9M', - ], }, CAR.KIA_STINGER: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -642,14 +374,6 @@ FW_VERSIONS = { b'\xf1\x00CK__ SCC F_CUP 1.00 1.02 96400-J5100 ', b'\xf1\x00CK__ SCC F_CUP 1.00 1.03 96400-J5100 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\xe0\x19\xff\xe7\xe7g\x01\xa2\x00\x0f\x00\x9e\x00\x06\x00\xff\xff\xff\xff\xff\xff\x00\x00\xff\xff\xff\xff\xff\xff\x00\x00\x0f\x0e\x0f\x0f\x0e\r\x00\x00\x7f\x02.\xff\x00\x00~p\x00\x00\x00\x00u\xff\xf9\xff\x00\x00\x00\x00V\t\xd5\x01\xc0\x00\x00\x00\x007\xfb\xfc\x0b\x8d\x00', - b'\xf1\x81606DE051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640E0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x82CKJN3TMSDE0B\x00\x00\x00\x00', - b'\xf1\x82CKKN3TMD_H0A\x00\x00\x00\x00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5200 4C2CL104', b'\xf1\x00CK MDPS R 1.00 1.04 57700-J5220 4C2VL104', @@ -663,20 +387,6 @@ FW_VERSIONS = { b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 95740-J5000 170822', b'\xf1\x00CK MFC AT USA LHD 1.00 1.04 95740-J5000 180504', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\t\xb7\x17\xf5', - b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', - b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SCK0T33NB2\xb3\xee\xba\xdc', - b'\xf1\x87VCJLE17622572DK0vd6D\x99\x98y\x97vwVffUfvfC%CuT&Dx\x87o\xff{\x1c\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', - b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\t\xb7\x17\xf5', - b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', - b'\xf1\x87VDHLG17000192DK2xdFffT\xa5VUD$DwT\x86wveVeeD&T\x99\xba\x8f\xff\xcc\x99\xf1\x89E21\x00\x00\x00\x00\x00\x00\x00\xf1\x82SCK0T33NB0', - b'\xf1\x87VDHLG17034412DK2vD6DfVvVTD$D\x99w\x88\x98EDEDeT6DgfO\xff\xc3=\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', - b'\xf1\x87VDHLG17118862DK2\x8awWwgu\x96wVfUVwv\x97xWvfvUTGTx\x87o\xff\xc9\xed\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SCK0T33NB0\x88\xa2\xe6\xf0', - b'\xf1\x87VDHLG17274082DK2wfFf\x89x\x98wUT5T\x88v\x97xgeGefTGTVvO\xff\x1c\x14\xf1\x81E19\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E19\x00\x00\x00\x00\x00\x00\x00SCK0T33UB2\xee[\x97S', - b'\xf1\x87VDKLJ18675252DK6\x89vhgwwwwveVU\x88w\x87w\x99vgf\x97vXfgw_\xff\xc2\xfb\xf1\x89E25\x00\x00\x00\x00\x00\x00\x00\xf1\x82TCK0T33NB2', - b'\xf1\x87WAJTE17552812CH4vfFffvfVeT5DwvvVVdFeegeg\x88\x88o\xff\x1a]\xf1\x81E21\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00TCK2T20NB1\x19\xd2\x00\x94', - ], }, CAR.KIA_STINGER_2022: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -685,15 +395,11 @@ FW_VERSIONS = { b'\xf1\x00CK__ SCC FHCUP 1.00 1.00 99110-J5600 ', b'\xf1\x00CK__ SCC FHCUP 1.00 1.01 99110-J5100 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81640N2051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640R0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81HM6M1_0a0_H00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5300 4C2CL503', b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5320 4C2VL503', b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5380 4C2VR503', + b'\xf1\x00CK MDPS R 1.00 5.03 57700-J5520 4C4VL503', b'\xf1\x00CK MDPS R 1.00 5.04 57700-J5520 4C4VL504', ], (Ecu.fwdCamera, 0x7c4, None): [ @@ -702,29 +408,29 @@ FW_VERSIONS = { b'\xf1\x00CK MFC AT USA LHD 1.00 1.00 99211-J5500 210622', b'\xf1\x00CK MFC AT USA LHD 1.00 1.03 99211-J5000 201209', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 E31\x00\x00\x00\x00\x00\x00\x00SCK0T25KH2B\xfbI\xe2', - b'\xf1\x00bcsh8p54 E31\x00\x00\x00\x00\x00\x00\x00SCK0T33NH07\xdf\xf0\xc1', - b'\xf1\x00bcsh8p54 E31\x00\x00\x00\x00\x00\x00\x00TCK0T33NH0%g~\xd3', - b'\xf1\x87VCNLF11383972DK1vffV\x99\x99\x89\x98\x86eUU\x88wg\x89vfff\x97fff\x99\x87o\xff"\xc1\xf1\x81E30\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E30\x00\x00\x00\x00\x00\x00\x00SCK0T33GH0\xbe`\xfb\xc6', - ], }, - CAR.PALISADE: { + CAR.HYUNDAI_PALISADE: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00LX2 SCC FHCUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC F-CUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC F-CUP 1.00 1.05 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCU- 1.00 1.05 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCUP 1.00 1.00 99110-S8110 ', + b'\xf1\x00LX2_ SCC FHCUP 1.00 1.03 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ', b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ', + b'\xf1\x00ON__ FCA FHCUP 1.00 1.00 99110-S9110 ', b'\xf1\x00ON__ FCA FHCUP 1.00 1.01 99110-S9110 ', b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ', + b'\xf1\x00ON__ FCA FHCUP 1.00 1.03 99110-S9100 ', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360', b'\xf1\x00LX ESC \x01 1031\t\x10 58910-S8360', + b'\xf1\x00LX ESC \x01 104 \x10\x15 58910-S8350', + b'\xf1\x00LX ESC \x01 104 \x10\x16 58910-S8360', b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330', + b'\xf1\x00LX ESC \x0b 101\x19\x03 58910-S8360', b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330', b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330', b'\xf1\x00LX ESC \x0b 103\x19\t\t 58910-S8350', @@ -735,20 +441,19 @@ FW_VERSIONS = { b'\xf1\x00ON ESC \x0b 101\x19\t\x05 58910-S9320', b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640K0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640S1051\x00\x00\x00\x00\x00\x00\x00\x00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00LX2 MDPS C 1,00 1,03 56310-S8020 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8000 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-S8020 4LXDC103', + b'\xf1\x00LX2 MDPS C 1.00 1.03 56310-XX000 4LXDC103', b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8020 4LXDC104', + b'\xf1\x00LX2 MDPS C 1.00 1.04 56310-S8420 4LXDC104', + b'\xf1\x00LX2 MDPS R 1.00 1.02 56370-S8300 9318', b'\xf1\x00ON MDPS C 1.00 1.00 56340-S9000 8B13', b'\xf1\x00ON MDPS C 1.00 1.01 56340-S9000 9201', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00LX2 MFC AT KOR LHD 1.00 1.08 99211-S8100 200903', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.03 99211-S8100 190125', b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.05 99211-S8100 190909', @@ -758,74 +463,12 @@ FW_VERSIONS = { b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720', b'\xf1\x00ON MFC AT USA LHD 1.00 1.04 99211-S9100 211227', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', - b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00TON2G38NB5j\x94.\xde', - b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1', - b"\xf1\x87LBLUFN622950KF36\xa8\x88\x88\x88\x87w\x87xh\x99\x96\x89\x88\x99\x98\x89\x88\x99\x98\x89\x87o\xf6\xff\x98\x88o\xffx'\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8", - b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', - b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', - b'\xf1\x87LBLUFN731381KF36\xb9\x99\x89\x98\x98\x88\x88\x88\x89\x99\xa8\x99\x88\x99\xa8\x89\x88\x88\x98\x88V\x7f\xf6\xff\x99w\x8f\xff\xad\xd8\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8', - b'\xf1\x87LDKVAA0028604HH1\xa8\x88x\x87vgvw\x88\x99\xa8\x89gw\x86ww\x88\x97x\x97o\xf9\xff\x97w\x7f\xffo\x02\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', - b'\xf1\x87LDKVAA3068374HH1wwww\x87xw\x87y\x99\xa7\x99w\x88\x87xw\x88\x97x\x85\xaf\xfa\xffvU/\xffU\xdc\xf1\x81U872\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28', - b'\xf1\x87LDKVBN382172KF26\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\xa7\x89\x87\x88\x98x\x98\x99\xa9\x89\xa5_\xf6\xffDDO\xff\xcd\x16\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', - b'\xf1\x87LDKVBN424201KF26\xba\xaa\x9a\xa9\x99\x99\x89\x98\x89\x99\xa8\x99\x88\x99\x98\x89\x88\x99\xa8\x89v\x7f\xf7\xffwf_\xffq\xa6\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', - b'\xf1\x87LDKVBN540766KF37\x87wgv\x87w\x87xx\x99\x97\x89v\x88\x97h\x88\x88\x88\x88x\x7f\xf6\xffvUo\xff\xd3\x01\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', - b'\xf1\x87LDLVAA4225634HH1\x98\x88\x88\x88eUeVx\x88\x87\x88g\x88\x86xx\x88\x87\x88\x86o\xf9\xff\x87w\x7f\xff\xf2\xf7\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x87LDLVAA4478824HH1\x87wwwvfvg\x89\x99\xa8\x99w\x88\x87x\x89\x99\xa8\x99\xa6o\xfa\xfffU/\xffu\x92\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x87LDLVAA4777834HH1\x98\x88x\x87\x87wwwx\x88\x87\x88x\x99\x97\x89x\x88\x97\x88\x86o\xfa\xff\x86fO\xff\x1d9\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x87LDLVAA5194534HH1ffvguUUUx\x88\xa7\x88h\x99\x96\x89x\x88\x97\x88ro\xf9\xff\x98wo\xff\xaaM\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x87LDLVAA5949924HH1\xa9\x99y\x97\x87wwwx\x99\x97\x89x\x99\xa7\x89x\x99\xa7\x89\x87_\xfa\xffeD?\xff\xf1\xfd\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6', - b'\xf1\x87LDLVBN560098KF26\x86fff\x87vgfg\x88\x96xfw\x86gfw\x86g\x95\xf6\xffeU_\xff\x92c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB2\xafL]\xe7', - b'\xf1\x87LDLVBN602045KF26\xb9\x99\x89\x98\x97vwgy\xaa\xb7\x9af\x88\x96hw\x99\xa7y\xa9\x7f\xf5\xff\x99w\x7f\xff,\xd3\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN628911KF26\xa9\x99\x89\x98\x98\x88\x88\x88y\x99\xa7\x99fw\x86gw\x88\x87x\x83\x7f\xf6\xff\x98wo\xff2\xda\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN645817KF37\x87www\x98\x87xwx\x99\x97\x89\x99\x99\x99\x99g\x88\x96x\xb6_\xf7\xff\x98fo\xff\xe2\x86\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN662115KF37\x98\x88\x88\x88\xa8\x88\x88\x88x\x99\x97\x89x\x99\xa7\x89\x88\x99\xa8\x89\x88\x7f\xf7\xfffD_\xff\xdc\x84\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN667933KF37\xb9\x99\x89\x98\xb9\x99\x99\x99x\x88\x87\x88w\x88\x87x\x88\x88\x98\x88\xcbo\xf7\xffe3/\xffQ!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN673087KF37\x97www\x86fvgx\x99\x97\x89\x99\xaa\xa9\x9ag\x88\x86x\xe9_\xf8\xff\x98w\x7f\xff"\xad\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN673841KF37\x98\x88x\x87\x86g\x86xy\x99\xa7\x99\x88\x99\xa8\x89w\x88\x97xdo\xf5\xff\x98\x88\x8f\xffT\xec\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN681363KF37\x98\x88\x88\x88\x97x\x87\x88y\xaa\xa7\x9a\x88\x88\x98\x88\x88\x88\x88\x88vo\xf6\xffvD\x7f\xff%v\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN713782KF37\x99\x99y\x97\x98\x88\x88\x88x\x88\x97\x88\x88\x99\x98\x89\x88\x99\xa8\x89\x87o\xf7\xffeU?\xff7,\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN713890KF26\xb9\x99\x89\x98\xa9\x99\x99\x99x\x99\x97\x89\x88\x99\xa8\x89\x88\x99\xb8\x89Do\xf7\xff\xa9\x88o\xffs\r\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN733215KF37\x99\x98y\x87\x97wwwi\x99\xa6\x99x\x99\xa7\x89V\x88\x95h\x86o\xf7\xffeDO\xff\x12\xe7\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN750044KF37\xca\xa9\x8a\x98\xa7wwwy\xaa\xb7\x9ag\x88\x96x\x88\x99\xa8\x89\xb9\x7f\xf6\xff\xa8w\x7f\xff\xbe\xde\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN752612KF37\xba\xaa\x8a\xa8\x87w\x87xy\xaa\xa7\x9a\x88\x99\x98\x89x\x88\x97\x88\x96o\xf6\xffvU_\xffh\x1b\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN755553KF37\x87xw\x87\x97w\x87xy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95gwo\xf6\xffwUO\xff\xb5T\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08', - b'\xf1\x87LDLVBN757883KF37\x98\x87xw\x98\x87\x88xy\xaa\xb7\x9ag\x88\x96x\x89\x99\xa8\x99e\x7f\xf6\xff\xa9\x88o\xff5\x15\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN778156KF37\x87vWe\xa9\x99\x99\x99y\x99\xb7\x99\x99\x99\x99\x99x\x99\x97\x89\xa8\x7f\xf8\xffwf\x7f\xff\x82_\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN780576KF37\x98\x87hv\x97x\x97\x89x\x99\xa7\x89\x88\x99\x98\x89w\x88\x97x\x98\x7f\xf7\xff\xba\x88\x8f\xff\x1e0\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN783485KF37\x87www\x87vwgy\x99\xa7\x99\x99\x99\xa9\x99Vw\x95g\x89_\xf6\xff\xa9w_\xff\xc5\xd6\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN811844KF37\x87vwgvfffx\x99\xa7\x89Vw\x95gg\x88\xa6xe\x8f\xf6\xff\x97wO\xff\t\x80\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN830601KF37\xa7www\xa8\x87xwx\x99\xa7\x89Uw\x85Ww\x88\x97x\x88o\xf6\xff\x8a\xaa\x7f\xff\xe2:\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB4\xd6\xe8\xd7\xa6', - b'\xf1\x87LDMVBN848789KF37\x87w\x87x\x87w\x87xy\x99\xb7\x99\x87\x88\x98x\x88\x99\xa8\x89\x87\x7f\xf6\xfffUo\xff\xe3!\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN851595KF37\x97wgvvfffx\x99\xb7\x89\x88\x99\x98\x89\x87\x88\x98x\x99\x7f\xf7\xff\x97w\x7f\xff@\xf3\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN871852KF37\xb9\x99\x99\x99\xa8\x88\x88\x88y\x99\xa7\x99x\x99\xa7\x89\x88\x88\x98\x88\x89o\xf7\xff\xaa\x88o\xff\x0e\xed\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN873175KF26\xa8\x88\x88\x88vfVex\x99\xb7\x89\x88\x99\x98\x89x\x88\x97\x88f\x7f\xf7\xff\xbb\xaa\x8f\xff,\x04\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN879401KF26veVU\xa8\x88\x88\x88g\x88\xa6xVw\x95gx\x88\xa7\x88v\x8f\xf9\xff\xdd\xbb\xbf\xff\xb3\x99\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN881314KF37\xa8\x88h\x86\x97www\x89\x99\xa8\x99w\x88\x97xx\x99\xa7\x89\xca\x7f\xf8\xff\xba\x99\x8f\xff\xd8v\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN888651KF37\xa9\x99\x89\x98vfff\x88\x99\x98\x89w\x99\xa7y\x88\x88\x98\x88D\x8f\xf9\xff\xcb\x99\x8f\xff\xa5\x1e\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN889419KF37\xa9\x99y\x97\x87w\x87xx\x88\x97\x88w\x88\x97x\x88\x99\x98\x89e\x9f\xf9\xffeUo\xff\x901\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN895969KF37vefV\x87vgfx\x99\xa7\x89\x99\x99\xb9\x99f\x88\x96he_\xf7\xffxwo\xff\x14\xf9\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN899222KF37\xa8\x88x\x87\x97www\x98\x99\x99\x89\x88\x99\x98\x89f\x88\x96hdo\xf7\xff\xbb\xaa\x9f\xff\xe2U\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - b'\xf1\x87LDMVBN950669KF37\x97www\x96fffy\x99\xa7\x99\xa9\x99\xaa\x99g\x88\x96x\xb8\x8f\xf9\xffTD/\xff\xa7\xcb\xf1\x81U922\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00SLX4G38NB5\xb9\x94\xe8\x89', - ], - }, - CAR.VELOSTER: { + }, + CAR.HYUNDAI_VELOSTER: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00JS__ SCC H-CUP 1.00 1.02 95650-J3200 ', b'\xf1\x00JS__ SCC HNCUP 1.00 1.02 95650-J3100 ', ], - (Ecu.abs, 0x7d1, None): [ - b'\xf1\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816V8RAC00121.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x7e0, None): [ - b'\x01TJS-JDK06F200H0A', - b'\x01TJS-JNU06F200H0A', - b'391282BJF5 ', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00JSL MDPS C 1.00 1.03 56340-J3000 8308', ], @@ -833,119 +476,77 @@ FW_VERSIONS = { b'\xf1\x00JS LKAS AT KOR LHD 1.00 1.03 95740-J3000 K33', b'\xf1\x00JS LKAS AT USA LHD 1.00 1.02 95740-J3000 K32', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16KS2\x0e\xba\x1e\xa2', - b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\x00\x00\x00\x00', - b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJS0T16NS1\xba\x02\xb8\x80', - ], }, CAR.GENESIS_G70: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IK__ SCC F-CUP 1.00 1.01 96400-G9100 ', b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9420 4I4VL106', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', - b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\x7f\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', - ], }, CAR.GENESIS_G70_2020: { (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00IK MDPS R 1.00 1.06 57700-G9220 4I2VL106', b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9220 4I2VL107', b'\xf1\x00IK MDPS R 1.00 1.07 57700-G9420 4I4VL107', b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9200 4I2CL108', b'\xf1\x00IK MDPS R 1.00 1.08 57700-G9420 4I4VL108', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', - b'\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T20KB3Wuvz', - b'\xf1\x87VCJLP18407832DN3\x88vXfvUVT\x97eFU\x87d7v\x88eVeveFU\x89\x98\x7f\xff\xb2\xb0\xf1\x81E25\x00\x00\x00', - b'\xf1\x87VDJLC18480772DK9\x88eHfwfff\x87eFUeDEU\x98eFe\x86T5DVyo\xff\x87s\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33KB5\x9f\xa5&\x81', - b'\xf1\x87VDKLT18912362DN4wfVfwefeveVUwfvw\x88vWfvUFU\x89\xa9\x8f\xff\x87w\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB4\xecE\xefL', + b'\xf1\x00IK MDPS R 1.00 5.09 57700-G9520 4I4VL509', ], (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00IK__ SCC F-CUP 1.00 1.01 96400-G9100 ', b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 ', b'\xf1\x00IK__ SCC F-CUP 1.00 1.02 96400-G9100 \xf1\xa01.02', + b'\xf1\x00IK__ SCC FHCUP 1.00 1.00 99110-G9300 ', b'\xf1\x00IK__ SCC FHCUP 1.00 1.02 96400-G9000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00IK MFC AT KOR LHD 1.00 1.01 95740-G9000 170920', b'\xf1\x00IK MFC AT USA LHD 1.00 1.01 95740-G9000 170920', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81606G2051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640H0051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00IK MFC AT USA LHD 1.00 1.04 99211-G9000 220401', ], }, CAR.GENESIS_G80: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DH__ SCC F-CUP 1.00 1.01 96400-B1120 ', + b'\xf1\x00DH__ SCC F-CUP 1.00 1.02 96400-B1120 ', b'\xf1\x00DH__ SCC FHCUP 1.00 1.01 96400-B1110 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DH LKAS AT KOR LHD 1.01 1.01 95895-B1500 161014', b'\xf1\x00DH LKAS AT KOR LHD 1.01 1.02 95895-B1500 170810', b'\xf1\x00DH LKAS AT USA LHD 1.01 1.01 95895-B1500 161014', b'\xf1\x00DH LKAS AT USA LHD 1.01 1.02 95895-B1500 170810', b'\xf1\x00DH LKAS AT USA LHD 1.01 1.03 95895-B1500 180713', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G33KH2\xae\xde\xd5!', - b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0G38NH2j\x9dA\x1c', - b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00SDH0T33NH3\x97\xe6\xbc\xb8', - b'\xf1\x00bcsh8p54 E18\x00\x00\x00\x00\x00\x00\x00TDH0G38NH3:-\xa9n', - b'\xf1\x00bcsh8p54 E21\x00\x00\x00\x00\x00\x00\x00SDH0T33NH4\xd7O\x9e\xc9', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81640A4051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00DH LKAS AT USA LHD 1.01 1.04 95895-B1500 181213', ], }, CAR.GENESIS_G90: { - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87VDGMD15352242DD3w\x87gxwvgv\x87wvw\x88wXwffVfffUfw\x88o\xff\x06J\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', - b'\xf1\x87VDGMD15866192DD3x\x88x\x89wuFvvfUf\x88vWwgwwwvfVgx\x87o\xff\xbc^\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', - b'\xf1\x87VDHMD16446682DD3WwwxxvGw\x88\x88\x87\x88\x88whxx\x87\x87\x87\x85fUfwu_\xffT\xf8\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcshcm49 E14\x00\x00\x00\x00\x00\x00\x00SHI0G50NB1tc5\xb7', - ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00HI__ SCC F-CUP 1.00 1.01 96400-D2100 ', + b'\xf1\x00HI__ SCC FHCUP 1.00 1.02 99110-D2100 ', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2020 160302', b'\xf1\x00HI LKAS AT USA LHD 1.00 1.00 95895-D2030 170208', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x810000000000\x00', + b'\xf1\x00HI MFC AT USA LHD 1.00 1.03 99211-D2000 190831', ], }, - CAR.KONA: { + CAR.HYUNDAI_KONA: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00OS__ SCC F-CUP 1.00 1.00 95655-J9200 ', ], - (Ecu.abs, 0x7d1, None): [ - b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x7e0, None): [ - b'"\x01TOS-0NU06F301J02', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00OS MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U2VE051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VE051\x00\x00DOS4T16NS3\x00\x00\x00\x00', - ], }, CAR.KIA_CEED: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -957,13 +558,6 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CD LKAS AT EUR LHD 1.00 1.01 99211-J7000 B40', ], - (Ecu.engine, 0x7e0, None): [ - b'\x01TCD-JECU4F202H0K', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1\x00\x00\x00\x00', - b'\xf1\x816U2V7051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V7051\x00\x00DCD0T14US1U\x867Z', - ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00CD ESC \x03 102\x18\x08\x05 58920-J7350', ], @@ -984,22 +578,6 @@ FW_VERSIONS = { b'\xf1\x00BDPE_SCC FHCUPC 1.00 1.04 99110-M6500\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\xf1\x00BD__ SCC H-CUP 1.00 1.02 99110-M6000 ', ], - (Ecu.engine, 0x7e0, None): [ - b'\x01TBDM1NU06F200H01', - b'391182B945\x00', - b'\xf1\x81616F2051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.abs, 0x7d1, None): [ - b'\xf1\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x8758900-M7AB0 \xf1\x816VQRAD00127.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006V2B0_C2\x00\x006V2C6051\x00\x00CBD0N20NL1\x00\x00\x00\x00', - b'\xf1\x006V2B0_C2\x00\x006V2C6051\x00\x00CBD0N20NL1\x90@\xc6\xae', - b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\x00\x00\x00\x00', - b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3", - ], }, CAR.KIA_K5_2021: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -1007,17 +585,13 @@ FW_VERSIONS = { b'\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', b'\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ', b'\xf1\x00DL3_ SCC FHCUP 1.00 1.04 99110-L2100 ', - b'\xf1\x8799110L2000\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2000 ', - b'\xf1\x8799110L2100\xf1\x00DL3_ SCC F-CUP 1.00 1.03 99110-L2100 ', - b'\xf1\x8799110L2100\xf1\x00DL3_ SCC FHCUP 1.00 1.03 99110-L2100 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101', b'\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', b'\xf1\x00DL3 MDPS C 1.00 1.02 56310-L2220 4DLDC102', b'\xf1\x00DL3 MDPS C 1.00 1.02 56310L3220\x00 4DLAC102', - b'\xf1\x8756310-L3110\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101', - b'\xf1\x8756310-L3220\xf1\x00DL3 MDPS C 1.00 1.01 56310-L3220 4DLAC101', - b'\xf1\x8757700-L3000\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102', + b'\xf1\x00DL3 MDPS R 1.00 1.02 57700-L3000 4DLAP102', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00DL3 MFC AT KOR LHD 1.00 1.04 99210-L2000 210527', @@ -1027,29 +601,12 @@ FW_VERSIONS = { ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00DL ESC \x01 104 \x07\x12 58910-L2200', + b'\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600', b'\xf1\x00DL ESC \x06 101 \x04\x02 58910-L3200', b'\xf1\x00DL ESC \x06 103"\x08\x06 58910-L3200', b'\xf1\x00DL ESC \t 100 \x06\x02 58910-L3800', - b'\xf1\x8758910-L3200\xf1\x00DL ESC \x06 101 \x04\x02 58910-L3200', - b'\xf1\x8758910-L3600\xf1\x00DL ESC \x03 100 \x08\x02 58910-L3600', - b'\xf1\x8758910-L3800\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81HM6M2_0a0_DQ0', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x82DLDWN5TMDCXXXJ1B', - b'\xf1\x87391212MKT0', - b'\xf1\x87391212MKT3', - b'\xf1\x87391212MKV0', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00HT6TA261BLHT6TAB00A1SDL0C20KS0\x00\x00\x00\x00\x00\x00\\\x9f\xa5\x15', - b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', - b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB2.\x13\xf6\xed', - b'\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', - b'\xf1\x87954A02N300\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 WDL3T25XXX730NS2b\x1f\xb8%', - b'\xf1\x87SALFEA5652514GK2UUeV\x88\x87\x88xxwg\x87ww\x87wwfwvd/\xfb\xffvU_\xff\x93\xd3\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', - b'\xf1\x87SALFEA6046104GK2wvwgeTeFg\x88\x96xwwwwffvfe?\xfd\xff\x86fo\xff\x97A\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL2T16NB1ia\x0b\xb8', - b'\xf1\x87SCMSAA8572454GK1\x87x\x87\x88Vf\x86hgwvwvwwgvwwgT?\xfb\xff\x97fo\xffH\xb8\xf1\x81U913\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U913\x00\x00\x00\x00\x00\x00TDL4T16NB05\x94t\x18', + b'\xf1\x00DL ESC \t 101 \x07\x02 58910-L3800', + b'\xf1\x00DL ESC \t 102"\x08\x10 58910-L3800', ], }, CAR.KIA_K5_HEV_2020: { @@ -1064,17 +621,11 @@ FW_VERSIONS = { b'\xf1\x00DL3HMFC AT KOR LHD 1.00 1.02 99210-L2000 200309', b'\xf1\x00DL3HMFC AT KOR LHD 1.00 1.04 99210-L2000 210527', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391162JLA0', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00PSBG2323 E08\x00\x00\x00\x00\x00\x00\x00TDL2H20KA2\xe3\xc6cz', - b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDL2H20KA5T\xf2\xc9\xc2', - ], }, - CAR.KONA_EV: { + CAR.HYUNDAI_KONA_EV: { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000', + b'\xf1\x00OS IEB \x02 210 \x02\x14 58520-K4000', b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000', b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000', b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000', @@ -1085,10 +636,12 @@ FW_VERSIONS = { b'\xf1\x00OSE LKAS AT EUR LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT EUR RHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT KOR LHD 1.00 1.00 95740-K4100 W40', + b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4100 W40', b'\xf1\x00OSE LKAS AT USA LHD 1.00 1.00 95740-K4300 W50', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00OS MDPS C 1.00 1.03 56310/K4550 4OEDC103', + b'\xf1\x00OS MDPS C 1.00 1.04 56310-XX000 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4000\x00 4OEDC104', b'\xf1\x00OS MDPS C 1.00 1.04 56310K4050\x00 4OEDC104', ], @@ -1099,14 +652,12 @@ FW_VERSIONS = { b'\xf1\x00OSev SCC FNCUP 1.00 1.01 99110-K4000 ', ], }, - CAR.KONA_EV_2022: { + CAR.HYUNDAI_KONA_EV_2022: { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00OS IEB \x02 102"\x05\x16 58520-K4010', + b'\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010', b'\xf1\x00OS IEB \x03 102"\x05\x16 58520-K4010', b'\xf1\x00OS IEB \r 102"\x05\x16 58520-K4010', - b'\xf1\x8758520-K4010\xf1\x00OS IEB \x02 101 \x11\x13 58520-K4010', - b'\xf1\x8758520-K4010\xf1\x00OS IEB \x03 101 \x11\x13 58520-K4010', - b'\xf1\x8758520-K4010\xf1\x00OS IEB \x04 101 \x11\x13 58520-K4010', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00OSP LKA AT AUS RHD 1.00 1.04 99211-J9200 904', @@ -1128,7 +679,7 @@ FW_VERSIONS = { b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-K4500 \x00\x00\x00', ], }, - CAR.KONA_EV_2ND_GEN: { + CAR.HYUNDAI_KONA_EV_2ND_GEN: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00SXev RDR ----- 1.00 1.00 99110-BF000 ', ], @@ -1139,16 +690,15 @@ FW_VERSIONS = { CAR.KIA_NIRO_EV: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', + b'\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4000 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.02 96400-Q4100 ', b'\xf1\x00DEev SCC F-CUP 1.00 1.03 96400-Q4100 ', + b'\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', b'\xf1\x00DEev SCC FHCUP 1.00 1.03 96400-Q4000 ', - b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4000 ', - b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4100 ', - b'\xf1\x8799110Q4500\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4500 ', - b'\xf1\x8799110Q4600\xf1\x00DEev SCC F-CUP 1.00 1.00 99110-Q4600 ', - b'\xf1\x8799110Q4600\xf1\x00DEev SCC FHCUP 1.00 1.00 99110-Q4600 ', - b'\xf1\x8799110Q4600\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', + b'\xf1\x00DEev SCC FNCUP 1.00 1.00 99110-Q4600 ', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DE MDPS C 1.00 1.04 56310Q4100\x00 4DEEC104', @@ -1175,23 +725,12 @@ FW_VERSIONS = { ], }, CAR.KIA_NIRO_PHEV: { - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6F4051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL2&[\xc3\x01', - b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x00\x00\x00\x00', - b'\xf1\x816U3H3051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H3051\x00\x00PDE0G16NS1\x13\xcd\x88\x92', - b'\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\x00\x00\x00\x00', - b"\xf1\x816U3J2051\x00\x00\xf1\x006U3H0_C2\x00\x006U3J2051\x00\x00PDE0G16NS2\xf4'\\\x91", - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', b'\xf1\x00DE MDPS C 1.00 1.09 56310G5301\x00 4DEHC109', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DEH MFC AT USA LHD 1.00 1.00 95740-G5010 170117', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.00 95740-G5010 170117', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.01 95740-G5010 170424', b'\xf1\x00DEP MFC AT USA LHD 1.00 1.05 99211-G5000 190826', @@ -1202,14 +741,6 @@ FW_VERSIONS = { ], }, CAR.KIA_NIRO_PHEV_2022: { - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL3\x00\x00\x00\x00', - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00PDE0G16NL3\x00\x00\x00\x00', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', ], @@ -1219,40 +750,30 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DEhe SCC F-CUP 1.00 1.00 99110-G5600 ', + b'\xf1\x00DEhe SCC FHCUP 1.00 1.00 99110-G5600 ', ], }, CAR.KIA_NIRO_HEV_2021: { - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\x00\x00\x00\x00', - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HDE0G16NL3\xb9\xd3\xfaW', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00DE MDPS C 1.00 1.01 56310G5520\x00 4DEPC101', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DEH MFC AT KOR LHD 1.00 1.04 99211-G5000 190516', b'\xf1\x00DEH MFC AT USA LHD 1.00 1.00 99211-G5500 210428', b'\xf1\x00DEH MFC AT USA LHD 1.00 1.07 99211-G5000 201221', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DEhe SCC FHCUP 1.00 1.00 99110-G5600 ', + b'\xf1\x00DEhe SCC FHCUP 1.00 1.01 99110-G5000 ', ], }, CAR.KIA_SELTOS: { (Ecu.fwdRadar, 0x7d0, None): [ - b'\xf1\x8799110Q5100\xf1\x00SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 ', + b'\xf1\x00SP2_ SCC FHCUP 1.01 1.05 99110-Q5100 ', ], (Ecu.abs, 0x7d1, None): [ - b'\xf1\x8758910-Q5450\xf1\x00SP ESC \x07 101\x19\t\x05 58910-Q5450', - b'\xf1\x8758910-Q5450\xf1\x00SP ESC \t 101\x19\t\x05 58910-Q5450', - ], - (Ecu.engine, 0x7e0, None): [ - b'\x01TSP2KNL06F100J0K', - b'\x01TSP2KNL06F200J0K', - b'\xf1\x81616D2051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x81616D5051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x00SP ESC \x07 101\x19\t\x05 58910-Q5450', + b'\xf1\x00SP ESC \t 101\x19\t\x05 58910-Q5450', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00SP2 MDPS C 1.00 1.04 56300Q5200 ', @@ -1262,11 +783,6 @@ FW_VERSIONS = { b'\xf1\x00SP2 MFC AT USA LHD 1.00 1.04 99210-Q5000 191114', b'\xf1\x00SP2 MFC AT USA LHD 1.00 1.05 99210-Q5000 201012', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\x00T0190XBL T01950A1 DSP2T16X4X950NS6\xd30\xa5\xb9', - b'\xf1\x87954A22D200\xf1\x81T01950A1 \xf1\x00T0190XBL T01950A1 DSP2T16X4X950NS8\r\xfe\x9c\x8b', - b'\xf1\x87CZLUB49370612JF7h\xa8y\x87\x99\xa7hv\x99\x97fv\x88\x87x\x89x\x96O\xff\x88\xff\xff\xff.@\xf1\x816V2C2051\x00\x00\xf1\x006V2B0_C2\x00\x006V2C2051\x00\x00CSP4N20NS3\x00\x00\x00\x00', - ], }, CAR.KIA_OPTIMA_G4: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -1278,9 +794,6 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JFWGN LDWS AT USA LHD 1.00 1.02 95895-D4100 G21', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6J0051\x00\x00\xf1\x006T6J0_C2\x00\x006T6J0051\x00\x00TJF0T20NSB\x00\x00\x00\x00', - ], }, CAR.KIA_OPTIMA_G4_FL: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -1294,17 +807,6 @@ FW_VERSIONS = { b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5001 h32', b'\xf1\x00JFA LKAS AT USA LHD 1.00 1.00 95895-D5100 h32', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW', - b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\x00\x00\x00\x00', - b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\xca3\xeb.', - b'\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DJF0T16NL2\x9eA\x80\x01', - b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW', - b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\x00\x00\x00\x00', - b'\xf1\x816U2VA051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DJF0T16NL1\xca3\xeb.', - b'\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DJF0T16NL2\x9eA\x80\x01', - b'\xf1\x87\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xff\xf1\x816T6B8051\x00\x00\xf1\x006T6H0_C2\x00\x006T6B8051\x00\x00TJFSG24NH27\xa7\xc2\xb4', - ], }, CAR.KIA_OPTIMA_H: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -1321,28 +823,12 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00JFH MFC AT KOR LHD 1.00 1.01 95895-A8200 180323', ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6D1051\x00\x00\x00\x00\x00\x00\x00\x00', - ], }, - CAR.ELANTRA: { + CAR.HYUNDAI_ELANTRA: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00AD LKAS AT USA LHD 1.01 1.01 95895-F2000 251', b'\xf1\x00ADP LKAS AT USA LHD 1.00 1.03 99211-F2000 X31', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\x00\x00\x00\x00', - b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20NS2\xc5\x92\x9e\x8a', - b'\xf1\x006T6J0_C2\x00\x006T6F0051\x00\x00TAD0N20SS2.~\x90\x87', - b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD\x00\x00\x00\x00', - b'\xf1\x006T6K0_C2\x00\x006T6S2051\x00\x00TAD0N20NSD(\xfcA\x9d', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x8161657051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816165D051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816165E051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x8161698051\x00\x00\x00\x00\x00\x00\x00\x00', - ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00AD ESC \x11 11 \x18\x05\x06 58910-F2840', b'\xf1\x00AD ESC \x11 12 \x15\t\t 58920-F2810', @@ -1352,18 +838,12 @@ FW_VERSIONS = { b'\xf1\x00AD__ SCC H-CUP 1.00 1.01 96400-F2100 ', ], }, - CAR.ELANTRA_GT_I30: { + CAR.HYUNDAI_ELANTRA_GT_I30: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00PD LKAS AT KOR LHD 1.00 1.02 95740-G3000 A51', b'\xf1\x00PD LKAS AT USA LHD 1.00 1.02 95740-G3000 A51', b'\xf1\x00PD LKAS AT USA LHD 1.01 1.01 95740-G3100 A54', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U2U0_C2\x00\x006U2T0051\x00\x00DPD0D16KS0u\xce\x1fk', - b'\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DPD0T16NS4\x00\x00\x00\x00', - b'\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DPD0T16NS4\xda\x7f\xd6\xa7', - b'\xf1\x006U2V0_C2\x00\x006U2VA051\x00\x00DPD0H16NS0e\x0e\xcd\x8e', - ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00PD MDPS C 1.00 1.00 56310G3300\x00 4PDDC100', b'\xf1\x00PD MDPS C 1.00 1.03 56310/G3300 4PDDC103', @@ -1380,49 +860,35 @@ FW_VERSIONS = { b'\xf1\x00PD__ SCC FNCUP 1.01 1.00 96400-G3000 ', ], }, - CAR.ELANTRA_2021: { + CAR.HYUNDAI_ELANTRA_2021: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', b'\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', b'\xf1\x00CN7_ SCC FNCUP 1.00 1.01 99110-AA000 ', - b'\xf1\x8799110AA000\xf1\x00CN7_ SCC F-CUP 1.00 1.01 99110-AA000 ', - b'\xf1\x8799110AA000\xf1\x00CN7_ SCC FHCUP 1.00 1.01 99110-AA000 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA050 4CNDC106', b'\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106', b'\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106', + b'\xf1\x00CN7 MDPS C 1.00 1.07 56310AA050\x00 4CNDC107', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.00 99210-AB000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.01 99210-AB000 210205', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.02 99210-AB000 220111', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.03 99210-AB000 220426', b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.07 99210-AA000 220426', + b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.08 99210-AA000 220728', ], (Ecu.abs, 0x7d1, None): [ b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800', - b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', - b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', - b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\xe8\xba\xce\xfa', - b'\xf1\x87CXLQF40189012JL2f\x88\x86\x88\x88vUex\xb8\x88\x88\x88\x87\x88\x89fh?\xffz\xff\xff\xff\x08z\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', - b'\xf1\x87CXMQFM1916035JB2\x88vvgg\x87Wuwgev\xa9\x98\x88\x98h\x99\x9f\xffh\xff\xff\xff\xa5\xee\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', - b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\x7f\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', - b'\xf1\x87CXMQFM2728305JB2E\x97\x87xw\x87vwgw\x84x\x88\x88w\x89EI\xbf\xff{\xff\xff\xff\xe6\x0e\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', - b'\xf1\x87CXMQFM3806705JB2\x89\x87wwx\x88g\x86\x99\x87\x86xwwv\x88yv\x7f\xffz\xff\xff\xffV\x15\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81HM6M2_0a0_FF0', - b'\xf1\x82CNCVD0AMFCXCSFFB', - b'\xf1\x82CNCWD0AMFCXCSFFA', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_G80', - b'\xf1\x870\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81HM6M2_0a0_HC0', + b'\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800', + b'\xf1\x00CN ESC \t 105 \x10\x03 58910-AA800', ], }, - CAR.ELANTRA_HEV_2021: { + CAR.HYUNDAI_ELANTRA_HEV_2021: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.03 99210-AA000 200819', b'\xf1\x00CN7HMFC AT USA LHD 1.00 1.05 99210-AA000 210930', @@ -1432,28 +898,16 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', - b'\xf1\x8799110BY000\xf1\x00CNhe SCC FHCUP 1.00 1.01 99110-BY000 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102', + b'\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103', + b'\xf1\x00CN7 MDPS C 1.00 1.03 56310BY050\x00 4CNHC103', b'\xf1\x00CN7 MDPS C 1.00 1.03 56310BY0500 4CNHC103', b'\xf1\x00CN7 MDPS C 1.00 1.04 56310BY050\x00 4CNHC104', - b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102', - b'\xf1\x8756310/BY050\xf1\x00CN7 MDPS C 1.00 1.03 56310/BY050 4CNHC103', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00', - b'\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\xb9?A\xaa', - b'\xf1\x006U3L0_C2\x00\x006U3K9051\x00\x00HCN0G16NS1\x00\x00\x00\x00', - b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\x00\x00\x00\x00', - b'\xf1\x816U3K3051\x00\x00\xf1\x006U3L0_C2\x00\x006U3K3051\x00\x00HCN0G16NS0\xb9?A\xaa', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6G6051\x00\x00\x00\x00\x00\x00\x00\x00', - b'\xf1\x816H6G8051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.KONA_HEV: { + CAR.HYUNDAI_KONA_HEV: { (Ecu.abs, 0x7d1, None): [ b'\xf1\x00OS IEB \x01 104 \x11 58520-CM000', ], @@ -1466,64 +920,43 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00OSH LKAS AT KOR LHD 1.00 1.01 95740-CM000 l31', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HOS0G16DS1\x16\xc7\xb0\xd9', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00', - ], }, - CAR.SONATA_HYBRID: { + CAR.HYUNDAI_SONATA_HYBRID: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', + b'\xf1\x00DNhe SCC FHCUP 1.00 1.00 99110-L5000 ', b'\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', - b'\xf1\x8799110L5000\xf1\x00DNhe SCC F-CUP 1.00 1.02 99110-L5000 ', - b'\xf1\x8799110L5000\xf1\x00DNhe SCC FHCUP 1.00 1.02 99110-L5000 ', ], (Ecu.eps, 0x7d4, None): [ + b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L5000 4DNHC101', + b'\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102', + b'\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102', + b'\xf1\x00DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103', b'\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104', - b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102', - b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103', - b'\xf1\x8756310-L5500\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102', - b'\xf1\x8756310L5450\x00\xf1\x00DN8 MDPS C 1.00 1.03 56310L5450\x00 4DNHC104', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00DN8HMFC AT KOR LHD 1.00 1.03 99211-L1000 190705', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.04 99211-L1000 191016', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.05 99211-L1000 201109', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.06 99211-L1000 210325', b'\xf1\x00DN8HMFC AT USA LHD 1.00 1.07 99211-L1000 211223', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x00PSBG2323 E09\x00\x00\x00\x00\x00\x00\x00TDN2H20SA5\x97R\x88\x9e', - b'\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW', - b'\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDN2H20SA7\x1a3\xf9\xab', - b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E09\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2323 E09\x00\x00\x00\x00\x00\x00\x00TDN2H20SA5\x97R\x88\x9e', - b'\xf1\x87959102T250\x00\x00\x00\x00\x00\xf1\x81E14\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW', - b'\xf1\x87PCU\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x81E16\x00\x00\x00\x00\x00\x00\x00\xf1\x00PSBG2333 E16\x00\x00\x00\x00\x00\x00\x00TDN2H20SA7\x1a3\xf9\xab', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x87391062J002', - b'\xf1\x87391162J012', - b'\xf1\x87391162J013', - b'\xf1\x87391162J014', - ], }, CAR.KIA_SORENTO: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00UMP LKAS AT KOR LHD 1.00 1.00 95740-C5550 S30', + b'\xf1\x00UMP LKAS AT USA LHD 1.00 1.00 95740-C6550 d00', b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01', ], (Ecu.abs, 0x7d1, None): [ + b'\xf1\x00UM ESC \x02 12 \x18\x05\x05 58910-C6300', b'\xf1\x00UM ESC \x0c 12 \x18\x05\x06 58910-C6330', + b'\xf1\x00UM ESC \x13 12 \x17\x07\x05 58910-C5320', ], (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00UM__ SCC F-CUP 1.00 1.00 96400-C5500 ', b'\xf1\x00UM__ SCC F-CUP 1.00 1.00 96400-C6500 ', ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x87LDKUAA0348164HE3\x87www\x87www\x88\x88\xa8\x88w\x88\x97xw\x88\x97x\x86o\xf8\xff\x87f\x7f\xff\x15\xe0\xf1\x81U811\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U811\x00\x00\x00\x00\x00\x00TUM4G33NL3V|DG', - ], - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', - ], }, CAR.KIA_EV6: { (Ecu.fwdRadar, 0x7d0, None): [ @@ -1537,16 +970,21 @@ FW_VERSIONS = { b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.05 99210-CV000 211027', b'\xf1\x00CV1 MFC AT KOR LHD 1.00 1.06 99210-CV000 220328', b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV100 220630', + b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.00 99210-CV200 230510', b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.05 99210-CV000 211027', b'\xf1\x00CV1 MFC AT USA LHD 1.00 1.06 99210-CV000 220328', ], }, - CAR.IONIQ_5: { + CAR.HYUNDAI_IONIQ_5: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NE1_ RDR ----- 1.00 1.00 99110-GI000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NE1 MFC AT CAN LHD 1.00 1.01 99211-GI010 211007', + b'\xf1\x00NE1 MFC AT CAN LHD 1.00 1.05 99211-GI010 220614', + b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813', + b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI010 230110', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007', b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206', b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.00 99211-GI020 230719', @@ -1559,17 +997,21 @@ FW_VERSIONS = { b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.06 99211-GI010 230110', ], }, - CAR.IONIQ_6: { + CAR.HYUNDAI_IONIQ_6: { (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00CE__ RDR ----- 1.00 1.01 99110-KL000 ', ], (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00CE MFC AT CAN LHD 1.00 1.04 99211-KL000 221213', b'\xf1\x00CE MFC AT EUR LHD 1.00 1.03 99211-KL000 221011', + b'\xf1\x00CE MFC AT EUR LHD 1.00 1.04 99211-KL000 221213', b'\xf1\x00CE MFC AT USA LHD 1.00 1.04 99211-KL000 221213', ], }, - CAR.TUCSON_4TH_GEN: { + CAR.HYUNDAI_TUCSON_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.00 99211-N9260 14Y', + b'\xf1\x00NX4 FR_CMR AT CAN LHD 1.00 1.01 99211-N9100 14A', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 1.00 99211-N9220 14K', b'\xf1\x00NX4 FR_CMR AT EUR LHD 1.00 2.02 99211-N9000 14E', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-N9210 14G', @@ -1587,10 +1029,11 @@ FW_VERSIONS = { b'\xf1\x00NX4__ 1.01 1.00 99110-N9100 ', ], }, - CAR.SANTA_CRUZ_1ST_GEN: { + CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW000 14M', b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW010 14X', + b'\xf1\x00NX4 FR_CMR AT USA LHD 1.00 1.00 99211-CW020 14Z', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NX4__ 1.00 1.00 99110-K5000 ', @@ -1600,13 +1043,16 @@ FW_VERSIONS = { CAR.KIA_SPORTAGE_5TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00NQ5 FR_CMR AT AUS RHD 1.00 1.00 99211-P1040 663', + b'\xf1\x00NQ5 FR_CMR AT EUR LHD 1.00 1.00 99211-P1040 663', b'\xf1\x00NQ5 FR_CMR AT GEN LHD 1.00 1.00 99211-P1060 665', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1030 662', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1040 663', b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1060 665', + b'\xf1\x00NQ5 FR_CMR AT USA LHD 1.00 1.00 99211-P1070 690', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00NQ5__ 1.00 1.02 99110-P1000 ', + b'\xf1\x00NQ5__ 1.00 1.03 99110-CH000 ', b'\xf1\x00NQ5__ 1.00 1.03 99110-P1000 ', b'\xf1\x00NQ5__ 1.01 1.03 99110-CH000 ', b'\xf1\x00NQ5__ 1.01 1.03 99110-P1000 ', @@ -1679,13 +1125,16 @@ FW_VERSIONS = { }, CAR.KIA_CARNIVAL_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ + b'\xf1\x00KA4 MFC AT EUR LHD 1.00 1.06 99210-R0000 220221', b'\xf1\x00KA4 MFC AT KOR LHD 1.00 1.06 99210-R0000 220221', b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.00 99210-R0100 230105', + b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.01 99210-R0100 230710', b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.05 99210-R0000 201221', b'\xf1\x00KA4 MFC AT USA LHD 1.00 1.06 99210-R0000 220221', b'\xf1\x00KA4CMFC AT CHN LHD 1.00 1.01 99211-I4000 210525', ], (Ecu.fwdRadar, 0x7d0, None): [ + b'\xf1\x00KA4_ SCC F-CUP 1.00 1.03 99110-R0000 ', b'\xf1\x00KA4_ SCC FHCUP 1.00 1.00 99110-R0100 ', b'\xf1\x00KA4_ SCC FHCUP 1.00 1.03 99110-R0000 ', b'\xf1\x00KA4c SCC FHCUP 1.00 1.01 99110-I4000 ', @@ -1694,12 +1143,13 @@ FW_VERSIONS = { CAR.KIA_K8_HEV_1ST_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00GL3HMFC AT KOR LHD 1.00 1.03 99211-L8000 210907', + b'\xf1\x00GL3HMFC AT KOR LHD 1.00 1.04 99211-L8000 230207', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00GL3_ RDR ----- 1.00 1.02 99110-L8000 ', ], }, - CAR.STARIA_4TH_GEN: { + CAR.HYUNDAI_STARIA_4TH_GEN: { (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00US4 MFC AT KOR LHD 1.00 1.06 99211-CG000 230524', ], diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index bc29aeb985..b4b951f89e 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -1,9 +1,9 @@ import crcmod -from openpilot.selfdrive.car.hyundai.values import CAR, CHECKSUM, CAMERA_SCC_CAR +from openpilot.selfdrive.car.hyundai.values import CAR, HyundaiFlags hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) -def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, +def create_lkas11(packer, frame, CP, apply_steer, steer_req, torque_fault, lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, left_lane_depart, right_lane_depart): @@ -33,12 +33,12 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_ToiFlt"] = torque_fault # seems to allow actuation on CR_Lkas_StrToqReq values["CF_Lkas_MsgCount"] = frame % 0x10 - if car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE, - CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.GENESIS_G70_2020, - CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022, - CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, - CAR.SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, - CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN, CAR.CUSTIN_1ST_GEN): + if CP.carFingerprint in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.HYUNDAI_SANTA_FE, + CAR.HYUNDAI_IONIQ_EV_2020, CAR.HYUNDAI_IONIQ_PHEV, CAR.KIA_SELTOS, CAR.HYUNDAI_ELANTRA_2021, CAR.GENESIS_G70_2020, + CAR.HYUNDAI_ELANTRA_HEV_2021, CAR.HYUNDAI_SONATA_HYBRID, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022, + CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_IONIQ_HEV_2022, CAR.HYUNDAI_SANTA_FE_HEV_2022, + CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, + CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_CUSTIN_1ST_GEN): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 @@ -57,7 +57,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 # Likely cars lacking the ability to show individual lane lines in the dash - elif car_fingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): + elif CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): # SysWarning 4 = keep hands on wheel + beep values["CF_Lkas_SysWarning"] = 4 if sys_warning else 0 @@ -72,18 +72,18 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, values["CF_Lkas_LdwsActivemode"] = 0 values["CF_Lkas_FcwOpt_USM"] = 0 - elif car_fingerprint == CAR.HYUNDAI_GENESIS: + elif CP.carFingerprint == CAR.HYUNDAI_GENESIS: # This field is actually LdwsActivemode # Genesis and Optima fault when forwarding while engaged values["CF_Lkas_LdwsActivemode"] = 2 dat = packer.make_can_msg("LKAS11", 0, values)[2] - if car_fingerprint in CHECKSUM["crc8"]: + if CP.flags & HyundaiFlags.CHECKSUM_CRC8: # CRC Checksum as seen on 2019 Hyundai Santa Fe dat = dat[:6] + dat[7:8] checksum = hyundai_checksum(dat) - elif car_fingerprint in CHECKSUM["6B"]: + elif CP.flags & HyundaiFlags.CHECKSUM_6B: # Checksum of first 6 Bytes, as seen on 2018 Kia Sorento checksum = sum(dat[:6]) % 256 else: @@ -95,7 +95,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req, return packer.make_can_msg("LKAS11", 0, values) -def create_clu11(packer, frame, clu11, button, car_fingerprint): +def create_clu11(packer, frame, clu11, button, CP): values = {s: clu11[s] for s in [ "CF_Clu_CruiseSwState", "CF_Clu_CruiseSwMain", @@ -113,7 +113,7 @@ def create_clu11(packer, frame, clu11, button, car_fingerprint): values["CF_Clu_CruiseSwState"] = button values["CF_Clu_AliveCnt1"] = frame % 0x10 # send buttons to camera on camera-scc based cars - bus = 2 if car_fingerprint in CAMERA_SCC_CAR else 0 + bus = 2 if CP.flags & HyundaiFlags.CAMERA_SCC else 0 return packer.make_can_msg("CLU11", bus, values) @@ -126,12 +126,12 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0): } return packer.make_can_msg("LFAHDA_MFC", 0, values) -def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, set_speed, stopping, long_override, use_fca): +def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca): commands = [] scc11_values = { "MainMode_ACC": 1, - "TauGapSet": 4, + "TauGapSet": hud_control.leadDistanceBars, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, "ObjValid": 1, # close lead makes controls tighter @@ -167,7 +167,7 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, lead_visible, s "JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values "JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values "ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage - "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead + "ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead } commands.append(packer.make_can_msg("SCC14", 0, scc14_values)) diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index a35fcb7779..54804f94fd 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -121,7 +121,7 @@ def create_lfahda_cluster(packer, CAN, enabled): return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -146,7 +146,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov "SET_ME_2": 0x4, "SET_ME_3": 0x3, "SET_ME_TMP_64": 0x64, - "DISTANCE_SETTING": 4, + "DISTANCE_SETTING": hud_control.leadDistanceBars, } return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 049a63399c..22c54bce6b 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,6 +1,5 @@ from cereal import car from panda import Panda -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CANFD_CAR, CAMERA_SCC_CAR, CANFD_RADAR_SCC_CAR, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, EV_CAR, HYBRID_CAR, LEGACY_SAFETY_MODE_CAR, \ @@ -76,195 +75,8 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 0.4 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate in (CAR.AZERA_6TH_GEN, CAR.AZERA_HEV_6TH_GEN): - ret.mass = 1600. if candidate == CAR.AZERA_6TH_GEN else 1675. # ICE is ~average of 2.5L and 3.5L - ret.wheelbase = 2.885 - ret.steerRatio = 14.5 - elif 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 - ret.wheelbase = 2.766 - # Values from optimizer - ret.steerRatio = 16.55 # 13.8 is spec end-to-end - ret.tireStiffnessFactor = 0.82 - elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): - ret.mass = 1513. - ret.wheelbase = 2.84 - ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.SONATA_LF: - ret.mass = 1536. - ret.wheelbase = 2.804 - ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable - elif candidate == CAR.PALISADE: - ret.mass = 1999. - ret.wheelbase = 2.90 - ret.steerRatio = 15.6 * 1.15 - ret.tireStiffnessFactor = 0.63 - elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): - ret.mass = 1275. - ret.wheelbase = 2.7 - ret.steerRatio = 15.4 # 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535 - 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 - ret.wheelbase = 2.72 - ret.steerRatio = 12.9 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.ELANTRA_HEV_2021: - ret.mass = 3017. * CV.LB_TO_KG - ret.wheelbase = 2.72 - ret.steerRatio = 12.9 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.HYUNDAI_GENESIS: - 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, CAR.KONA_EV_2ND_GEN): - ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425., CAR.KONA_EV_2022: 1743., CAR.KONA_EV_2ND_GEN: 1740.}.get(candidate, 1275.) - ret.wheelbase = {CAR.KONA_EV_2ND_GEN: 2.66, }.get(candidate, 2.6) - ret.steerRatio = {CAR.KONA_EV_2ND_GEN: 13.6, }.get(candidate, 13.42) # Spec - 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. # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx - ret.wheelbase = 2.7 - ret.steerRatio = 13.73 # Spec - 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 in (CAR.IONIQ_5, CAR.IONIQ_6): - ret.mass = 1948 - ret.wheelbase = 2.97 - ret.steerRatio = 14.26 - ret.tireStiffnessFactor = 0.65 - elif candidate == CAR.VELOSTER: - ret.mass = 2917. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 * 1.15 - 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 - ret.tireStiffnessFactor = 0.385 - elif candidate == CAR.TUCSON_4TH_GEN: - ret.mass = 1630. # average - ret.wheelbase = 2.756 - ret.steerRatio = 16. - ret.tireStiffnessFactor = 0.385 - elif candidate == CAR.SANTA_CRUZ_1ST_GEN: - 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 - elif candidate == CAR.CUSTIN_1ST_GEN: - ret.mass = 1690. # from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0 - ret.wheelbase = 3.055 - ret.steerRatio = 17.0 # from learner - elif candidate == CAR.STARIA_4TH_GEN: - ret.mass = 2205. - ret.wheelbase = 3.273 - ret.steerRatio = 11.94 # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf - - # Kia - elif candidate == CAR.KIA_SORENTO: - 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, CAR.KIA_NIRO_PHEV_2022): - 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 - 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. - ret.wheelbase = 2.63 - ret.steerRatio = 14.56 - elif candidate == CAR.KIA_SPORTAGE_5TH_GEN: - ret.mass = 1725. # 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, CAR.KIA_OPTIMA_H_G4_FL): - ret.mass = 3558. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - 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. - ret.wheelbase = 2.78 - ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable - elif candidate == CAR.KIA_FORTE: - ret.mass = 2878. * CV.LB_TO_KG - ret.wheelbase = 2.80 - ret.steerRatio = 13.75 - ret.tireStiffnessFactor = 0.5 - elif candidate == CAR.KIA_CEED: - ret.mass = 1450. - ret.wheelbase = 2.65 - ret.steerRatio = 13.75 - ret.tireStiffnessFactor = 0.5 - elif candidate in (CAR.KIA_K5_2021, CAR.KIA_K5_HEV_2020): - ret.mass = 3381. * CV.LB_TO_KG - ret.wheelbase = 2.85 - ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) - ret.tireStiffnessFactor = 0.5 - elif candidate == CAR.KIA_EV6: - ret.mass = 2055 - ret.wheelbase = 2.9 - ret.steerRatio = 16. - ret.tireStiffnessFactor = 0.65 - elif candidate in (CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_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 - else: - ret.mass = 4396 * CV.LB_TO_KG - elif candidate == CAR.KIA_CARNIVAL_4TH_GEN: - ret.mass = 2087. - ret.wheelbase = 3.09 - ret.steerRatio = 14.23 - elif candidate == CAR.KIA_K8_HEV_1ST_GEN: - ret.mass = 1630. # https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid - ret.wheelbase = 2.895 - ret.steerRatio = 13.27 # guesstimate from K5 platform - - # Genesis - elif candidate == CAR.GENESIS_GV60_EV_1ST_GEN: - 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 - ret.wheelbase = 2.84 - ret.steerRatio = 13.56 - elif candidate == CAR.GENESIS_G70_2020: - 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. - ret.wheelbase = 2.87 - ret.steerRatio = 14.6 - elif candidate == CAR.GENESIS_G80: - ret.mass = 2060. - ret.wheelbase = 3.01 - ret.steerRatio = 16.5 - elif candidate == CAR.GENESIS_G90: - ret.mass = 2200. - ret.wheelbase = 3.15 - ret.steerRatio = 12.069 - elif candidate == CAR.GENESIS_GV80: - ret.mass = 2258. - ret.wheelbase = 2.95 - ret.steerRatio = 14.14 + if candidate == CAR.KIA_OPTIMA_G4_FL: + ret.steerActuatorDelay = 0.2 # *** longitudinal control *** if candidate in CANFD_CAR: @@ -323,7 +135,7 @@ class CarInterface(CarInterfaceBase): elif ret.flags & HyundaiFlags.EV: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_EV_GAS - if candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV, CAR.KONA_EV_2022): + if candidate in (CAR.HYUNDAI_KONA, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022): ret.flags |= HyundaiFlags.ALT_LIMITS.value ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_ALT_LIMITS @@ -366,6 +178,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/hyundai/tests/__init__.py b/selfdrive/car/hyundai/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/hyundai/tests/print_platform_codes.py b/selfdrive/car/hyundai/tests/print_platform_codes.py new file mode 100755 index 0000000000..f641535678 --- /dev/null +++ b/selfdrive/car/hyundai/tests/print_platform_codes.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +from cereal import car +from openpilot.selfdrive.car.hyundai.values import PLATFORM_CODE_ECUS, get_platform_codes +from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +if __name__ == "__main__": + for car_model, ecus in FW_VERSIONS.items(): + print() + print(car_model) + for ecu in sorted(ecus, key=lambda x: int(x[0])): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + platform_codes = get_platform_codes(ecus[ecu]) + codes = {code for code, _ in platform_codes} + dates = {date for _, date in platform_codes if date is not None} + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + print(f' Codes: {codes}') + print(f' Dates: {dates}') diff --git a/selfdrive/car/hyundai/tests/test_hyundai.py b/selfdrive/car/hyundai/tests/test_hyundai.py new file mode 100644 index 0000000000..4d31d7d15e --- /dev/null +++ b/selfdrive/car/hyundai/tests/test_hyundai.py @@ -0,0 +1,218 @@ +from hypothesis import settings, given, strategies as st + +import pytest + +from cereal import car +from openpilot.selfdrive.car.fw_versions import build_fw_dict +from openpilot.selfdrive.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ + HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ + UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ + get_platform_codes +from openpilot.selfdrive.car.hyundai.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +# Some platforms have date codes in a different format we don't yet parse (or are missing). +# For now, assert list of expected missing date cars +NO_DATES_PLATFORMS = { + # CAN FD + CAR.KIA_SPORTAGE_5TH_GEN, + CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN, + CAR.HYUNDAI_TUCSON_4TH_GEN, + # CAN + CAR.HYUNDAI_ELANTRA, + CAR.HYUNDAI_ELANTRA_GT_I30, + CAR.KIA_CEED, + CAR.KIA_FORTE, + CAR.KIA_OPTIMA_G4, + CAR.KIA_OPTIMA_G4_FL, + CAR.KIA_SORENTO, + CAR.HYUNDAI_KONA, + CAR.HYUNDAI_KONA_EV, + CAR.HYUNDAI_KONA_EV_2022, + CAR.HYUNDAI_KONA_HEV, + CAR.HYUNDAI_SONATA_LF, + CAR.HYUNDAI_VELOSTER, +} + +CANFD_EXPECTED_ECUS = {Ecu.fwdCamera, Ecu.fwdRadar} + + +class TestHyundaiFingerprint: + def test_can_features(self): + # Test no EV/HEV in any gear lists (should all use ELECT_GEAR) + assert set.union(*CAN_GEARS.values()) & (HYBRID_CAR | EV_CAR) == set() + + # Test CAN FD car not in CAN feature lists + can_specific_feature_list = set.union(*CAN_GEARS.values(), *CHECKSUM.values(), LEGACY_SAFETY_MODE_CAR, UNSUPPORTED_LONGITUDINAL_CAR, CAMERA_SCC_CAR) + for car_model in CANFD_CAR: + assert car_model not in can_specific_feature_list, "CAN FD car unexpectedly found in a CAN feature list" + + def test_hybrid_ev_sets(self): + assert HYBRID_CAR & EV_CAR == set(), "Shared cars between hybrid and EV" + assert CANFD_CAR & HYBRID_CAR == set(), "Hard coding CAN FD cars as hybrid is no longer supported" + + def test_canfd_ecu_whitelist(self): + # Asserts only expected Ecus can exist in database for CAN-FD cars + for car_model in CANFD_CAR: + ecus = {fw[0] for fw in FW_VERSIONS[car_model].keys()} + ecus_not_in_whitelist = ecus - CANFD_EXPECTED_ECUS + ecu_strings = ", ".join([f"Ecu.{ECU_NAME[ecu]}" for ecu in ecus_not_in_whitelist]) + assert len(ecus_not_in_whitelist) == 0, \ + f"{car_model}: Car model has unexpected ECUs: {ecu_strings}" + + def test_blacklisted_parts(self, subtests): + # Asserts no ECUs known to be shared across platforms exist in the database. + # Tucson having Santa Cruz camera and EPS for example + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + if car_model == CAR.HYUNDAI_SANTA_CRUZ_1ST_GEN: + pytest.skip("Skip checking Santa Cruz for its parts") + + for code, _ in get_platform_codes(ecus[(Ecu.fwdCamera, 0x7c4, None)]): + if b"-" not in code: + continue + part = code.split(b"-")[1] + assert not part.startswith(b'CW'), "Car has bad part number" + + def test_correct_ecu_response_database(self, subtests): + """ + Assert standard responses for certain ECUs, since they can + respond to multiple queries with different data + """ + expected_fw_prefix = HYUNDAI_VERSION_REQUEST_LONG[1:] + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for ecu, fws in ecus.items(): + assert all(fw.startswith(expected_fw_prefix) for fw in fws), \ + f"FW from unexpected request in database: {(ecu, fws)}" + + @settings(max_examples=100) + @given(data=st.data()) + def test_platform_codes_fuzzy_fw(self, data): + """Ensure function doesn't raise an exception""" + fw_strategy = st.lists(st.binary()) + fws = data.draw(fw_strategy) + get_platform_codes(fws) + + def test_expected_platform_codes(self, subtests): + # Ensures we don't accidentally add multiple platform codes for a car unless it is intentional + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + # Third and fourth character are usually EV/hybrid identifiers + codes = {code.split(b"-")[0][:2] for code, _ in get_platform_codes(fws)} + if car_model == CAR.HYUNDAI_PALISADE: + assert codes == {b"LX", b"ON"}, f"Car has unexpected platform codes: {car_model} {codes}" + elif car_model == CAR.HYUNDAI_KONA_EV and ecu[0] == Ecu.fwdCamera: + assert codes == {b"OE", b"OS"}, f"Car has unexpected platform codes: {car_model} {codes}" + else: + assert len(codes) == 1, f"Car has multiple platform codes: {car_model} {codes}" + + # Tests for platform codes, part numbers, and FW dates which Hyundai will use to fuzzy + # fingerprint in the absence of full FW matches: + def test_platform_code_ecus_available(self, subtests): + # TODO: add queries for these non-CAN FD cars to get EPS + no_eps_platforms = CANFD_CAR | {CAR.KIA_SORENTO, CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.KIA_OPTIMA_H, + CAR.KIA_OPTIMA_H_G4_FL, CAR.HYUNDAI_SONATA_LF, CAR.HYUNDAI_TUCSON, CAR.GENESIS_G90, CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA} + + # Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for platform_code_ecu in PLATFORM_CODE_ECUS: + if platform_code_ecu in (Ecu.fwdRadar, Ecu.eps) and car_model == CAR.HYUNDAI_GENESIS: + continue + if platform_code_ecu == Ecu.eps and car_model in no_eps_platforms: + continue + assert platform_code_ecu in [e[0] for e in ecus] + + def test_fw_format(self, subtests): + # Asserts: + # - every supported ECU FW version returns one platform code + # - every supported ECU FW version has a part number + # - expected parsing of ECU FW dates + + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + codes = set() + for fw in fws: + result = get_platform_codes([fw]) + assert 1 == len(result), f"Unable to parse FW: {fw}" + codes |= result + + if ecu[0] not in DATE_FW_ECUS or car_model in NO_DATES_PLATFORMS: + assert all(date is None for _, date in codes) + else: + assert all(date is not None for _, date in codes) + + if car_model == CAR.HYUNDAI_GENESIS: + pytest.skip("No part numbers for car model") + + # Hyundai places the ECU part number in their FW versions, assert all parsable + # Some examples of valid formats: b"56310-L0010", b"56310L0010", b"56310/M6300" + assert all(b"-" in code for code, _ in codes), \ + f"FW does not have part number: {fw}" + + def test_platform_codes_spot_check(self): + # Asserts basic platform code parsing behavior for a few cases + results = get_platform_codes([b"\xf1\x00DH LKAS 1.1 -150210"]) + assert results == {(b"DH", b"150210")} + + # Some cameras and all radars do not have dates + results = get_platform_codes([b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 "]) + assert results == {(b"AEhe-G2000", None)} + + results = get_platform_codes([b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 "]) + assert results == {(b"CV1-CV000", None)} + + results = get_platform_codes([ + b"\xf1\x00DH LKAS 1.1 -150210", + b"\xf1\x00AEhe SCC H-CUP 1.01 1.01 96400-G2000 ", + b"\xf1\x00CV1_ RDR ----- 1.00 1.01 99110-CV000 ", + ]) + assert results == {(b"DH", b"150210"), (b"AEhe-G2000", None), (b"CV1-CV000", None)} + + results = get_platform_codes([ + b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.07 99211-S8100 220222", + b"\xf1\x00LX2 MFC AT USA LHD 1.00 1.08 99211-S8100 211103", + b"\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 190405", + b"\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 190720", + ]) + assert results == {(b"LX2-S8100", b"220222"), (b"LX2-S8100", b"211103"), + (b"ON-S9100", b"190405"), (b"ON-S9100", b"190720")} + + def test_fuzzy_excluded_platforms(self): + # Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared. + # This list can be shrunk as we combine platforms and detect features + excluded_platforms = { + CAR.GENESIS_G70, # shared platform code, part number, and date + CAR.GENESIS_G70_2020, + } + excluded_platforms |= CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST # shared platform codes + excluded_platforms |= NO_DATES_PLATFORMS # date codes are required to match + + platforms_with_shared_codes = set() + for platform, fw_by_addr in FW_VERSIONS.items(): + car_fw = [] + for ecu, fw_versions in fw_by_addr.items(): + ecu_name, addr, sub_addr = ecu + for fw in fw_versions: + car_fw.append({"ecu": ecu_name, "fwVersion": fw, "address": addr, + "subAddress": 0 if sub_addr is None else sub_addr}) + + CP = car.CarParams.new_message(carFw=car_fw) + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS) + if len(matches) == 1: + assert list(matches)[0] == platform + else: + platforms_with_shared_codes.add(platform) + + assert platforms_with_shared_codes == excluded_platforms diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 20fc29897d..c489ea0042 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1,13 +1,12 @@ import re -from dataclasses import dataclass -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Optional, Set, Tuple, Union +from dataclasses import dataclass, field +from enum import Enum, IntFlag from cereal import car from panda.python import uds from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu @@ -36,8 +35,8 @@ class CarControllerParams: # To determine the limit for your car, find the maximum value that the stock LKAS will request. # If the max stock LKAS request is <384, add your car to this list. - elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.IONIQ, - CAR.IONIQ_EV_LTD, CAR.SANTA_FE_PHEV_2022, CAR.SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, + elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.GENESIS_G90, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ, + CAR.HYUNDAI_IONIQ_EV_LTD, CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.HYUNDAI_SONATA_LF, CAR.KIA_FORTE, CAR.KIA_NIRO_PHEV, CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.KIA_SORENTO): self.STEER_MAX = 255 @@ -53,92 +52,48 @@ class CarControllerParams: class HyundaiFlags(IntFlag): + # Dynamic Flags CANFD_HDA2 = 1 CANFD_ALT_BUTTONS = 2 - CANFD_ALT_GEARS = 4 - CANFD_CAMERA_SCC = 8 + CANFD_ALT_GEARS = 2 ** 2 + CANFD_CAMERA_SCC = 2 ** 3 - ALT_LIMITS = 16 - ENABLE_BLINKERS = 32 - CANFD_ALT_GEARS_2 = 64 - SEND_LFA = 128 - USE_FCA = 256 - CANFD_HDA2_ALT_STEERING = 512 - HYBRID = 1024 - EV = 2048 + ALT_LIMITS = 2 ** 4 + ENABLE_BLINKERS = 2 ** 5 + CANFD_ALT_GEARS_2 = 2 ** 6 + SEND_LFA = 2 ** 7 + USE_FCA = 2 ** 8 + CANFD_HDA2_ALT_STEERING = 2 ** 9 + # these cars use a different gas signal + HYBRID = 2 ** 10 + EV = 2 ** 11 -class CAR(StrEnum): - # Hyundai - AZERA_6TH_GEN = "HYUNDAI AZERA 6TH GEN" - AZERA_HEV_6TH_GEN = "HYUNDAI AZERA HYBRID 6TH GEN" - ELANTRA = "HYUNDAI ELANTRA 2017" - ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT" - ELANTRA_2021 = "HYUNDAI ELANTRA 2021" - ELANTRA_HEV_2021 = "HYUNDAI ELANTRA HYBRID 2021" - HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016" - IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019" - IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022" - IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019" - IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020" - IONIQ_PHEV_2019 = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" - IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020" - KONA = "HYUNDAI KONA 2020" - KONA_EV = "HYUNDAI KONA ELECTRIC 2019" - KONA_EV_2022 = "HYUNDAI KONA ELECTRIC 2022" - KONA_EV_2ND_GEN = "HYUNDAI KONA ELECTRIC 2ND GEN" - KONA_HEV = "HYUNDAI KONA HYBRID 2020" - SANTA_FE = "HYUNDAI SANTA FE 2019" - SANTA_FE_2022 = "HYUNDAI SANTA FE 2022" - SANTA_FE_HEV_2022 = "HYUNDAI SANTA FE HYBRID 2022" - SANTA_FE_PHEV_2022 = "HYUNDAI SANTA FE PlUG-IN HYBRID 2022" - SONATA = "HYUNDAI SONATA 2020" - SONATA_LF = "HYUNDAI SONATA 2019" - STARIA_4TH_GEN = "HYUNDAI STARIA 4TH GEN" - TUCSON = "HYUNDAI TUCSON 2019" - PALISADE = "HYUNDAI PALISADE 2020" - VELOSTER = "HYUNDAI VELOSTER 2019" - SONATA_HYBRID = "HYUNDAI SONATA HYBRID 2021" - IONIQ_5 = "HYUNDAI IONIQ 5 2022" - IONIQ_6 = "HYUNDAI IONIQ 6 2023" - TUCSON_4TH_GEN = "HYUNDAI TUCSON 4TH GEN" - SANTA_CRUZ_1ST_GEN = "HYUNDAI SANTA CRUZ 1ST GEN" - CUSTIN_1ST_GEN = "HYUNDAI CUSTIN 1ST GEN" + # Static flags - # Kia - KIA_FORTE = "KIA FORTE E 2018 & GT 2021" - KIA_K5_2021 = "KIA K5 2021" - KIA_K5_HEV_2020 = "KIA K5 HYBRID 2020" - KIA_K8_HEV_1ST_GEN = "KIA K8 HYBRID 1ST GEN" - KIA_NIRO_EV = "KIA NIRO EV 2020" - KIA_NIRO_EV_2ND_GEN = "KIA NIRO EV 2ND GEN" - KIA_NIRO_PHEV = "KIA NIRO HYBRID 2019" - KIA_NIRO_PHEV_2022 = "KIA NIRO PLUG-IN HYBRID 2022" - KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021" - KIA_NIRO_HEV_2ND_GEN = "KIA NIRO HYBRID 2ND GEN" - KIA_OPTIMA_G4 = "KIA OPTIMA 4TH GEN" - KIA_OPTIMA_G4_FL = "KIA OPTIMA 4TH GEN FACELIFT" - KIA_OPTIMA_H = "KIA OPTIMA HYBRID 2017 & SPORTS 2019" - KIA_OPTIMA_H_G4_FL = "KIA OPTIMA HYBRID 4TH GEN FACELIFT" - KIA_SELTOS = "KIA SELTOS 2021" - KIA_SPORTAGE_5TH_GEN = "KIA SPORTAGE 5TH GEN" - KIA_SORENTO = "KIA SORENTO GT LINE 2018" - KIA_SORENTO_4TH_GEN = "KIA SORENTO 4TH GEN" - KIA_SORENTO_HEV_4TH_GEN = "KIA SORENTO HYBRID 4TH GEN" - KIA_STINGER = "KIA STINGER GT2 2018" - KIA_STINGER_2022 = "KIA STINGER 2022" - KIA_CEED = "KIA CEED INTRO ED 2019" - KIA_EV6 = "KIA EV6 2022" - KIA_CARNIVAL_4TH_GEN = "KIA CARNIVAL 4TH GEN" + # If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. + # If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py + MANDO_RADAR = 2 ** 12 + CANFD = 2 ** 13 - # Genesis - GENESIS_GV60_EV_1ST_GEN = "GENESIS GV60 ELECTRIC 1ST GEN" - GENESIS_G70 = "GENESIS G70 2018" - GENESIS_G70_2020 = "GENESIS G70 2020" - GENESIS_GV70_1ST_GEN = "GENESIS GV70 1ST GEN" - GENESIS_G80 = "GENESIS G80 2017" - GENESIS_G90 = "GENESIS G90 2017" - GENESIS_GV80 = "GENESIS GV80 2023" + # The radar does SCC on these cars when HDA I, rather than the camera + RADAR_SCC = 2 ** 14 + CAMERA_SCC = 2 ** 15 + CHECKSUM_CRC8 = 2 ** 16 + CHECKSUM_6B = 2 ** 17 + + # these cars require a special panda safety mode due to missing counters and checksums in the messages + LEGACY = 2 ** 18 + + # these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. + UNSUPPORTED_LONGITUDINAL = 2 ** 19 + + CANFD_NO_RADAR_DISABLE = 2 ** 20 + + CLUSTER_GEARS = 2 ** 21 + TCU_GEARS = 2 ** 22 + + MIN_STEER_32_MPH = 2 ** 23 class Footnote(Enum): @@ -149,164 +104,443 @@ class Footnote(Enum): @dataclass -class HyundaiCarInfo(CarInfo): +class HyundaiCarDocs(CarDocs): package: str = "Smart Cruise Control (SCC)" def init_make(self, CP: car.CarParams): - if CP.carFingerprint in CANFD_CAR: + if CP.flags & HyundaiFlags.CANFD: self.footnotes.insert(0, Footnote.CANFD) -CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { - CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.AZERA_HEV_6TH_GEN: [ - HyundaiCarInfo("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - ], - CAR.ELANTRA: [ - # TODO: 2017-18 could be Hyundai G - HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), - HyundaiCarInfo("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), - ], - CAR.ELANTRA_GT_I30: [ - HyundaiCarInfo("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - HyundaiCarInfo("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", - car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.HYUNDAI_GENESIS: [ - # TODO: check 2015 packages - HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), - HyundaiCarInfo("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), - ], - CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h])), # TODO: confirm 2020-21 harness - CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", car_parts=CarParts.common([CarHarness.hyundai_b])), - CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g])), - CAR.KONA_EV_2022: HyundaiCarInfo("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o])), - CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i])), # TODO: check packages - # TODO: this is the 2024 US MY, not yet released - CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw", - car_parts=CarParts.common([CarHarness.hyundai_r])), - CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", - car_parts=CarParts.common([CarHarness.hyundai_d])), - CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", - car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", - car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.STARIA_4TH_GEN: HyundaiCarInfo("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.TUCSON: [ - HyundaiCarInfo("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])), - ], - CAR.PALISADE: [ - HyundaiCarInfo("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])), - HyundaiCarInfo("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - ], - CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.IONIQ_5: [ - HyundaiCarInfo("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])), - HyundaiCarInfo("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), - HyundaiCarInfo("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), - ], - CAR.IONIQ_6: [ - HyundaiCarInfo("Hyundai Ioniq 6 (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])), - ], - CAR.TUCSON_4TH_GEN: [ - HyundaiCarInfo("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Hyundai Tucson 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), - ], - CAR.SANTA_CRUZ_1ST_GEN: HyundaiCarInfo("Hyundai Santa Cruz 2022-23", car_parts=CarParts.common([CarHarness.hyundai_n])), - CAR.CUSTIN_1ST_GEN: HyundaiCarInfo("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), +@dataclass +class HyundaiPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_kia_generic", None)) + + def init(self): + if self.flags & HyundaiFlags.MANDO_RADAR: + self.dbc_dict = dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated') + + if self.flags & HyundaiFlags.MIN_STEER_32_MPH: + self.specs = self.specs.override(minSteerSpeed=32 * CV.MPH_TO_MS) + + +@dataclass +class HyundaiCanFDPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict("hyundai_canfd", None)) + + def init(self): + self.flags |= HyundaiFlags.CANFD + + +class CAR(Platforms): + # Hyundai + HYUNDAI_AZERA_6TH_GEN = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=1600, wheelbase=2.885, steerRatio=14.5), + ) + HYUNDAI_AZERA_HEV_6TH_GEN = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], + CarSpecs(mass=1675, wheelbase=2.885, steerRatio=14.5), + flags=HyundaiFlags.HYBRID, + ) + HYUNDAI_ELANTRA = HyundaiPlatformConfig( + [ + # TODO: 2017-18 could be Hyundai G + HyundaiCarDocs("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), + HyundaiCarDocs("Hyundai Elantra 2019", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), + ], + # steerRatio: 14 is Stock | Settled Params Learner values are steerRatio: 15.401566348670535, stiffnessFactor settled on 1.0081302973865127 + CarSpecs(mass=1275, wheelbase=2.7, steerRatio=15.4, tireStiffnessFactor=0.385), + flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + HYUNDAI_ELANTRA_GT_I30 = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Hyundai Elantra GT 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), + HyundaiCarDocs("Hyundai i30 2017-19", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + HYUNDAI_ELANTRA.specs, + flags=HyundaiFlags.LEGACY | HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + HYUNDAI_ELANTRA_2021 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Elantra 2021-23", video_link="https://youtu.be/_EdYQtV52-c", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=2800 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_ELANTRA_HEV_2021 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Elantra Hybrid 2021-23", video_link="https://youtu.be/_EdYQtV52-c", + car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=3017 * CV.LB_TO_KG, wheelbase=2.72, steerRatio=12.9, tireStiffnessFactor=0.65), + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + HYUNDAI_GENESIS = HyundaiPlatformConfig( + [ + # TODO: check 2015 packages + HyundaiCarDocs("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), + HyundaiCarDocs("Genesis G80 2017", "All", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_j])), + ], + CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5, minSteerSpeed=60 * CV.KPH_TO_MS), + flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.LEGACY, + ) + HYUNDAI_IONIQ = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Hybrid 2017-19", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, + ) + HYUNDAI_IONIQ_HEV_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h]))], # TODO: confirm 2020-21 harness, + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, + ) + HYUNDAI_IONIQ_EV_LTD = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Electric 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV | HyundaiFlags.LEGACY | HyundaiFlags.MIN_STEER_32_MPH, + ) + HYUNDAI_IONIQ_EV_2020 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Electric 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV, + ) + HYUNDAI_IONIQ_PHEV_2019 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH, + ) + HYUNDAI_IONIQ_PHEV = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq Plug-in Hybrid 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID, + ) + HYUNDAI_KONA = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Kona 2020", min_enable_speed=6 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b]))], + CarSpecs(mass=1275, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.CLUSTER_GEARS, + ) + HYUNDAI_KONA_EV = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Kona Electric 2018-21", car_parts=CarParts.common([CarHarness.hyundai_g]))], + CarSpecs(mass=1685, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV, + ) + HYUNDAI_KONA_EV_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Kona Electric 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o]))], + CarSpecs(mass=1743, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.CAMERA_SCC | HyundaiFlags.EV, + ) + HYUNDAI_KONA_EV_2ND_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw", + car_parts=CarParts.common([CarHarness.hyundai_r]))], + CarSpecs(mass=1740, wheelbase=2.66, steerRatio=13.6, tireStiffnessFactor=0.385), + flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, + ) + HYUNDAI_KONA_HEV = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Kona Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_i]))], # TODO: check packages, + CarSpecs(mass=1425, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385), + flags=HyundaiFlags.HYBRID, + ) + HYUNDAI_SANTA_FE = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", + car_parts=CarParts.common([CarHarness.hyundai_d]))], + CarSpecs(mass=3982 * CV.LB_TO_KG, wheelbase=2.766, steerRatio=16.55, tireStiffnessFactor=0.82), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_SANTA_FE_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", + car_parts=CarParts.common([CarHarness.hyundai_l]))], + HYUNDAI_SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_SANTA_FE_HEV_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], + HYUNDAI_SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + HYUNDAI_SANTA_FE_PHEV_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Santa Fe Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l]))], + HYUNDAI_SANTA_FE.specs, + flags=HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + HYUNDAI_SONATA = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Sonata 2020-23", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", + car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=1513, wheelbase=2.84, steerRatio=13.27 * 1.15, tireStiffnessFactor=0.65), # 15% higher at the center seems reasonable + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_SONATA_LF = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Sonata 2018-19", car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=1536, wheelbase=2.804, steerRatio=13.27 * 1.15), # 15% higher at the center seems reasonable + + flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, + ) + HYUNDAI_STARIA_4TH_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Hyundai Staria 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=2205, wheelbase=3.273, steerRatio=11.94), # https://www.hyundai.com/content/dam/hyundai/au/en/models/staria-load/premium-pip-update-2023/spec-sheet/STARIA_Load_Spec-Table_March_2023_v3.1.pdf + ) + HYUNDAI_TUCSON = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Hyundai Tucson 2021", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Hyundai Tucson Diesel 2019", car_parts=CarParts.common([CarHarness.hyundai_l])), + ], + CarSpecs(mass=3520 * CV.LB_TO_KG, wheelbase=2.67, steerRatio=16.1, tireStiffnessFactor=0.385), + flags=HyundaiFlags.TCU_GEARS, + ) + HYUNDAI_PALISADE = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Hyundai Palisade 2020-22", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])), + HyundaiCarDocs("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), + ], + CarSpecs(mass=1999, wheelbase=2.9, steerRatio=15.6 * 1.15, tireStiffnessFactor=0.63), + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8, + ) + HYUNDAI_VELOSTER = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=2917 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75 * 1.15, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS, + ) + HYUNDAI_SONATA_HYBRID = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Sonata Hybrid 2020-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + HYUNDAI_SONATA.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + HYUNDAI_IONIQ_5 = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Hyundai Ioniq 5 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_q])), + HyundaiCarDocs("Hyundai Ioniq 5 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_k])), + HyundaiCarDocs("Hyundai Ioniq 5 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), + ], + CarSpecs(mass=1948, wheelbase=2.97, steerRatio=14.26, tireStiffnessFactor=0.65), + flags=HyundaiFlags.EV, + ) + HYUNDAI_IONIQ_6 = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Hyundai Ioniq 6 (with HDA II) 2023-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p]))], + HYUNDAI_IONIQ_5.specs, + flags=HyundaiFlags.EV | HyundaiFlags.CANFD_NO_RADAR_DISABLE, + ) + HYUNDAI_TUCSON_4TH_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Hyundai Tucson 2022", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Hyundai Tucson 2023-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Hyundai Tucson Hybrid 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Hyundai Tucson Plug-in Hybrid 2024", "All", car_parts=CarParts.common([CarHarness.hyundai_n])), + ], + CarSpecs(mass=1630, wheelbase=2.756, steerRatio=13.7, tireStiffnessFactor=0.385), + ) + HYUNDAI_SANTA_CRUZ_1ST_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Hyundai Santa Cruz 2022-24", car_parts=CarParts.common([CarHarness.hyundai_n]))], + # weight from Limited trim - the only supported trim, steering ratio according to Hyundai News https://www.hyundainews.com/assets/documents/original/48035-2022SantaCruzProductGuideSpecsv2081521.pdf + CarSpecs(mass=1870, wheelbase=3, steerRatio=14.2), + ) + HYUNDAI_CUSTIN_1ST_GEN = HyundaiPlatformConfig( + [HyundaiCarDocs("Hyundai Custin 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=1690, wheelbase=3.055, steerRatio=17), # mass: from https://www.hyundai-motor.com.tw/clicktobuy/custin#spec_0, steerRatio: from learner + flags=HyundaiFlags.CHECKSUM_CRC8, + ) # Kia - CAR.KIA_FORTE: [ - HyundaiCarInfo("Kia Forte 2019-21", car_parts=CarParts.common([CarHarness.hyundai_g])), - HyundaiCarInfo("Kia Forte 2023", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_K5_HEV_2020: HyundaiCarInfo("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_K8_HEV_1ST_GEN: HyundaiCarInfo("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q])), - CAR.KIA_NIRO_EV: [ - HyundaiCarInfo("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), - HyundaiCarInfo("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])), - HyundaiCarInfo("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), - ], - CAR.KIA_NIRO_EV_2ND_GEN: HyundaiCarInfo("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_NIRO_PHEV: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_d])), - ], - CAR.KIA_NIRO_PHEV_2022: [ - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2021", "All", car_parts=CarParts.common([CarHarness.hyundai_d])), - HyundaiCarInfo("Kia Niro Plug-in Hybrid 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - ], - CAR.KIA_NIRO_HEV_2021: [ - HyundaiCarInfo("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), - HyundaiCarInfo("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), - ], - CAR.KIA_NIRO_HEV_2ND_GEN: HyundaiCarInfo("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_OPTIMA_G4: HyundaiCarInfo("Kia Optima 2017", "Advanced Smart Cruise Control", - car_parts=CarParts.common([CarHarness.hyundai_b])), # TODO: may support 2016, 2018 - CAR.KIA_OPTIMA_G4_FL: HyundaiCarInfo("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g])), + KIA_FORTE = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Forte 2019-21", min_enable_speed=6 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_g])), + HyundaiCarDocs("Kia Forte 2022-23", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + CarSpecs(mass=2878 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5) + ) + KIA_K5_2021 = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia K5 2021-24", car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=3381 * CV.LB_TO_KG, wheelbase=2.85, steerRatio=13.27, tireStiffnessFactor=0.5), # 2021 Kia K5 Steering Ratio (all trims) + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + KIA_K5_HEV_2020 = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia K5 Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_K5_2021.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8 | HyundaiFlags.HYBRID, + ) + KIA_K8_HEV_1ST_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Kia K8 Hybrid (with HDA II) 2023", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_q]))], + # mass: https://carprices.ae/brands/kia/2023/k8/1.6-turbo-hybrid, steerRatio: guesstimate from K5 platform + CarSpecs(mass=1630, wheelbase=2.895, steerRatio=13.27) + ) + KIA_NIRO_EV = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Niro EV 2019", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), + HyundaiCarDocs("Kia Niro EV 2020", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_f])), + HyundaiCarDocs("Kia Niro EV 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro EV 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", car_parts=CarParts.common([CarHarness.hyundai_h])), + ], + CarSpecs(mass=3543 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=13.6, tireStiffnessFactor=0.385), # average of all the cars + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.EV, + ) + KIA_NIRO_EV_2ND_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Kia Niro EV 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.EV, + ) + KIA_NIRO_PHEV = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Niro Hybrid 2018", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2018-19", "All", min_enable_speed=10. * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2020", car_parts=CarParts.common([CarHarness.hyundai_d])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.MIN_STEER_32_MPH, + ) + KIA_NIRO_PHEV_2022 = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), + HyundaiCarDocs("Kia Niro Plug-in Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.HYBRID | HyundaiFlags.MANDO_RADAR, + ) + KIA_NIRO_HEV_2021 = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Niro Hybrid 2021", car_parts=CarParts.common([CarHarness.hyundai_d])), + HyundaiCarDocs("Kia Niro Hybrid 2022", car_parts=CarParts.common([CarHarness.hyundai_f])), + ], + KIA_NIRO_EV.specs, + flags=HyundaiFlags.HYBRID, + ) + KIA_NIRO_HEV_2ND_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Kia Niro Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_a]))], + KIA_NIRO_EV.specs, + ) + KIA_OPTIMA_G4 = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Optima 2017", "Advanced Smart Cruise Control", + car_parts=CarParts.common([CarHarness.hyundai_b]))], # TODO: may support 2016, 2018 + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY | HyundaiFlags.TCU_GEARS | HyundaiFlags.MIN_STEER_32_MPH, + ) + KIA_OPTIMA_G4_FL = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Optima 2019-20", car_parts=CarParts.common([CarHarness.hyundai_g]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.UNSUPPORTED_LONGITUDINAL | HyundaiFlags.TCU_GEARS, + ) # TODO: may support adjacent years. may have a non-zero minimum steering speed - CAR.KIA_OPTIMA_H: HyundaiCarInfo("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.KIA_OPTIMA_H_G4_FL: HyundaiCarInfo("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a])), - CAR.KIA_SPORTAGE_5TH_GEN: [ - HyundaiCarInfo("Kia Sportage 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), - HyundaiCarInfo("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), - ], - CAR.KIA_SORENTO: [ - HyundaiCarInfo("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", - car_parts=CarParts.common([CarHarness.hyundai_e])), - HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])), - ], - CAR.KIA_SORENTO_4TH_GEN: HyundaiCarInfo("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.KIA_SORENTO_HEV_4TH_GEN: [ - HyundaiCarInfo("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - ], - CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", - car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.KIA_STINGER_2022: HyundaiCarInfo("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e])), - CAR.KIA_EV6: [ - HyundaiCarInfo("Kia EV6 (Southeast Asia only) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_p])), - HyundaiCarInfo("Kia EV6 (without HDA II) 2022-23", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Kia EV6 (with HDA II) 2022-23", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) - ], - CAR.KIA_CARNIVAL_4TH_GEN: [ - HyundaiCarInfo("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k])) - ], + KIA_OPTIMA_H = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Optima Hybrid 2017", "Advanced Smart Cruise Control", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY, + ) + KIA_OPTIMA_H_G4_FL = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Optima Hybrid 2019", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=3558 * CV.LB_TO_KG, wheelbase=2.8, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.HYBRID | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, + ) + KIA_SELTOS = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Seltos 2021", car_parts=CarParts.common([CarHarness.hyundai_a]))], + CarSpecs(mass=1337, wheelbase=2.63, steerRatio=14.56), + flags=HyundaiFlags.CHECKSUM_CRC8, + ) + KIA_SPORTAGE_5TH_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Kia Sportage 2023-24", car_parts=CarParts.common([CarHarness.hyundai_n])), + HyundaiCarDocs("Kia Sportage Hybrid 2023", car_parts=CarParts.common([CarHarness.hyundai_n])), + ], + # weight from SX and above trims, average of FWD and AWD version, steering ratio according to Kia News https://www.kiamedia.com/us/en/models/sportage/2023/specifications + CarSpecs(mass=1725, wheelbase=2.756, steerRatio=13.6), + ) + KIA_SORENTO = HyundaiPlatformConfig( + [ + HyundaiCarDocs("Kia Sorento 2018", "Advanced Smart Cruise Control & LKAS", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", + car_parts=CarParts.common([CarHarness.hyundai_e])), + HyundaiCarDocs("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", car_parts=CarParts.common([CarHarness.hyundai_e])), + ], + CarSpecs(mass=1985, wheelbase=2.78, steerRatio=14.4 * 1.1), # 10% higher at the center seems reasonable + flags=HyundaiFlags.CHECKSUM_6B | HyundaiFlags.UNSUPPORTED_LONGITUDINAL, + ) + KIA_SORENTO_4TH_GEN = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Kia Sorento 2021-23", car_parts=CarParts.common([CarHarness.hyundai_k]))], + CarSpecs(mass=3957 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms + flags=HyundaiFlags.RADAR_SCC, + ) + KIA_SORENTO_HEV_4TH_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Kia Sorento Hybrid 2021-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Kia Sorento Plug-in Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + ], + CarSpecs(mass=4395 * CV.LB_TO_KG, wheelbase=2.81, steerRatio=13.5), # average of the platforms + flags=HyundaiFlags.RADAR_SCC, + ) + KIA_STINGER = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Stinger 2018-20", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", + car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=1825, wheelbase=2.78, steerRatio=14.4 * 1.15) # 15% higher at the center seems reasonable + ) + KIA_STINGER_2022 = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Stinger 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_k]))], + KIA_STINGER.specs, + ) + KIA_CEED = HyundaiPlatformConfig( + [HyundaiCarDocs("Kia Ceed 2019", car_parts=CarParts.common([CarHarness.hyundai_e]))], + CarSpecs(mass=1450, wheelbase=2.65, steerRatio=13.75, tireStiffnessFactor=0.5), + flags=HyundaiFlags.LEGACY, + ) + KIA_EV6 = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Kia EV6 (Southeast Asia only) 2022-24", "All", car_parts=CarParts.common([CarHarness.hyundai_p])), + HyundaiCarDocs("Kia EV6 (without HDA II) 2022-24", "Highway Driving Assist", car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Kia EV6 (with HDA II) 2022-24", "Highway Driving Assist II", car_parts=CarParts.common([CarHarness.hyundai_p])) + ], + CarSpecs(mass=2055, wheelbase=2.9, steerRatio=16, tireStiffnessFactor=0.65), + flags=HyundaiFlags.EV, + ) + KIA_CARNIVAL_4TH_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Kia Carnival 2022-24", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Kia Carnival (China only) 2023", car_parts=CarParts.common([CarHarness.hyundai_k])) + ], + CarSpecs(mass=2087, wheelbase=3.09, steerRatio=14.23), + flags=HyundaiFlags.RADAR_SCC, + ) # Genesis - CAR.GENESIS_GV60_EV_1ST_GEN: [ - HyundaiCarInfo("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), - HyundaiCarInfo("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - ], - CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), - CAR.GENESIS_GV70_1ST_GEN: [ - HyundaiCarInfo("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), - HyundaiCarInfo("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), - ], - CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h])), - CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2017-18", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), - CAR.GENESIS_GV80: HyundaiCarInfo("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), -} + GENESIS_GV60_EV_1ST_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Genesis GV60 (Advanced Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_a])), + HyundaiCarDocs("Genesis GV60 (Performance Trim) 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], + CarSpecs(mass=2205, wheelbase=2.9, steerRatio=12.6), # steerRatio: https://www.motor1.com/reviews/586376/2023-genesis-gv60-first-drive/#:~:text=Relative%20to%20the%20related%20Ioniq,5%2FEV6%27s%2014.3%3A1. + flags=HyundaiFlags.EV, + ) + GENESIS_G70 = HyundaiPlatformConfig( + [HyundaiCarDocs("Genesis G70 2018", "All", car_parts=CarParts.common([CarHarness.hyundai_f]))], + CarSpecs(mass=1640, wheelbase=2.84, steerRatio=13.56), + flags=HyundaiFlags.LEGACY, + ) + GENESIS_G70_2020 = HyundaiPlatformConfig( + [ + # TODO: 2021 MY harness is unknown + HyundaiCarDocs("Genesis G70 2019-21", "All", car_parts=CarParts.common([CarHarness.hyundai_f])), + # TODO: From 3.3T Sport Advanced 2022 & Prestige 2023 Trim, 2.0T is unknown + HyundaiCarDocs("Genesis G70 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), + ], + GENESIS_G70.specs, + flags=HyundaiFlags.MANDO_RADAR, + ) + GENESIS_GV70_1ST_GEN = HyundaiCanFDPlatformConfig( + [ + HyundaiCarDocs("Genesis GV70 (2.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), + HyundaiCarDocs("Genesis GV70 (3.5T Trim) 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_m])), + ], + CarSpecs(mass=1950, wheelbase=2.87, steerRatio=14.6), + flags=HyundaiFlags.RADAR_SCC, + ) + GENESIS_G80 = HyundaiPlatformConfig( + [HyundaiCarDocs("Genesis G80 2018-19", "All", car_parts=CarParts.common([CarHarness.hyundai_h]))], + CarSpecs(mass=2060, wheelbase=3.01, steerRatio=16.5), + flags=HyundaiFlags.LEGACY, + ) + GENESIS_G90 = HyundaiPlatformConfig( + [HyundaiCarDocs("Genesis G90 2017-20", "All", car_parts=CarParts.common([CarHarness.hyundai_c]))], + CarSpecs(mass=2200, wheelbase=3.15, steerRatio=12.069), + ) + GENESIS_GV80 = HyundaiCanFDPlatformConfig( + [HyundaiCarDocs("Genesis GV80 2023", "All", car_parts=CarParts.common([CarHarness.hyundai_m]))], + CarSpecs(mass=2258, wheelbase=2.95, steerRatio=14.14), + flags=HyundaiFlags.RADAR_SCC, + ) + class Buttons: NONE = 0 @@ -316,7 +550,7 @@ class Buttons: CANCEL = 4 # on newer models, this is a pause/resume button -def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[bytes]]]: +def get_platform_codes(fw_versions: list[bytes]) -> set[tuple[bytes, bytes | None]]: # Returns unique, platform-specific identification codes for a set of versions codes = set() # (code-Optional[part], date) for fw in fw_versions: @@ -335,12 +569,12 @@ def get_platform_codes(fw_versions: List[bytes]) -> Set[Tuple[bytes, Optional[by return codes -def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]: +def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]: # Non-electric CAN FD platforms often do not have platform code specifiers needed # to distinguish between hybrid and ICE. All EVs so far are either exclusively # electric or specify electric in the platform code. fuzzy_platform_blacklist = {str(c) for c in (CANFD_CAR - EV_CAR - CANFD_FUZZY_WHITELIST)} - candidates: Set[str] = set() + candidates: set[str] = set() for candidate, fws in offline_fw_versions.items(): # Keep track of ECUs which pass all checks (platform codes, within date range) @@ -391,11 +625,6 @@ HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) HYUNDAI_VERSION_REQUEST_ALT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(0xf110) # Alt long description -HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ - p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \ - p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \ - p16(0xf100) - HYUNDAI_ECU_MANUFACTURING_DATE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.ECU_MANUFACTURING_DATE) @@ -408,7 +637,9 @@ DATE_FW_PATTERN = re.compile(b'(?<=[ -])([0-9]{6}$)') PART_NUMBER_FW_PATTERN = re.compile(b'(?<=[0-9][.,][0-9]{2} )([0-9]{5}[-/]?[A-Z][A-Z0-9]{3}[0-9])') # We've seen both ICE and hybrid for these platforms, and they have hybrid descriptors (e.g. MQ4 vs MQ4H) -CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN} +CANFD_FUZZY_WHITELIST = {CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KIA_K8_HEV_1ST_GEN, + # TODO: the hybrid variant is not out yet + CAR.KIA_CARNIVAL_4TH_GEN} # List of ECUs expected to have platform codes, camera and radar should exist on all cars # TODO: use abs, it has the platform code and part number on many platforms @@ -417,36 +648,27 @@ PLATFORM_CODE_ECUS = [Ecu.fwdRadar, Ecu.fwdCamera, Ecu.eps] # TODO: there are date codes in the ABS firmware versions in hex DATE_FW_ECUS = [Ecu.fwdCamera] -ALL_HYUNDAI_ECUS = [Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.engine, Ecu.parkingAdas, Ecu.transmission, Ecu.adas, Ecu.hvac, Ecu.cornerRadar] - +# Note: an ECU on CAN FD cars may sometimes send 0x30080aaaaaaaaaaa (flow control continue) while we +# are attempting to query ECUs. This currently does not seem to affect fingerprinting from the camera FW_QUERY_CONFIG = FwQueryConfig( requests=[ - # TODO: minimize shared whitelists for CAN and cornerRadar for CAN-FD + # TODO: add back whitelists # CAN queries (OBD-II port) Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera], - ), - Request( - [HYUNDAI_VERSION_REQUEST_MULTI], - [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.transmission, Ecu.eps, Ecu.abs, Ecu.fwdRadar], ), - # CAN-FD queries (from camera) - # TODO: combine shared whitelists with CAN requests + # CAN & CAN-FD queries (from camera) Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.cornerRadar, Ecu.hvac], bus=0, auxiliary=True, ), Request( [HYUNDAI_VERSION_REQUEST_LONG], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.hvac], bus=1, auxiliary=True, obd_multiplexing=False, @@ -457,44 +679,15 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [HYUNDAI_ECU_MANUFACTURING_DATE], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.fwdCamera], - bus=0, - auxiliary=True, - logging=True, - ), - - # CAN & CAN FD logging queries (from camera) - Request( - [HYUNDAI_VERSION_REQUEST_LONG], - [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=ALL_HYUNDAI_ECUS, - bus=0, - auxiliary=True, - logging=True, - ), - Request( - [HYUNDAI_VERSION_REQUEST_MULTI], - [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=ALL_HYUNDAI_ECUS, bus=0, auxiliary=True, logging=True, ), - Request( - [HYUNDAI_VERSION_REQUEST_LONG], - [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=ALL_HYUNDAI_ECUS, - bus=1, - auxiliary=True, - obd_multiplexing=False, - logging=True, - ), - # CAN-FD alt request logging queries + # CAN-FD alt request logging queries for hvac and parkingAdas Request( [HYUNDAI_VERSION_REQUEST_ALT], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac], bus=0, auxiliary=True, logging=True, @@ -502,135 +695,57 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [HYUNDAI_VERSION_REQUEST_ALT], [HYUNDAI_VERSION_RESPONSE], - whitelist_ecus=[Ecu.parkingAdas, Ecu.hvac], bus=1, auxiliary=True, logging=True, obd_multiplexing=False, ), ], + # We lose these ECUs without the comma power on these cars. + # Note that we still attempt to match with them when they are present + non_essential_ecus={ + Ecu.abs: [CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_ELANTRA_2021, + CAR.HYUNDAI_SANTA_FE, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.KIA_SORENTO, + CAR.KIA_CEED, CAR.KIA_SELTOS], + }, extra_ecus=[ - (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms - (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) - (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly + (Ecu.adas, 0x730, None), # ADAS Driving ECU on HDA2 platforms + (Ecu.parkingAdas, 0x7b1, None), # ADAS Parking ECU (may exist on all platforms) + (Ecu.hvac, 0x7b3, None), # HVAC Control Assembly (Ecu.cornerRadar, 0x7b7, None), + (Ecu.combinationMeter, 0x7c6, None), # CAN FD Instrument cluster ], # Custom fuzzy fingerprinting function using platform codes, part numbers + FW dates: match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) - CHECKSUM = { - "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, - CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, - CAR.KIA_K5_HEV_2020, CAR.CUSTIN_1ST_GEN], - "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS], + "crc8": CAR.with_flags(HyundaiFlags.CHECKSUM_CRC8), + "6B": CAR.with_flags(HyundaiFlags.CHECKSUM_6B), } CAN_GEARS = { # which message has the gear. hybrid and EV use ELECT_GEAR - "use_cluster_gears": {CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA}, - "use_tcu_gears": {CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL, CAR.SONATA_LF, CAR.VELOSTER, CAR.TUCSON}, + "use_cluster_gears": CAR.with_flags(HyundaiFlags.CLUSTER_GEARS), + "use_tcu_gears": CAR.with_flags(HyundaiFlags.TCU_GEARS), } -CANFD_CAR = {CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.TUCSON_4TH_GEN, CAR.SANTA_CRUZ_1ST_GEN, CAR.KIA_SPORTAGE_5TH_GEN, CAR.GENESIS_GV70_1ST_GEN, - CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.KIA_NIRO_HEV_2ND_GEN, CAR.KIA_NIRO_EV_2ND_GEN, - CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN, CAR.KONA_EV_2ND_GEN, CAR.KIA_K8_HEV_1ST_GEN, - CAR.STARIA_4TH_GEN} - -# The radar does SCC on these cars when HDA I, rather than the camera -CANFD_RADAR_SCC_CAR = {CAR.GENESIS_GV70_1ST_GEN, CAR.KIA_SORENTO_4TH_GEN, CAR.GENESIS_GV80, CAR.KIA_CARNIVAL_4TH_GEN, CAR.KIA_SORENTO_HEV_4TH_GEN} +CANFD_CAR = CAR.with_flags(HyundaiFlags.CANFD) +CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) # These CAN FD cars do not accept communication control to disable the ADAS ECU, # responds with 0x7F2822 - 'conditions not correct' -CANFD_UNSUPPORTED_LONGITUDINAL_CAR = {CAR.IONIQ_6, CAR.KONA_EV_2ND_GEN} +CANFD_UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) # The camera does SCC on these cars, rather than the radar -CAMERA_SCC_CAR = {CAR.KONA_EV_2022, } - -# these cars use a different gas signal -HYBRID_CAR = {CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_PHEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, - CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.SANTA_FE_PHEV_2022, CAR.IONIQ_PHEV_2019, CAR.KIA_K5_HEV_2020, - CAR.KIA_OPTIMA_H, CAR.KIA_OPTIMA_H_G4_FL, CAR.AZERA_HEV_6TH_GEN, CAR.KIA_NIRO_PHEV_2022} - -EV_CAR = {CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_EV_2ND_GEN, CAR.KONA_EV_2022, - CAR.KIA_EV6, CAR.IONIQ_5, CAR.IONIQ_6, CAR.GENESIS_GV60_EV_1ST_GEN, CAR.KONA_EV_2ND_GEN} - -# these cars require a special panda safety mode due to missing counters and checksums in the messages -LEGACY_SAFETY_MODE_CAR = {CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_LTD, CAR.KIA_OPTIMA_G4, - CAR.VELOSTER, CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022, - CAR.KIA_OPTIMA_H, CAR.ELANTRA_GT_I30} - -# these cars have not been verified to work with longitudinal yet - radar disable, sending correct messages, etc. -UNSUPPORTED_LONGITUDINAL_CAR = LEGACY_SAFETY_MODE_CAR | {CAR.KIA_NIRO_PHEV, CAR.KIA_SORENTO, CAR.SONATA_LF, CAR.KIA_OPTIMA_G4_FL, - CAR.KIA_OPTIMA_H_G4_FL} - -# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points. -# If no points are outputted by default it might be possible to turn it on using selfdrive/debug/hyundai_enable_radar_points.py -DBC = { - CAR.AZERA_6TH_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.AZERA_HEV_6TH_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_GT_I30: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None), - CAR.ELANTRA_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G70: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G70_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.GENESIS_G80: dbc_dict('hyundai_kia_generic', None), - CAR.GENESIS_G90: dbc_dict('hyundai_kia_generic', None), - CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_PHEV_2019: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.IONIQ: dbc_dict('hyundai_kia_generic', None), - CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_K5_HEV_2020: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_PHEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_G4: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_G4_FL: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_OPTIMA_H_G4_FL: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format - CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_STINGER_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KONA: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_EV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_HEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SANTA_FE_PHEV_2022: dbc_dict('hyundai_kia_generic', None), - CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format - CAR.TUCSON: dbc_dict('hyundai_kia_generic', None), - CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_EV6: dbc_dict('hyundai_canfd', None), - CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.TUCSON_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.IONIQ_5: dbc_dict('hyundai_canfd', None), - CAR.IONIQ_6: dbc_dict('hyundai_canfd', None), - CAR.SANTA_CRUZ_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SPORTAGE_5TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV70_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV60_EV_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SORENTO_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_NIRO_HEV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_NIRO_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.GENESIS_GV80: dbc_dict('hyundai_canfd', None), - CAR.KIA_CARNIVAL_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_SORENTO_HEV_4TH_GEN: dbc_dict('hyundai_canfd', None), - CAR.KONA_EV_2ND_GEN: dbc_dict('hyundai_canfd', None), - CAR.KIA_K8_HEV_1ST_GEN: dbc_dict('hyundai_canfd', None), - CAR.CUSTIN_1ST_GEN: dbc_dict('hyundai_kia_generic', None), - CAR.KIA_NIRO_PHEV_2022: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar_generated'), - CAR.STARIA_4TH_GEN: dbc_dict('hyundai_canfd', None), -} +CAMERA_SCC_CAR = CAR.with_flags(HyundaiFlags.CAMERA_SCC) + +HYBRID_CAR = CAR.with_flags(HyundaiFlags.HYBRID) + +EV_CAR = CAR.with_flags(HyundaiFlags.EV) + +LEGACY_SAFETY_MODE_CAR = CAR.with_flags(HyundaiFlags.LEGACY) + +UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.LEGACY) | CAR.with_flags(HyundaiFlags.UNSUPPORTED_LONGITUDINAL) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9767752edb..a3572238ba 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -4,7 +4,9 @@ import numpy as np import tomllib from abc import abstractmethod, ABC from enum import StrEnum -from typing import Any, Dict, Optional, Tuple, List, Callable, NamedTuple +from typing import Any, NamedTuple +from collections.abc import Callable +from functools import cache from cereal import car from openpilot.common.basedir import BASEDIR @@ -13,6 +15,7 @@ from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG +from openpilot.selfdrive.car.values import PLATFORMS from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel @@ -30,6 +33,18 @@ TORQUE_PARAMS_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/params.tom TORQUE_OVERRIDE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/override.toml') TORQUE_SUBSTITUTE_PATH = os.path.join(BASEDIR, 'selfdrive/car/torque_data/substitute.toml') +GEAR_SHIFTER_MAP: dict[str, car.CarState.GearShifter] = { + 'P': GearShifter.park, 'PARK': GearShifter.park, + 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, + 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, + 'E': GearShifter.eco, 'ECO': GearShifter.eco, + 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, + 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, + 'S': GearShifter.sport, 'SPORT': GearShifter.sport, + 'L': GearShifter.low, 'LOW': GearShifter.low, + 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, +} + class LatControlInputs(NamedTuple): lateral_acceleration: float @@ -41,29 +56,34 @@ class LatControlInputs(NamedTuple): TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, car.CarParams.LateralTorqueTuning, float, float, bool, bool], float] -def get_torque_params(candidate): +@cache +def get_torque_params(): with open(TORQUE_SUBSTITUTE_PATH, 'rb') as f: sub = tomllib.load(f) - if candidate in sub: - candidate = sub[candidate] - with open(TORQUE_PARAMS_PATH, 'rb') as f: params = tomllib.load(f) with open(TORQUE_OVERRIDE_PATH, 'rb') as f: override = tomllib.load(f) - # Ensure no overlap - if sum([candidate in x for x in [sub, params, override]]) > 1: - raise RuntimeError(f'{candidate} is defined twice in torque config') + torque_params = {} + for candidate in (sub.keys() | params.keys() | override.keys()) - {'legend'}: + if sum([candidate in x for x in [sub, params, override]]) > 1: + raise RuntimeError(f'{candidate} is defined twice in torque config') + + sub_candidate = sub.get(candidate, candidate) + + if sub_candidate in override: + out = override[sub_candidate] + elif sub_candidate in params: + out = params[sub_candidate] + else: + raise NotImplementedError(f"Did not find torque params for {sub_candidate}") - if candidate in override: - out = override[candidate] - elif candidate in params: - out = params[candidate] - else: - raise NotImplementedError(f"Did not find torque params for {candidate}") - return {key: out[i] for i, key in enumerate(params['legend'])} + torque_params[sub_candidate] = {key: out[i] for i, key in enumerate(params['legend'])} + if candidate in sub: + torque_params[candidate] = torque_params[sub_candidate] + return torque_params # generic car and radar interfaces @@ -79,21 +99,19 @@ class CarInterfaceBase(ABC): self.silent_steer_warning = True self.v_ego_cluster_seen = False - self.CS = None - self.can_parsers = [] - if CarState is not None: - self.CS = CarState(CP) + self.CS = CarState(CP) + self.cp = self.CS.get_can_parser(CP) + self.cp_cam = self.CS.get_cam_can_parser(CP) + self.cp_adas = self.CS.get_adas_can_parser(CP) + self.cp_body = self.CS.get_body_can_parser(CP) + self.cp_loopback = self.CS.get_loopback_can_parser(CP) + self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback] - self.cp = self.CS.get_can_parser(CP) - self.cp_cam = self.CS.get_cam_can_parser(CP) - self.cp_adas = self.CS.get_adas_can_parser(CP) - self.cp_body = self.CS.get_body_can_parser(CP) - self.cp_loopback = self.CS.get_loopback_can_parser(CP) - self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback] + dbc_name = "" if self.cp is None else self.cp.dbc_name + self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP, self.VM) + def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[tuple[int, int, bytes, int]]]: + return self.CC.update(c, self.CS, now_nanos) @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): @@ -107,8 +125,19 @@ class CarInterfaceBase(ABC): return cls.get_params(candidate, gen_empty_fingerprint(), list(), False, False) @classmethod - def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def get_params(cls, candidate: str, fingerprint: dict[int, dict[int, int]], car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): ret = CarInterfaceBase.get_std_params(candidate) + + platform = PLATFORMS[candidate] + ret.mass = platform.config.specs.mass + ret.wheelbase = platform.config.specs.wheelbase + ret.steerRatio = platform.config.specs.steerRatio + ret.centerToFront = ret.wheelbase * platform.config.specs.centerToFrontRatio + ret.minEnableSpeed = platform.config.specs.minEnableSpeed + ret.minSteerSpeed = platform.config.specs.minSteerSpeed + ret.tireStiffnessFactor = platform.config.specs.tireStiffnessFactor + ret.flags |= int(platform.config.flags) + ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs) # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload @@ -123,8 +152,8 @@ class CarInterfaceBase(ABC): @staticmethod @abstractmethod - def _get_params(ret: car.CarParams, candidate: str, fingerprint: Dict[int, Dict[int, int]], - car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool): + def _get_params(ret: car.CarParams, candidate, fingerprint: dict[int, dict[int, int]], + car_fw: list[car.CarParams.CarFw], experimental_long: bool, docs: bool): raise NotImplementedError @staticmethod @@ -155,7 +184,7 @@ class CarInterfaceBase(ABC): ret.carFingerprint = candidate # Car docs fields - ret.maxLateralAccel = get_torque_params(candidate)['MAX_LAT_ACCEL_MEASURED'] + ret.maxLateralAccel = get_torque_params()[candidate]['MAX_LAT_ACCEL_MEASURED'] ret.autoResumeSng = True # describes whether car can resume from a stop automatically # standard ALC params @@ -188,7 +217,7 @@ class CarInterfaceBase(ABC): @staticmethod def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_steering_angle=True): - params = get_torque_params(candidate) + params = get_torque_params()[candidate] tune.init('torque') tune.torque.useSteeringAngle = use_steering_angle @@ -204,7 +233,7 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: List[bytes]) -> car.CarState: + def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState: # parse can for cp in self.can_parsers: if cp is not None: @@ -231,15 +260,11 @@ class CarInterfaceBase(ABC): ret.cruiseState.speedCluster = ret.cruiseState.speed # copy back for next iteration - reader = ret.as_reader() if self.CS is not None: - self.CS.out = reader + self.CS.out = ret.as_reader() - return reader + return ret - @abstractmethod - def apply(self, c: car.CarControl, now_nanos: int) -> Tuple[car.CarControl.Actuators, List[bytes]]: - pass def create_common_events(self, cs_out, extra_gears=None, pcm_enable=True, allow_enable=True, enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)): @@ -274,6 +299,10 @@ class CarInterfaceBase(ABC): events.add(EventName.accFaulted) if cs_out.steeringPressed: events.add(EventName.steerOverride) + if cs_out.brakePressed and cs_out.standstill: + events.add(EventName.preEnableStandstill) + if cs_out.gasPressed: + events.add(EventName.gasPressedOverride) # Handle button presses for b in cs_out.buttonEvents: @@ -322,7 +351,6 @@ class RadarInterfaceBase(ABC): self.delay = 0 self.radar_ts = CP.radarTimeStep self.frame = 0 - self.no_radar_sleep = 'NO_RADAR_SLEEP' in os.environ def update(self, can_strings): self.frame += 1 @@ -409,22 +437,14 @@ class CarStateBase(ABC): return bool(left_blinker_stalk or self.left_blinker_cnt > 0), bool(right_blinker_stalk or self.right_blinker_cnt > 0) @staticmethod - def parse_gear_shifter(gear: Optional[str]) -> car.CarState.GearShifter: + def parse_gear_shifter(gear: str | None) -> car.CarState.GearShifter: if gear is None: return GearShifter.unknown + return GEAR_SHIFTER_MAP.get(gear.upper(), GearShifter.unknown) - d: Dict[str, car.CarState.GearShifter] = { - 'P': GearShifter.park, 'PARK': GearShifter.park, - 'R': GearShifter.reverse, 'REVERSE': GearShifter.reverse, - 'N': GearShifter.neutral, 'NEUTRAL': GearShifter.neutral, - 'E': GearShifter.eco, 'ECO': GearShifter.eco, - 'T': GearShifter.manumatic, 'MANUAL': GearShifter.manumatic, - 'D': GearShifter.drive, 'DRIVE': GearShifter.drive, - 'S': GearShifter.sport, 'SPORT': GearShifter.sport, - 'L': GearShifter.low, 'LOW': GearShifter.low, - 'B': GearShifter.brake, 'BRAKE': GearShifter.brake, - } - return d.get(gear.upper(), GearShifter.unknown) + @staticmethod + def get_can_parser(CP): + return None @staticmethod def get_cam_can_parser(CP): @@ -443,6 +463,18 @@ class CarStateBase(ABC): return None +SendCan = tuple[int, int, bytes, int] + + +class CarControllerBase(ABC): + def __init__(self, dbc_name: str, CP, VM): + pass + + @abstractmethod + def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]: + pass + + INTERFACE_ATTR_FILE = { "FINGERPRINTS": "fingerprints", "FW_VERSIONS": "fingerprints", @@ -450,7 +482,7 @@ INTERFACE_ATTR_FILE = { # interface-specific helpers -def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str | StrEnum, Any]: +def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> dict[str | StrEnum, Any]: # read all the folders in selfdrive/car and return a dict where: # - keys are all the car models or brand names # - values are attr values from all car folders @@ -483,7 +515,7 @@ class NanoFFModel: self.load_weights(platform) def load_weights(self, platform: str): - with open(self.weights_loc, 'r') as fob: + with open(self.weights_loc) as fob: self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()} def relu(self, x: np.ndarray): @@ -498,7 +530,7 @@ class NanoFFModel: x = np.dot(x, self.weights['w_4']) + self.weights['b_4'] return x - def predict(self, x: List[float], do_sample: bool = False): + def predict(self, x: list[float], do_sample: bool = False): x = self.forward(np.array(x)) if do_sample: pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature']) diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 678fe9ea46..447c7093c5 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -4,7 +4,7 @@ from functools import partial import cereal.messaging as messaging from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp +from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.selfdrive.car.fw_query_definitions import AddrType from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr @@ -12,7 +12,7 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a class IsoTpParallelQuery: def __init__(self, sendcan: messaging.PubSocket, logcan: messaging.SubSocket, bus: int, addrs: list[int] | list[AddrType], request: list[bytes], response: list[bytes], response_offset: int = 0x8, - functional_addrs: list[int] | None = None, debug: bool = False, response_pending_timeout: float = 10) -> None: + functional_addrs: list[int] = None, debug: bool = False, response_pending_timeout: float = 10) -> None: self.sendcan = sendcan self.logcan = logcan self.bus = bus diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 320ad19bb4..3d41634879 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,13 +1,14 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.mazda import mazdacan from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -35,13 +36,13 @@ class CarController: if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7): # Cancel Stock ACC if it's enabled while OP is disengaged # Send at a rate of 10hz until we sync with stock ACC state - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL)) + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.CANCEL)) else: self.brake_counter = 0 if CC.cruiseControl.resume and self.frame % 5 == 0: # Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds # Send Resume button when planner wants car to move - can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME)) + can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP, CS.crz_btns_counter, Buttons.RESUME)) self.apply_steer_last = apply_steer @@ -54,10 +55,10 @@ class CarController: can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required)) # send steering command - can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint, + can_sends.append(mazdacan.create_steering_control(self.packer, self.CP, self.frame, apply_steer, CS.cam_lkas)) - new_actuators = CC.actuators.copy() + new_actuators = CC.actuators.as_builder() new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX new_actuators.steerOutputCan = apply_steer diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index c0819592d4..83b238fb68 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, GEN1 +from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags class CarState(CarStateBase): def __init__(self, CP): @@ -18,9 +18,16 @@ class CarState(CarStateBase): self.lkas_allowed_speed = False self.lkas_disabled = False + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_cam): ret = car.CarState.new_message() + + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRZ_BTNS"]["DISTANCE_LESS"] + ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["FL"], cp.vl["WHEEL_SPEEDS"]["FR"], @@ -116,7 +123,7 @@ class CarState(CarStateBase): ("WHEEL_SPEEDS", 100), ] - if CP.carFingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: messages += [ ("ENGINE_DATA", 100), ("CRZ_CTRL", 50), @@ -136,7 +143,7 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] - if CP.carFingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: messages += [ # sig_address, frequency ("CAM_LANEINFO", 2), diff --git a/selfdrive/car/mazda/fingerprints.py b/selfdrive/car/mazda/fingerprints.py index 292f407935..f460fe9950 100644 --- a/selfdrive/car/mazda/fingerprints.py +++ b/selfdrive/car/mazda/fingerprints.py @@ -4,12 +4,14 @@ from openpilot.selfdrive.car.mazda.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.CX5_2022: { + CAR.MAZDA_CX5_2022: { (Ecu.eps, 0x730, None): [ b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'PEW5-188K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PW67-188K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PX2D-188K2-G\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PX2H-188K2-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -31,15 +33,17 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'PG69-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PW66-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXDL-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXFG-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXFG-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYB2-21PS1-J\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYJ3-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'SH51-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.CX5: { + CAR.MAZDA_CX5: { (Ecu.eps, 0x730, None): [ b'K319-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KCB8-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -108,7 +112,7 @@ FW_VERSIONS = { b'SH9T-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.CX9: { + CAR.MAZDA_CX9: { (Ecu.eps, 0x730, None): [ b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -158,7 +162,7 @@ FW_VERSIONS = { b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.MAZDA3: { + CAR.MAZDA_3: { (Ecu.eps, 0x730, None): [ b'BHN1-3210X-J-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K070-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -193,7 +197,7 @@ FW_VERSIONS = { b'PYKE-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.MAZDA6: { + CAR.MAZDA_6: { (Ecu.eps, 0x730, None): [ b'GBEF-3210X-B-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GBEF-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -225,7 +229,7 @@ FW_VERSIONS = { b'PYH7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, - CAR.CX9_2021: { + CAR.MAZDA_CX9_2021: { (Ecu.eps, 0x730, None): [ b'TC3M-3210X-A-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -250,6 +254,7 @@ FW_VERSIONS = { b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'GSH7-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'GSH7-67XK2-U\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.transmission, 0x7e1, None): [ b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 7ac93d9dee..6992d49ffe 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -2,7 +2,7 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type @@ -16,32 +16,14 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarUnavailable = True - ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) + ret.dashcamOnly = candidate not in (CAR.MAZDA_CX5_2022, CAR.MAZDA_CX9_2021) ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.8 - 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 - ret.wheelbase = 2.7 - ret.steerRatio = 15.5 - elif candidate in (CAR.CX9, CAR.CX9_2021): - 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 - ret.wheelbase = 2.7 - ret.steerRatio = 14.0 - elif candidate == CAR.MAZDA6: - ret.mass = 3443 * CV.LB_TO_KG - ret.wheelbase = 2.83 - ret.steerRatio = 15.5 - - if candidate not in (CAR.CX5_2022, ): + if candidate not in (CAR.MAZDA_CX5_2022, ): ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 @@ -52,6 +34,9 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + # TODO: add button types for inc and dec + ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + # events events = self.create_common_events(ret) @@ -63,6 +48,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index e350c5587f..74f6af04c5 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,7 +1,7 @@ -from openpilot.selfdrive.car.mazda.values import GEN1, Buttons +from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags -def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): +def create_steering_control(packer, CP, frame, apply_steer, lkas): tmp = apply_steer + 2048 @@ -45,7 +45,7 @@ def create_steering_control(packer, car_fingerprint, frame, apply_steer, lkas): csum = csum % 256 values = {} - if car_fingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: values = { "LKAS_REQUEST": apply_steer, "CTR": ctr, @@ -88,12 +88,12 @@ def create_alert_command(packer, cam_msg: dict, ldw: bool, steer_required: bool) return packer.make_can_msg("CAM_LANEINFO", 0, values) -def create_button_cmd(packer, car_fingerprint, counter, button): +def create_button_cmd(packer, CP, counter, button): can = int(button == Buttons.CANCEL) res = int(button == Buttons.RESUME) - if car_fingerprint in GEN1: + if CP.flags & MazdaFlags.GEN1: values = { "CAN_OFF": can, "CAN_OFF_INV": (can + 1) % 2, diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index b43ab3df66..a8c808d582 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -1,10 +1,10 @@ from dataclasses import dataclass, field -from enum import StrEnum -from typing import Dict, List, Union +from enum import IntFlag from cereal import car -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarHarness, CarInfo, CarParts +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -26,29 +26,54 @@ class CarControllerParams: pass -class CAR(StrEnum): - CX5 = "MAZDA CX-5" - CX9 = "MAZDA CX-9" - MAZDA3 = "MAZDA 3" - MAZDA6 = "MAZDA 6" - CX9_2021 = "MAZDA CX-9 2021" - CX5_2022 = "MAZDA CX-5 2022" - - @dataclass -class MazdaCarInfo(CarInfo): +class MazdaCarDocs(CarDocs): package: str = "All" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.mazda])) -CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = { - CAR.CX5: MazdaCarInfo("Mazda CX-5 2017-21"), - CAR.CX9: MazdaCarInfo("Mazda CX-9 2016-20"), - CAR.MAZDA3: MazdaCarInfo("Mazda 3 2017-18"), - CAR.MAZDA6: MazdaCarInfo("Mazda 6 2017-20"), - CAR.CX9_2021: MazdaCarInfo("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4"), - CAR.CX5_2022: MazdaCarInfo("Mazda CX-5 2022-24"), -} +@dataclass(frozen=True, kw_only=True) +class MazdaCarSpecs(CarSpecs): + tireStiffnessFactor: float = 0.7 # not optimized yet + + +class MazdaFlags(IntFlag): + # Static flags + # Gen 1 hardware: same CAN messages and same camera + GEN1 = 1 + + +@dataclass +class MazdaPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('mazda_2017', None)) + flags: int = MazdaFlags.GEN1 + + +class CAR(Platforms): + MAZDA_CX5 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-5 2017-21")], + MazdaCarSpecs(mass=3655 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.5) + ) + MAZDA_CX9 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-9 2016-20")], + MazdaCarSpecs(mass=4217 * CV.LB_TO_KG, wheelbase=3.1, steerRatio=17.6) + ) + MAZDA_3 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda 3 2017-18")], + MazdaCarSpecs(mass=2875 * CV.LB_TO_KG, wheelbase=2.7, steerRatio=14.0) + ) + MAZDA_6 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda 6 2017-20")], + MazdaCarSpecs(mass=3443 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=15.5) + ) + MAZDA_CX9_2021 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-9 2021-23", video_link="https://youtu.be/dA3duO4a0O4")], + MAZDA_CX9.specs + ) + MAZDA_CX5_2022 = MazdaPlatformConfig( + [MazdaCarDocs("Mazda CX-5 2022-24")], + MAZDA_CX5.specs, + ) class LKAS_LIMITS: @@ -76,15 +101,4 @@ FW_QUERY_CONFIG = FwQueryConfig( ], ) - -DBC = { - CAR.CX5: dbc_dict('mazda_2017', None), - CAR.CX9: dbc_dict('mazda_2017', None), - CAR.MAZDA3: dbc_dict('mazda_2017', None), - CAR.MAZDA6: dbc_dict('mazda_2017', None), - CAR.CX9_2021: dbc_dict('mazda_2017', None), - CAR.CX5_2022: dbc_dict('mazda_2017', None), -} - -# Gen 1 hardware: same CAN messages and same camera -GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/mock/carcontroller.py b/selfdrive/car/mock/carcontroller.py new file mode 100644 index 0000000000..0cd37c0369 --- /dev/null +++ b/selfdrive/car/mock/carcontroller.py @@ -0,0 +1,5 @@ +from openpilot.selfdrive.car.interfaces import CarControllerBase + +class CarController(CarControllerBase): + def update(self, CC, CS, now_nanos): + return CC.actuators.as_builder(), [] diff --git a/selfdrive/car/mock/carstate.py b/selfdrive/car/mock/carstate.py new file mode 100644 index 0000000000..ece908b51c --- /dev/null +++ b/selfdrive/car/mock/carstate.py @@ -0,0 +1,4 @@ +from openpilot.selfdrive.car.interfaces import CarStateBase + +class CarState(CarStateBase): + pass diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 2e4ac43033..7506bab053 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -18,6 +18,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.70 ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 13. + ret.dashcamOnly = True return ret def _update(self, c): @@ -29,7 +30,3 @@ class CarInterface(CarInterfaceBase): ret.vEgoRaw = self.sm[gps_sock].speed return ret - - def apply(self, c, now_nanos): - actuators = car.CarControl.Actuators.new_message() - return actuators, [] diff --git a/selfdrive/car/mock/values.py b/selfdrive/car/mock/values.py index c6c96579b4..f98aac2ee3 100644 --- a/selfdrive/car/mock/values.py +++ b/selfdrive/car/mock/values.py @@ -1,13 +1,9 @@ -from enum import StrEnum -from typing import Dict, List, Optional, Union +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms -from openpilot.selfdrive.car.docs_definitions import CarInfo - -class CAR(StrEnum): - MOCK = 'mock' - - -CAR_INFO: Dict[str, Optional[Union[CarInfo, List[CarInfo]]]] = { - CAR.MOCK: None, -} +class CAR(Platforms): + MOCK = PlatformConfig( + [], + CarSpecs(mass=1700, wheelbase=2.7, steerRatio=13), + {} + ) diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 2da518bbf1..c7bd231398 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -1,13 +1,14 @@ from cereal import car from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.nissan import nissancan from openpilot.selfdrive.car.nissan.values import CAR, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.car_fingerprint = CP.carFingerprint @@ -50,21 +51,21 @@ class CarController: self.apply_angle_last = apply_angle - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd: + if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA) and pcm_cancel_cmd: can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg)) # TODO: Find better way to cancel! # For some reason spamming the cancel button is unreliable on the Leaf # We now cancel by making propilot think the seatbelt is unlatched, # this generates a beep and a warning message every time you disengage - if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0: + if self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC) and self.frame % 2 == 0: can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd)) can_sends.append(nissancan.create_steering_control( self.packer, apply_angle, self.frame, CC.latActive, self.lkas_max_torque)) # Below are the HUD messages. We copy the stock message and modify - if self.CP.carFingerprint != CAR.ALTIMA: + if self.CP.carFingerprint != CAR.NISSAN_ALTIMA: if self.frame % 2 == 0: can_sends.append(nissancan.create_lkas_hud_msg(self.packer, CS.lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)) @@ -74,7 +75,7 @@ class CarController: self.packer, CS.lkas_hud_info_msg, steer_hud_alert )) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = apply_angle self.frame += 1 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index b2ba9ce290..57146b49c4 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -20,19 +20,25 @@ class CarState(CarStateBase): self.steeringTorqueSamples = deque(TORQUE_SAMPLES*[0], TORQUE_SAMPLES) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] + self.prev_distance_button = 0 + self.distance_button = 0 + def update(self, cp, cp_adas, cp_cam): ret = car.CarState.new_message() - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + self.prev_distance_button = self.distance_button + self.distance_button = cp.vl["CRUISE_THROTTLE"]["FOLLOW_DISTANCE_BUTTON"] + + if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] - elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): ret.gas = cp.vl["CRUISE_THROTTLE"]["GAS_PEDAL"] ret.gasPressed = bool(ret.gas > 3) - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): ret.brakePressed = bool(cp.vl["DOORS_LIGHTS"]["USER_BRAKE_PRESSED"]) - elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"]) ret.wheelSpeeds = self.get_wheel_speeds( @@ -46,38 +52,38 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] == 0.0 and cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] == 0.0 - if self.CP.carFingerprint == CAR.ALTIMA: + if self.CP.carFingerprint == CAR.NISSAN_ALTIMA: ret.cruiseState.enabled = bool(cp.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) else: ret.cruiseState.enabled = bool(cp_adas.vl["CRUISE_STATE"]["CRUISE_ENABLED"]) - if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + if self.CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL): ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 ret.cruiseState.available = bool(cp_cam.vl["PRO_PILOT"]["CRUISE_ON"]) - elif self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): - if self.CP.carFingerprint == CAR.LEAF: + elif self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): + if self.CP.carFingerprint == CAR.NISSAN_LEAF: ret.seatbeltUnlatched = cp.vl["SEATBELT"]["SEATBELT_DRIVER_LATCHED"] == 0 - elif self.CP.carFingerprint == CAR.LEAF_IC: + elif self.CP.carFingerprint == CAR.NISSAN_LEAF_IC: ret.seatbeltUnlatched = cp.vl["CANCEL_MSG"]["CANCEL_SEATBELT"] == 1 ret.cruiseState.available = bool(cp.vl["CRUISE_THROTTLE"]["CRUISE_AVAILABLE"]) - elif self.CP.carFingerprint == CAR.ALTIMA: + elif self.CP.carFingerprint == CAR.NISSAN_ALTIMA: ret.seatbeltUnlatched = cp.vl["HUD"]["SEATBELT_DRIVER_LATCHED"] == 0 ret.cruiseState.available = bool(cp_adas.vl["PRO_PILOT"]["CRUISE_ON"]) - if self.CP.carFingerprint == CAR.ALTIMA: + if self.CP.carFingerprint == CAR.NISSAN_ALTIMA: speed = cp.vl["PROPILOT_HUD"]["SET_SPEED"] else: speed = cp_adas.vl["PROPILOT_HUD"]["SET_SPEED"] if speed != 255: - if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + if self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): conversion = CV.MPH_TO_MS if cp.vl["HUD_SETTINGS"]["SPEED_MPH"] else CV.KPH_TO_MS else: conversion = CV.MPH_TO_MS if cp.vl["HUD"]["SPEED_MPH"] else CV.KPH_TO_MS ret.cruiseState.speed = speed * conversion ret.cruiseState.speedCluster = (speed - 1) * conversion # Speed on HUD is always 1 lower than actually sent on can bus - if self.CP.carFingerprint == CAR.ALTIMA: + if self.CP.carFingerprint == CAR.NISSAN_ALTIMA: ret.steeringTorque = cp_cam.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] else: ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] @@ -101,17 +107,17 @@ class CarState(CarStateBase): can_gear = int(cp.vl["GEARBOX"]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - if self.CP.carFingerprint == CAR.ALTIMA: + if self.CP.carFingerprint == CAR.NISSAN_ALTIMA: self.lkas_enabled = bool(cp.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) else: self.lkas_enabled = bool(cp_adas.vl["LKAS_SETTINGS"]["LKAS_ENABLED"]) self.cruise_throttle_msg = copy.copy(cp.vl["CRUISE_THROTTLE"]) - if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + if self.CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): self.cancel_msg = copy.copy(cp.vl["CANCEL_MSG"]) - if self.CP.carFingerprint != CAR.ALTIMA: + if self.CP.carFingerprint != CAR.NISSAN_ALTIMA: self.lkas_hud_msg = copy.copy(cp_adas.vl["PROPILOT_HUD"]) self.lkas_hud_info_msg = copy.copy(cp_adas.vl["PROPILOT_HUD_INFO_MSG"]) @@ -130,14 +136,14 @@ class CarState(CarStateBase): ("LIGHTS", 10), ] - if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): + if CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL, CAR.NISSAN_ALTIMA): messages += [ ("GAS_PEDAL", 100), ("CRUISE_THROTTLE", 50), ("HUD", 25), ] - elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): + elif CP.carFingerprint in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): messages += [ ("BRAKE_PEDAL", 100), ("CRUISE_THROTTLE", 50), @@ -146,7 +152,7 @@ class CarState(CarStateBase): ("SEATBELT", 10), ] - if CP.carFingerprint == CAR.ALTIMA: + if CP.carFingerprint == CAR.NISSAN_ALTIMA: messages += [ ("CRUISE_STATE", 10), ("LKAS_SETTINGS", 10), @@ -162,7 +168,7 @@ class CarState(CarStateBase): def get_adas_can_parser(CP): # this function generates lists for signal, messages and initial values - if CP.carFingerprint == CAR.ALTIMA: + if CP.carFingerprint == CAR.NISSAN_ALTIMA: messages = [ ("LKAS", 100), ("PRO_PILOT", 100), @@ -182,9 +188,9 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] - if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): + if CP.carFingerprint in (CAR.NISSAN_ROGUE, CAR.NISSAN_XTRAIL): messages.append(("PRO_PILOT", 100)) - elif CP.carFingerprint == CAR.ALTIMA: + elif CP.carFingerprint == CAR.NISSAN_ALTIMA: messages.append(("STEER_TORQUE_SENSOR", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) diff --git a/selfdrive/car/nissan/fingerprints.py b/selfdrive/car/nissan/fingerprints.py index 19267ded46..743feeace9 100644 --- a/selfdrive/car/nissan/fingerprints.py +++ b/selfdrive/car/nissan/fingerprints.py @@ -5,31 +5,31 @@ from openpilot.selfdrive.car.nissan.values import CAR Ecu = car.CarParams.Ecu FINGERPRINTS = { - CAR.XTRAIL: [{ + CAR.NISSAN_XTRAIL: [{ 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 2, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1821: 8, 1823: 8, 1837: 8, 2015: 8, 2016: 8, 2024: 8 }, { 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 527: 1, 548: 8, 637: 4, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 768: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1111: 4, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 8, 1497: 3, 1534: 6, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 2015: 8, 2016: 8, 2024: 8 }], - CAR.LEAF: [{ + CAR.NISSAN_LEAF: [{ 2: 5, 42: 6, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 944: 1, 976: 6, 1008: 7, 1011: 7, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8, 1856: 8, 1859: 8, 1861: 8, 1864: 8, 1874: 8, 1888: 8, 1891: 8, 1893: 8, 1906: 8, 1947: 8, 1949: 8, 1979: 8, 1981: 8, 2016: 8, 2017: 8, 2021: 8, 643: 5, 1792: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8 }, { 2: 5, 42: 8, 264: 3, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 724: 6, 758: 3, 761: 2, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 976: 6, 1008: 7, 1009: 8, 1010: 8, 1011: 7, 1012: 8, 1013: 8, 1019: 8, 1020: 8, 1021: 8, 1022: 8, 1057: 3, 1227: 8, 1228: 8, 1261: 5, 1342: 1, 1354: 8, 1361: 8, 1402: 8, 1459: 8, 1477: 8, 1497: 3, 1549: 8, 1573: 6, 1821: 8, 1837: 8 }], - CAR.LEAF_IC: [{ + CAR.NISSAN_LEAF_IC: [{ 2: 5, 42: 6, 264: 3, 282: 8, 361: 8, 372: 8, 384: 8, 389: 8, 403: 8, 459: 7, 460: 4, 470: 8, 520: 1, 569: 8, 581: 8, 634: 7, 640: 8, 643: 5, 644: 8, 645: 8, 646: 5, 658: 8, 682: 8, 683: 8, 689: 8, 756: 5, 758: 3, 761: 2, 783: 3, 830: 2, 852: 8, 853: 8, 856: 8, 861: 8, 943: 8, 944: 1, 1001: 6, 1057: 3, 1227: 8, 1228: 8, 1229: 8, 1342: 1, 1354: 8, 1361: 8, 1459: 8, 1477: 8, 1497: 3, 1514: 6, 1549: 8, 1573: 6, 1792: 8, 1821: 8, 1822: 8, 1837: 8, 1838: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8 }], - CAR.ROGUE: [{ + CAR.NISSAN_ROGUE: [{ 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 520: 2, 523: 6, 548: 8, 634: 7, 643: 5, 645: 8, 658: 8, 665: 8, 666: 8, 674: 2, 682: 8, 683: 8, 689: 8, 723: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 783: 3, 851: 8, 855: 8, 1041: 8, 1042: 8, 1055: 2, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1227: 8, 1228: 8, 1247: 4, 1266: 8, 1273: 7, 1342: 1, 1376: 6, 1401: 8, 1474: 2, 1497: 3, 1534: 7, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1839: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], - CAR.ALTIMA: [{ + CAR.NISSAN_ALTIMA: [{ 2: 5, 42: 6, 346: 6, 347: 5, 348: 8, 349: 7, 361: 8, 386: 8, 389: 8, 397: 8, 398: 8, 403: 8, 438: 8, 451: 8, 517: 8, 520: 2, 522: 8, 523: 6, 539: 8, 541: 7, 542: 8, 543: 8, 544: 8, 545: 8, 546: 8, 547: 8, 548: 8, 570: 8, 576: 8, 577: 8, 582: 8, 583: 8, 584: 8, 586: 8, 587: 8, 588: 8, 589: 8, 590: 8, 591: 8, 592: 8, 600: 8, 601: 8, 610: 8, 611: 8, 612: 8, 614: 8, 615: 8, 616: 8, 617: 8, 622: 8, 623: 8, 634: 7, 638: 8, 645: 8, 648: 5, 654: 6, 658: 8, 659: 8, 660: 8, 661: 8, 665: 8, 666: 8, 674: 2, 675: 8, 676: 8, 682: 8, 683: 8, 684: 8, 685: 8, 686: 8, 687: 8, 689: 8, 690: 8, 703: 8, 708: 7, 709: 7, 711: 7, 712: 7, 713: 7, 714: 8, 715: 8, 716: 8, 717: 7, 718: 7, 719: 7, 720: 7, 723: 8, 726: 7, 727: 7, 728: 7, 735: 8, 746: 8, 748: 6, 749: 6, 750: 8, 758: 3, 772: 8, 773: 6, 774: 7, 775: 8, 776: 6, 777: 7, 778: 6, 779: 7, 781: 7, 782: 7, 783: 3, 851: 8, 855: 5, 1001: 6, 1041: 8, 1042: 8, 1055: 3, 1100: 7, 1104: 4, 1105: 6, 1107: 4, 1108: 8, 1110: 7, 1111: 7, 1144: 7, 1145: 7, 1227: 8, 1228: 8, 1229: 8, 1232: 8, 1247: 4, 1258: 8, 1259: 8, 1266: 8, 1273: 7, 1306: 1, 1314: 8, 1323: 8, 1324: 8, 1342: 1, 1376: 8, 1401: 8, 1454: 8, 1497: 3, 1514: 6, 1526: 8, 1527: 5, 1792: 8, 1821: 8, 1823: 8, 1837: 8, 1872: 8, 1937: 8, 1953: 8, 1968: 8, 1988: 8, 2000: 8, 2001: 8, 2004: 8, 2005: 8, 2015: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 }], } FW_VERSIONS = { - CAR.ALTIMA: { + CAR.NISSAN_ALTIMA: { (Ecu.fwdCamera, 0x707, None): [ b'284N86CA1D', ], @@ -43,7 +43,7 @@ FW_VERSIONS = { b'284U29HE0A', ], }, - CAR.LEAF: { + CAR.NISSAN_LEAF: { (Ecu.abs, 0x740, None): [ b'476605SA1C', b'476605SA7D', @@ -72,7 +72,7 @@ FW_VERSIONS = { b'284U26WK0C', ], }, - CAR.LEAF_IC: { + CAR.NISSAN_LEAF_IC: { (Ecu.fwdCamera, 0x707, None): [ b'5SH1BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', b'5SH4BDB\x04\x18\x00\x00\x00\x00\x00_-?\x04\x91\xf2\x00\x00\x00\x80', @@ -94,7 +94,7 @@ FW_VERSIONS = { b'284U25SK2D', ], }, - CAR.XTRAIL: { + CAR.NISSAN_XTRAIL: { (Ecu.fwdCamera, 0x707, None): [ b'284N86FR2A', ], diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index aedcaa1887..2e9a990610 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -1,9 +1,11 @@ from cereal import car from panda import Panda -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.nissan.values import CAR +ButtonType = car.CarState.ButtonEvent.Type + class CarInterface(CarInterfaceBase): @@ -16,25 +18,13 @@ class CarInterface(CarInterfaceBase): ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 - ret.steerRatio = 17 ret.steerControlType = car.CarParams.SteerControlType.angle ret.radarUnavailable = True - if candidate in (CAR.ROGUE, CAR.XTRAIL): - ret.mass = 1610 - ret.wheelbase = 2.705 - ret.centerToFront = ret.wheelbase * 0.44 - elif candidate in (CAR.LEAF, CAR.LEAF_IC): - ret.mass = 1610 - ret.wheelbase = 2.705 - ret.centerToFront = ret.wheelbase * 0.44 - elif candidate == CAR.ALTIMA: + if candidate == CAR.NISSAN_ALTIMA: # Altima has EPS on C-CAN unlike the others that have it on V-CAN ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS - ret.mass = 1492 - ret.wheelbase = 2.824 - ret.centerToFront = ret.wheelbase * 0.44 return ret @@ -42,10 +32,7 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_adas, self.cp_cam) - buttonEvents = [] - be = car.CarState.ButtonEvent.new_message() - be.type = car.CarState.ButtonEvent.Type.accelCruise - buttonEvents.append(be) + ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.brake]) @@ -55,6 +42,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/nissan/nissancan.py b/selfdrive/car/nissan/nissancan.py index 49dcd6fe93..b9a1b4f843 100644 --- a/selfdrive/car/nissan/nissancan.py +++ b/selfdrive/car/nissan/nissancan.py @@ -39,7 +39,7 @@ def create_acc_cancel_cmd(packer, car_fingerprint, cruise_throttle_msg): "unsure2", "unsure3", ]} - can_bus = 1 if car_fingerprint == CAR.ALTIMA else 2 + can_bus = 1 if car_fingerprint == CAR.NISSAN_ALTIMA else 2 values["CANCEL_BUTTON"] = 1 values["NO_BUTTON_PRESSED"] = 0 diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index d064ce894d..eecffb21bc 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -1,11 +1,9 @@ from dataclasses import dataclass, field -from enum import StrEnum -from typing import Dict, List, Optional, Union from cereal import car from panda.python import uds -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo, CarHarness, CarParts +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs, CarHarness, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -21,29 +19,47 @@ class CarControllerParams: pass -class CAR(StrEnum): - XTRAIL = "NISSAN X-TRAIL 2017" - LEAF = "NISSAN LEAF 2018" - # Leaf with ADAS ECU found behind instrument cluster instead of glovebox - # Currently the only known difference between them is the inverted seatbelt signal. - LEAF_IC = "NISSAN LEAF 2018 Instrument Cluster" - ROGUE = "NISSAN ROGUE 2019" - ALTIMA = "NISSAN ALTIMA 2020" - - @dataclass -class NissanCarInfo(CarInfo): +class NissanCarDocs(CarDocs): package: str = "ProPILOT Assist" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.nissan_a])) -CAR_INFO: Dict[str, Optional[Union[NissanCarInfo, List[NissanCarInfo]]]] = { - CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"), - CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY"), - CAR.LEAF_IC: None, # same platforms - CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"), - CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b])), -} +@dataclass(frozen=True) +class NissanCarSpecs(CarSpecs): + centerToFrontRatio: float = 0.44 + steerRatio: float = 17. + + +@dataclass +class NissanPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('nissan_x_trail_2017_generated', None)) + + +class CAR(Platforms): + NISSAN_XTRAIL = NissanPlatformConfig( + [NissanCarDocs("Nissan X-Trail 2017")], + NissanCarSpecs(mass=1610, wheelbase=2.705) + ) + NISSAN_LEAF = NissanPlatformConfig( + [NissanCarDocs("Nissan Leaf 2018-23", video_link="https://youtu.be/vaMbtAh_0cY")], + NissanCarSpecs(mass=1610, wheelbase=2.705), + dbc_dict('nissan_leaf_2018_generated', None), + ) + # Leaf with ADAS ECU found behind instrument cluster instead of glovebox + # Currently the only known difference between them is the inverted seatbelt signal. + NISSAN_LEAF_IC = NISSAN_LEAF.override(car_docs=[]) + NISSAN_ROGUE = NissanPlatformConfig( + [NissanCarDocs("Nissan Rogue 2018-20")], + NissanCarSpecs(mass=1610, wheelbase=2.705) + ) + NISSAN_ALTIMA = NissanPlatformConfig( + [NissanCarDocs("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b]))], + NissanCarSpecs(mass=1492, wheelbase=2.824) + ) + + +DBC = CAR.create_dbc_map() # Default diagnostic session NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81]) @@ -89,11 +105,3 @@ FW_QUERY_CONFIG = FwQueryConfig( ), ]], ) - -DBC = { - CAR.XTRAIL: dbc_dict('nissan_x_trail_2017_generated', None), - CAR.LEAF: dbc_dict('nissan_leaf_2018_generated', None), - CAR.LEAF_IC: dbc_dict('nissan_leaf_2018_generated', None), - CAR.ROGUE: dbc_dict('nissan_x_trail_2017_generated', None), - CAR.ALTIMA: dbc_dict('nissan_x_trail_2017_generated', None), -} diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index cc8ce4f722..d89ae8c639 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,9 +1,9 @@ from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.subaru import subarucan -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, STEER_RATE_LIMITED, \ - CanBus, CarControllerParams, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags # FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and # involves the total steering angle change rather than rate, but these limits work well for now @@ -11,7 +11,7 @@ MAX_STEER_RATE = 25 # deg/s MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.apply_steer_last = 0 @@ -42,12 +42,12 @@ class CarController: if not CC.latActive: apply_steer = 0 - if self.CP.carFingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, self.frame // self.p.STEER_STEP, apply_steer, CC.latActive)) else: apply_steer_req = CC.latActive - if self.CP.carFingerprint in STEER_RATE_LIMITED: + if self.CP.flags & SubaruFlags.STEER_RATE_LIMITED: # Steering rate fault prevention self.steer_rate_counter, apply_steer_req = \ common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req, @@ -74,7 +74,7 @@ class CarController: cruise_brake = CarControllerParams.BRAKE_MIN # *** alerts and pcm cancel *** - if self.CP.carFingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: if self.frame % 5 == 0: # 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep # disengage ACC when OP is disengaged @@ -117,8 +117,8 @@ class CarController: self.CP.openpilotLongitudinalControl, cruise_brake > 0, cruise_throttle)) else: if pcm_cancel_cmd: - if self.CP.carFingerprint not in HYBRID_CARS: - bus = CanBus.alt if self.CP.carFingerprint in GLOBAL_GEN2 else CanBus.main + if not (self.CP.flags & SubaruFlags.HYBRID): + bus = CanBus.alt if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else CanBus.main can_sends.append(subarucan.create_es_distance(self.packer, CS.es_distance_msg["COUNTER"] + 1, CS.es_distance_msg, bus, pcm_cancel_cmd)) if self.CP.flags & SubaruFlags.DISABLE_EYESIGHT: @@ -136,7 +136,7 @@ class CarController: if self.frame % 2 == 0: can_sends.append(subarucan.create_es_static_2(self.packer)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index c8a6dfe1e6..821ff2c151 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.can_define import CANDefine from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, CanBus, SubaruFlags +from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags from openpilot.selfdrive.car import CanSignalRateCalculator @@ -19,17 +19,27 @@ class CarState(CarStateBase): def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() - throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] + throttle_msg = cp.vl["Throttle"] if not (self.CP.flags & SubaruFlags.HYBRID) else cp_body.vl["Throttle_Hybrid"] ret.gas = throttle_msg["Throttle_Pedal"] / 255. ret.gasPressed = ret.gas > 1e-5 - if self.car_fingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 0 else: - cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + cp_brakes = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 - cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + cp_es_distance = cp_body if self.CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID) else cp_cam + if not (self.CP.flags & SubaruFlags.HYBRID): + eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"]) + + # if openpilot is controlling long, an eyesight fault is a non-critical fault. otherwise it's an ACC fault + if self.CP.openpilotLongitudinalControl: + ret.carFaultedNonCritical = eyesight_fault + else: + ret.accFaulted = eyesight_fault + + cp_wheels = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp ret.wheelSpeeds = self.get_wheel_speeds( cp_wheels.vl["Wheel_Speeds"]["FL"], cp_wheels.vl["Wheel_Speeds"]["FR"], @@ -48,24 +58,24 @@ class CarState(CarStateBase): ret.leftBlindspot = (cp.vl["BSD_RCTA"]["L_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSD_RCTA"]["R_ADJACENT"] == 1) or (cp.vl["BSD_RCTA"]["R_APPROACHING"] == 1) - cp_transmission = cp_body if self.car_fingerprint in HYBRID_CARS else cp + cp_transmission = cp_body if self.CP.flags & SubaruFlags.HYBRID else cp can_gear = int(cp_transmission.vl["Transmission"]["Gear"]) 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: + if not (self.CP.flags & SubaruFlags.PREGLOBAL): # 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"] - steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 + steer_threshold = 75 if self.CP.flags & SubaruFlags.PREGLOBAL else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold - cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp - if self.car_fingerprint in HYBRID_CARS: + cp_cruise = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp + if self.CP.flags & SubaruFlags.HYBRID: ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: @@ -73,8 +83,8 @@ class CarState(CarStateBase): ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS - if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ - (self.car_fingerprint not in PREGLOBAL_CARS and cp.vl["Dashlights"]["UNITS"] == 1): + if (self.CP.flags & SubaruFlags.PREGLOBAL and cp.vl["Dash_State2"]["UNITS"] == 1) or \ + (not (self.CP.flags & SubaruFlags.PREGLOBAL) and cp.vl["Dashlights"]["UNITS"] == 1): ret.cruiseState.speed *= CV.MPH_TO_KPH ret.seatbeltUnlatched = cp.vl["Dashlights"]["SEATBELT_FL"] == 1 @@ -84,8 +94,7 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 - cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam - if self.car_fingerprint in PREGLOBAL_CARS: + if self.CP.flags & SubaruFlags.PREGLOBAL: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] else: @@ -96,12 +105,12 @@ class CarState(CarStateBase): (cp_cam.vl["ES_LKAS_State"]["LKAS_Alert"] == 2) 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 + cp_es_brake = cp_body if self.CP.flags & SubaruFlags.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 + cp_es_status = cp_body if self.CP.flags & SubaruFlags.GLOBAL_GEN2 else cp_cam # TODO: Hybrid cars don't have ES_Distance, need a replacement - if self.car_fingerprint not in HYBRID_CARS: + if not (self.CP.flags & SubaruFlags.HYBRID): # 8 is known AEB, there are a few other values related to AEB we ignore ret.stockAeb = (cp_es_distance.vl["ES_Brake"]["AEB_Status"] == 8) and \ (cp_es_distance.vl["ES_Brake"]["Brake_Pressure"] != 0) @@ -109,7 +118,7 @@ class CarState(CarStateBase): self.es_status_msg = copy.copy(cp_es_status.vl["ES_Status"]) self.cruise_control_msg = copy.copy(cp_cruise.vl["CruiseControl"]) - if self.car_fingerprint not in HYBRID_CARS: + if not (self.CP.flags & SubaruFlags.HYBRID): self.es_distance_msg = copy.copy(cp_es_distance.vl["ES_Distance"]) self.es_dashstatus_msg = copy.copy(cp_cam.vl["ES_DashStatus"]) @@ -125,7 +134,7 @@ class CarState(CarStateBase): ("Brake_Status", 50), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages.append(("CruiseControl", 20)) return messages @@ -136,7 +145,7 @@ class CarState(CarStateBase): ("ES_Brake", 20), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages += [ ("ES_Distance", 20), ("ES_Status", 20) @@ -164,7 +173,7 @@ class CarState(CarStateBase): ("Brake_Pedal", 50), ] - if CP.carFingerprint not in HYBRID_CARS: + if not (CP.flags & SubaruFlags.HYBRID): messages += [ ("Throttle", 100), ("Transmission", 100) @@ -173,8 +182,8 @@ class CarState(CarStateBase): if CP.enableBsm: messages.append(("BSD_RCTA", 17)) - if CP.carFingerprint not in PREGLOBAL_CARS: - if CP.carFingerprint not in GLOBAL_GEN2: + if not (CP.flags & SubaruFlags.PREGLOBAL): + if not (CP.flags & SubaruFlags.GLOBAL_GEN2): messages += CarState.get_common_global_body_messages(CP) else: messages += CarState.get_common_preglobal_body_messages() @@ -183,7 +192,7 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): - if CP.carFingerprint in PREGLOBAL_CARS: + if CP.flags & SubaruFlags.PREGLOBAL: messages = [ ("ES_DashStatus", 20), ("ES_Distance", 20), @@ -194,7 +203,7 @@ class CarState(CarStateBase): ("ES_LKAS_State", 10), ] - if CP.carFingerprint not in GLOBAL_GEN2: + if not (CP.flags & SubaruFlags.GLOBAL_GEN2): messages += CarState.get_common_global_es_messages(CP) if CP.flags & SubaruFlags.SEND_INFOTAINMENT: @@ -206,11 +215,11 @@ class CarState(CarStateBase): def get_body_can_parser(CP): messages = [] - if CP.carFingerprint in GLOBAL_GEN2: + if CP.flags & SubaruFlags.GLOBAL_GEN2: messages += CarState.get_common_global_body_messages(CP) messages += CarState.get_common_global_es_messages(CP) - if CP.carFingerprint in HYBRID_CARS: + if CP.flags & SubaruFlags.HYBRID: messages += [ ("Throttle_Hybrid", 40), ("Transmission", 100) diff --git a/selfdrive/car/subaru/fingerprints.py b/selfdrive/car/subaru/fingerprints.py index 90fa6093d9..7f3ae73163 100644 --- a/selfdrive/car/subaru/fingerprints.py +++ b/selfdrive/car/subaru/fingerprints.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car.subaru.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.ASCENT: { + CAR.SUBARU_ASCENT: { (Ecu.abs, 0x7b0, None): [ b'\xa5 \x19\x02\x00', b'\xa5 !\x02\x00', @@ -26,13 +26,14 @@ FW_VERSIONS = { b'\xd1,\xa0q\x07', ], (Ecu.transmission, 0x7e1, None): [ + b'\x00>\xf0\x00\x00', b'\x00\xfe\xf7\x00\x00', b'\x01\xfe\xf7\x00\x00', b'\x01\xfe\xf9\x00\x00', b'\x01\xfe\xfa\x00\x00', ], }, - CAR.ASCENT_2023: { + CAR.SUBARU_ASCENT_2023: { (Ecu.abs, 0x7b0, None): [ b'\xa5 #\x03\x00', ], @@ -49,7 +50,7 @@ FW_VERSIONS = { b'\x04\xfe\xf3\x00\x00', ], }, - CAR.LEGACY: { + CAR.SUBARU_LEGACY: { (Ecu.abs, 0x7b0, None): [ b'\xa1 \x02\x01', b'\xa1 \x02\x02', @@ -77,7 +78,7 @@ FW_VERSIONS = { b'\xa7\xfe\xc4@\x00', ], }, - CAR.IMPREZA: { + CAR.SUBARU_IMPREZA: { (Ecu.abs, 0x7b0, None): [ b'z\x84\x19\x90\x00', b'z\x94\x08\x90\x00', @@ -148,6 +149,7 @@ FW_VERSIONS = { b'\xe3\xf5C\x00\x00', b'\xe3\xf5F\x00\x00', b'\xe3\xf5G\x00\x00', + b'\xe4\xe5\x021\x00', b'\xe4\xe5\x061\x00', b'\xe4\xf5\x02\x00\x00', b'\xe4\xf5\x07\x00\x00', @@ -156,12 +158,13 @@ FW_VERSIONS = { b'\xe5\xf5B\x00\x00', ], }, - CAR.IMPREZA_2020: { + CAR.SUBARU_IMPREZA_2020: { (Ecu.abs, 0x7b0, None): [ b'\xa2 \x193\x00', b'\xa2 \x194\x00', b'\xa2 `\x00', b'\xa2 !3\x00', + b'\xa2 !6\x00', b'\xa2 !`\x00', b'\xa2 !i\x00', ], @@ -170,6 +173,7 @@ FW_VERSIONS = { b'\n\xc0\x04\x01', b'\x9a\xc0\x00\x00', b'\x9a\xc0\x04\x00', + b'\x9a\xc0\n\x01', ], (Ecu.fwdCamera, 0x787, None): [ b'\x00\x00eb\x1f@ "', @@ -179,6 +183,7 @@ FW_VERSIONS = { b'\x00\x00e\x8f\x1f@ )', b'\x00\x00e\x92\x00\x00\x00\x00', b'\x00\x00e\xa4\x00\x00\x00\x00', + b'\x00\x00e\xa4\x1f@ (', ], (Ecu.engine, 0x7e0, None): [ b'\xca!`0\x07', @@ -186,6 +191,7 @@ FW_VERSIONS = { b'\xca!ap\x07', b'\xca!f@\x07', b'\xca!fp\x07', + b'\xcaacp\x07', b'\xcc!`p\x07', b'\xcc!fp\x07', b'\xcc"f0\x07', @@ -195,8 +201,10 @@ FW_VERSIONS = { b'\xe6"fp\x07', b'\xf3"f@\x07', b'\xf3"fp\x07', + b'\xf3"fr\x07', ], (Ecu.transmission, 0x7e1, None): [ + b'\xe6\x15\x042\x00', b'\xe6\xf5\x04\x00\x00', b'\xe6\xf5$\x00\x00', b'\xe6\xf5D0\x00', @@ -209,7 +217,7 @@ FW_VERSIONS = { b'\xe9\xf6F0\x00', ], }, - CAR.CROSSTREK_HYBRID: { + CAR.SUBARU_CROSSTREK_HYBRID: { (Ecu.abs, 0x7b0, None): [ b'\xa2 \x19e\x01', b'\xa2 !e\x01', @@ -227,12 +235,13 @@ FW_VERSIONS = { b'\xf4!`0\x07', ], }, - CAR.FORESTER: { + CAR.SUBARU_FORESTER: { (Ecu.abs, 0x7b0, None): [ b'\xa3 \x18\x14\x00', b'\xa3 \x18&\x00', b'\xa3 \x19\x14\x00', b'\xa3 \x19&\x00', + b'\xa3 \x19h\x00', b'\xa3 \x14\x00', b'\xa3 \x14\x01', ], @@ -245,6 +254,7 @@ FW_VERSIONS = { b'\x00\x00e!\x1f@ \x11', b'\x00\x00e^\x00\x00\x00\x00', b'\x00\x00e^\x1f@ !', + b'\x00\x00e`\x00\x00\x00\x00', b'\x00\x00e`\x1f@ ', b'\x00\x00e\x97\x00\x00\x00\x00', b'\x00\x00e\x97\x1f@ 0', @@ -265,9 +275,10 @@ FW_VERSIONS = { b'\x1a\xf6F`\x00', b'\x1a\xf6b0\x00', b'\x1a\xf6b`\x00', + b'\x1a\xf6f`\x00', ], }, - CAR.FORESTER_HYBRID: { + CAR.SUBARU_FORESTER_HYBRID: { (Ecu.abs, 0x7b0, None): [ b'\xa3 \x19T\x00', ], @@ -284,7 +295,7 @@ FW_VERSIONS = { b'\x1b\xa7@a\x00', ], }, - CAR.FORESTER_PREGLOBAL: { + CAR.SUBARU_FORESTER_PREGLOBAL: { (Ecu.abs, 0x7b0, None): [ b'm\x97\x14@', b'}\x97\x14@', @@ -300,6 +311,7 @@ FW_VERSIONS = { b'\x00\x00d\xd3\x1f@ \t', ], (Ecu.engine, 0x7e0, None): [ + b'\xa7"@0\x07', b'\xa7"@p\x07', b'\xa7)\xa0q\x07', b'\xba"@@\x07', @@ -307,6 +319,7 @@ FW_VERSIONS = { ], (Ecu.transmission, 0x7e1, None): [ b'\x1a\xf6F`\x00', + b'\xda\xf2`p\x00', b'\xda\xf2`\x80\x00', b'\xda\xfd\xe0\x80\x00', b'\xdc\xf2@`\x00', @@ -315,7 +328,7 @@ FW_VERSIONS = { b'\xdc\xf2`\x81\x00', ], }, - CAR.LEGACY_PREGLOBAL: { + CAR.SUBARU_LEGACY_PREGLOBAL: { (Ecu.abs, 0x7b0, None): [ b'[\x97D\x00', b'[\xba\xc4\x03', @@ -348,7 +361,7 @@ FW_VERSIONS = { b'\xbf\xfb\xc0\x80\x00', ], }, - CAR.OUTBACK_PREGLOBAL: { + CAR.SUBARU_OUTBACK_PREGLOBAL: { (Ecu.abs, 0x7b0, None): [ b'[\xba\xac\x03', b'[\xf7\xac\x00', @@ -401,7 +414,7 @@ FW_VERSIONS = { b'\xbf\xfb\xe0b\x00', ], }, - CAR.OUTBACK_PREGLOBAL_2018: { + CAR.SUBARU_OUTBACK_PREGLOBAL_2018: { (Ecu.abs, 0x7b0, None): [ b'\x8b\x97\xac\x00', b'\x8b\x97\xbc\x00', @@ -444,7 +457,7 @@ FW_VERSIONS = { b'\xbc\xfb\xe0\x80\x00', ], }, - CAR.OUTBACK: { + CAR.SUBARU_OUTBACK: { (Ecu.abs, 0x7b0, None): [ b'\xa1 \x06\x00', b'\xa1 \x06\x01', @@ -493,7 +506,7 @@ FW_VERSIONS = { b'\xa7\xfe\xf4@\x00', ], }, - CAR.FORESTER_2022: { + CAR.SUBARU_FORESTER_2022: { (Ecu.abs, 0x7b0, None): [ b'\xa3 !v\x00', b'\xa3 !x\x00', @@ -526,7 +539,7 @@ FW_VERSIONS = { b'\x1e\xf6D0\x00', ], }, - CAR.OUTBACK_2023: { + CAR.SUBARU_OUTBACK_2023: { (Ecu.abs, 0x7b0, None): [ b'\xa1 #\x14\x00', b'\xa1 #\x17\x00', diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 1296aead5e..1aa4bd95ea 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,125 +3,91 @@ from panda import Panda from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, LKAS_ANGLE, GLOBAL_GEN2, PREGLOBAL_CARS, HYBRID_CARS, SubaruFlags +from openpilot.selfdrive.car.subaru.values import CAR, GLOBAL_ES_ADDR, SubaruFlags class CarInterface(CarInterfaceBase): @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): ret.carName = "subaru" ret.radarUnavailable = True # for HYBRID CARS to be upstreamed, we need: # - replacement for ES_Distance so we can cancel the cruise control # - to find the Cruise_Activated bit from the car # - proper panda safety setup (use the correct cruise_activated bit, throttle from Throttle_Hybrid, etc) - ret.dashcamOnly = candidate in (PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) + ret.dashcamOnly = bool(ret.flags & (SubaruFlags.PREGLOBAL | SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) ret.autoResumeSng = False # Detect infotainment message sent from the camera - if candidate not in PREGLOBAL_CARS and 0x323 in fingerprint[2]: + if not (ret.flags & SubaruFlags.PREGLOBAL) and 0x323 in fingerprint[2]: ret.flags |= SubaruFlags.SEND_INFOTAINMENT.value - if candidate in PREGLOBAL_CARS: + if ret.flags & SubaruFlags.PREGLOBAL: ret.enableBsm = 0x25c in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruPreglobal)] else: ret.enableBsm = 0x228 in fingerprint[0] ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] - if candidate in GLOBAL_GEN2: + if ret.flags & SubaruFlags.GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 - if candidate in LKAS_ANGLE: + if ret.flags & SubaruFlags.LKAS_ANGLE: ret.steerControlType = car.CarParams.SteerControlType.angle else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - if candidate in (CAR.ASCENT, CAR.ASCENT_2023): - ret.mass = 2031. - ret.wheelbase = 2.89 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 13.5 - ret.steerActuatorDelay = 0.3 # end-to-end angle controller + if candidate in (CAR.SUBARU_ASCENT, CAR.SUBARU_ASCENT_2023): + ret.steerActuatorDelay = 0.3 # end-to-end angle controller ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.00003 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]] - elif candidate == CAR.IMPREZA: - ret.mass = 1568. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 15 - ret.steerActuatorDelay = 0.4 # end-to-end angle controller + elif candidate == CAR.SUBARU_IMPREZA: + ret.steerActuatorDelay = 0.4 # end-to-end angle controller ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] - elif candidate == CAR.IMPREZA_2020: - ret.mass = 1480. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 # learned, 14 stock + elif candidate == CAR.SUBARU_IMPREZA_2020: ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] - elif candidate == CAR.CROSSTREK_HYBRID: - ret.mass = 1668. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 + elif candidate == CAR.SUBARU_CROSSTREK_HYBRID: ret.steerActuatorDelay = 0.1 - elif candidate in (CAR.FORESTER, CAR.FORESTER_2022, CAR.FORESTER_HYBRID): - ret.mass = 1568. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 # learned, 14 stock + elif candidate in (CAR.SUBARU_FORESTER, CAR.SUBARU_FORESTER_2022, CAR.SUBARU_FORESTER_HYBRID): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kf = 0.000038 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] 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. - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 17 + elif candidate in (CAR.SUBARU_OUTBACK, CAR.SUBARU_LEGACY, CAR.SUBARU_OUTBACK_2023): ret.steerActuatorDelay = 0.1 - elif candidate in (CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018): + elif candidate in (CAR.SUBARU_FORESTER_PREGLOBAL, CAR.SUBARU_OUTBACK_PREGLOBAL_2018): ret.safetyConfigs[0].safetyParam = Panda.FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE # Outback 2018-2019 and Forester have reversed driver torque signal - 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 - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 12.5 # 14.5 stock + + elif candidate == CAR.SUBARU_LEGACY_PREGLOBAL: ret.steerActuatorDelay = 0.15 - elif candidate == CAR.OUTBACK_PREGLOBAL: - ret.mass = 1568 - ret.wheelbase = 2.67 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 20 # learned, 14 stock + elif candidate == CAR.SUBARU_OUTBACK_PREGLOBAL: + pass else: raise ValueError(f"unknown car: {candidate}") - ret.experimentalLongitudinalAvailable = candidate not in (GLOBAL_GEN2 | PREGLOBAL_CARS | LKAS_ANGLE | HYBRID_CARS) + ret.experimentalLongitudinalAvailable = not (ret.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.PREGLOBAL | + SubaruFlags.LKAS_ANGLE | SubaruFlags.HYBRID)) ret.openpilotLongitudinalControl = experimental_long and ret.experimentalLongitudinalAvailable - if candidate in GLOBAL_GEN2 and ret.openpilotLongitudinalControl: + if ret.flags & SubaruFlags.GLOBAL_GEN2 and ret.openpilotLongitudinalControl: ret.flags |= SubaruFlags.DISABLE_EYESIGHT.value if ret.openpilotLongitudinalControl: @@ -148,6 +114,3 @@ class CarInterface(CarInterfaceBase): def init(CP, logcan, sendcan): if CP.flags & SubaruFlags.DISABLE_EYESIGHT: disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/subaru/tests/test_subaru.py b/selfdrive/car/subaru/tests/test_subaru.py new file mode 100644 index 0000000000..33040442b6 --- /dev/null +++ b/selfdrive/car/subaru/tests/test_subaru.py @@ -0,0 +1,16 @@ +from cereal import car +from openpilot.selfdrive.car.subaru.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu + +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +class TestSubaruFingerprint: + def test_fw_version_format(self): + for platform, fws_per_ecu in FW_VERSIONS.items(): + for (ecu, _, _), fws in fws_per_ecu.items(): + fw_size = len(fws[0]) + for fw in fws: + assert len(fw) == fw_size, f"{platform} {ecu}: {len(fw)} {fw_size}" + diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 2f7350b966..dcbea1979f 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -1,11 +1,10 @@ from dataclasses import dataclass, field -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Union +from enum import Enum, IntFlag from cereal import car from panda.python import uds -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Tool, Column +from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Tool, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 Ecu = car.CarParams.Ecu @@ -20,11 +19,11 @@ class CarControllerParams: self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily self.STEER_DRIVER_FACTOR = 1 # from dbc - if CP.carFingerprint in GLOBAL_GEN2: + if CP.flags & SubaruFlags.GLOBAL_GEN2: self.STEER_MAX = 1000 self.STEER_DELTA_UP = 40 self.STEER_DELTA_DOWN = 40 - elif CP.carFingerprint == CAR.IMPREZA_2020: + elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020: self.STEER_MAX = 1439 else: self.STEER_MAX = 2047 @@ -39,7 +38,7 @@ class CarControllerParams: BRAKE_MAX = 600 # about -3.5m/s2 from testing RPM_MIN = 0 - RPM_MAX = 2400 + RPM_MAX = 3600 RPM_INACTIVE = 600 # a good base rpm for zero acceleration @@ -54,9 +53,20 @@ class CarControllerParams: class SubaruFlags(IntFlag): + # Detected flags SEND_INFOTAINMENT = 1 DISABLE_EYESIGHT = 2 + # Static flags + GLOBAL_GEN2 = 4 + + # Cars that temporarily fault when steering angle rate is greater than some threshold. + # Appears to be all torque-based cars produced around 2019 - present + STEER_RATE_LIMITED = 8 + PREGLOBAL = 16 + HYBRID = 32 + LKAS_ANGLE = 64 + GLOBAL_ES_ADDR = 0x787 GEN2_ES_BUTTONS_DID = b'\x11\x30' @@ -68,27 +78,6 @@ class CanBus: camera = 2 -class CAR(StrEnum): - # Global platform - ASCENT = "SUBARU ASCENT LIMITED 2019" - ASCENT_2023 = "SUBARU ASCENT 2023" - IMPREZA = "SUBARU IMPREZA LIMITED 2019" - IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020" - FORESTER = "SUBARU FORESTER 2019" - OUTBACK = "SUBARU OUTBACK 6TH GEN" - CROSSTREK_HYBRID = "SUBARU CROSSTREK HYBRID 2020" - FORESTER_HYBRID = "SUBARU FORESTER HYBRID 2020" - LEGACY = "SUBARU LEGACY 7TH GEN" - FORESTER_2022 = "SUBARU FORESTER 2022" - OUTBACK_2023 = "SUBARU OUTBACK 7TH GEN" - - # Pre-global - FORESTER_PREGLOBAL = "SUBARU FORESTER 2017 - 2018" - LEGACY_PREGLOBAL = "SUBARU LEGACY 2015 - 2018" - OUTBACK_PREGLOBAL = "SUBARU OUTBACK 2015 - 2017" - OUTBACK_PREGLOBAL_2018 = "SUBARU OUTBACK 2018 - 2019" - - class Footnote(Enum): GLOBAL = CarFootnote( "In the non-US market, openpilot requires the car to come equipped with EyeSight with Lane Keep Assistance.", @@ -99,10 +88,10 @@ class Footnote(Enum): @dataclass -class SubaruCarInfo(CarInfo): +class SubaruCarDocs(CarDocs): package: str = "EyeSight Driver Assistance" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.subaru_a])) - footnotes: List[Enum] = field(default_factory=lambda: [Footnote.GLOBAL]) + footnotes: list[Enum] = field(default_factory=lambda: [Footnote.GLOBAL]) def init_make(self, CP: car.CarParams): self.car_parts.parts.extend([Tool.socket_8mm_deep, Tool.pry_tool]) @@ -110,68 +99,165 @@ class SubaruCarInfo(CarInfo): if CP.experimentalLongitudinalAvailable: self.footnotes.append(Footnote.EXP_LONG) -CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { - CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-21", "All"), - CAR.OUTBACK: SubaruCarInfo("Subaru Outback 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.LEGACY: SubaruCarInfo("Subaru Legacy 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.IMPREZA: [ - SubaruCarInfo("Subaru Impreza 2017-19"), - SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), - SubaruCarInfo("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), - ], - CAR.IMPREZA_2020: [ - SubaruCarInfo("Subaru Impreza 2020-22"), - SubaruCarInfo("Subaru Crosstrek 2020-23"), - SubaruCarInfo("Subaru XV 2020-21"), - ], + +@dataclass +class SubaruPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('subaru_global_2017_generated', None)) + + def init(self): + if self.flags & SubaruFlags.HYBRID: + self.dbc_dict = dbc_dict('subaru_global_2020_hybrid_generated', None) + + +@dataclass +class SubaruGen2PlatformConfig(SubaruPlatformConfig): + def init(self): + super().init() + self.flags |= SubaruFlags.GLOBAL_GEN2 + if not (self.flags & SubaruFlags.LKAS_ANGLE): + self.flags |= SubaruFlags.STEER_RATE_LIMITED + + +class CAR(Platforms): + # Global platform + SUBARU_ASCENT = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Ascent 2019-21", "All")], + CarSpecs(mass=2031, wheelbase=2.89, steerRatio=13.5), + ) + SUBARU_OUTBACK = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Outback 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b]))], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=17), + ) + SUBARU_LEGACY = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Legacy 2020-22", "All", car_parts=CarParts.common([CarHarness.subaru_b]))], + SUBARU_OUTBACK.specs, + ) + SUBARU_IMPREZA = SubaruPlatformConfig( + [ + SubaruCarDocs("Subaru Impreza 2017-19"), + SubaruCarDocs("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + SubaruCarDocs("Subaru XV 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), + ], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=15), + ) + SUBARU_IMPREZA_2020 = SubaruPlatformConfig( + [ + SubaruCarDocs("Subaru Impreza 2020-22"), + SubaruCarDocs("Subaru Crosstrek 2020-23"), + SubaruCarDocs("Subaru XV 2020-21"), + ], + CarSpecs(mass=1480, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.STEER_RATE_LIMITED, + ) # TODO: is there an XV and Impreza too? - CAR.CROSSTREK_HYBRID: SubaruCarInfo("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b])), - CAR.FORESTER_HYBRID: SubaruCarInfo("Subaru Forester Hybrid 2020"), - CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), - CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), - CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), - CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), - CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"), - CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022-24", "All", car_parts=CarParts.common([CarHarness.subaru_c])), - CAR.OUTBACK_2023: SubaruCarInfo("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), - CAR.ASCENT_2023: SubaruCarInfo("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), -} - -LKAS_ANGLE = {CAR.FORESTER_2022, CAR.OUTBACK_2023, CAR.ASCENT_2023} -GLOBAL_GEN2 = {CAR.OUTBACK, CAR.LEGACY, CAR.OUTBACK_2023, CAR.ASCENT_2023} -PREGLOBAL_CARS = {CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018} -HYBRID_CARS = {CAR.CROSSTREK_HYBRID, CAR.FORESTER_HYBRID} - -# Cars that temporarily fault when steering angle rate is greater than some threshold. -# Appears to be all torque-based cars produced around 2019 - present -STEER_RATE_LIMITED = GLOBAL_GEN2 | {CAR.IMPREZA_2020, CAR.FORESTER} + SUBARU_CROSSTREK_HYBRID = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Crosstrek Hybrid 2020", car_parts=CarParts.common([CarHarness.subaru_b]))], + CarSpecs(mass=1668, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.HYBRID, + ) + SUBARU_FORESTER = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Forester 2019-21", "All")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=17), + flags=SubaruFlags.STEER_RATE_LIMITED, + ) + SUBARU_FORESTER_HYBRID = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Forester Hybrid 2020")], + SUBARU_FORESTER.specs, + flags=SubaruFlags.HYBRID, + ) + # Pre-global + SUBARU_FORESTER_PREGLOBAL = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Forester 2017-18")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=20), + dbc_dict('subaru_forester_2017_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + SUBARU_LEGACY_PREGLOBAL = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Legacy 2015-18")], + CarSpecs(mass=1568, wheelbase=2.67, steerRatio=12.5), + dbc_dict('subaru_outback_2015_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + SUBARU_OUTBACK_PREGLOBAL = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Outback 2015-17")], + SUBARU_FORESTER_PREGLOBAL.specs, + dbc_dict('subaru_outback_2015_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + SUBARU_OUTBACK_PREGLOBAL_2018 = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Outback 2018-19")], + SUBARU_FORESTER_PREGLOBAL.specs, + dbc_dict('subaru_outback_2019_generated', None), + flags=SubaruFlags.PREGLOBAL, + ) + # Angle LKAS + SUBARU_FORESTER_2022 = SubaruPlatformConfig( + [SubaruCarDocs("Subaru Forester 2022-24", "All", car_parts=CarParts.common([CarHarness.subaru_c]))], + SUBARU_FORESTER.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) + SUBARU_OUTBACK_2023 = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + SUBARU_OUTBACK.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) + SUBARU_ASCENT_2023 = SubaruGen2PlatformConfig( + [SubaruCarDocs("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d]))], + SUBARU_ASCENT.specs, + flags=SubaruFlags.LKAS_ANGLE, + ) + SUBARU_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION) +# The EyeSight ECU takes 10s to respond to SUBARU_VERSION_REQUEST properly, +# log this alternate manufacturer-specific query +SUBARU_ALT_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(0xf100) +SUBARU_ALT_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(0xf100) + FW_QUERY_CONFIG = FwQueryConfig( requests=[ Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], + logging=True, ), + # Non-OBD requests # Some Eyesight modules fail on TESTER_PRESENT_REQUEST # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars Request( [SUBARU_VERSION_REQUEST], [SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.fwdCamera], + bus=0, + ), + Request( + [SUBARU_ALT_VERSION_REQUEST], + [SUBARU_ALT_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera], + bus=0, + logging=True, + ), + Request( + [StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera], + bus=0, + logging=True, ), - # Non-OBD requests Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], bus=0, ), + # GEN2 powertrain bus query Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], @@ -182,24 +268,8 @@ FW_QUERY_CONFIG = FwQueryConfig( ], # We don't get the EPS from non-OBD queries on GEN2 cars. Note that we still attempt to match when it exists non_essential_ecus={ - Ecu.eps: list(GLOBAL_GEN2), + Ecu.eps: list(CAR.with_flags(SubaruFlags.GLOBAL_GEN2)), } ) -DBC = { - CAR.ASCENT: dbc_dict('subaru_global_2017_generated', None), - CAR.ASCENT_2023: dbc_dict('subaru_global_2017_generated', None), - CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), - CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None), - CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None), - CAR.CROSSTREK_HYBRID: dbc_dict('subaru_global_2020_hybrid_generated', None), - CAR.OUTBACK_2023: dbc_dict('subaru_global_2017_generated', None), - CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), - CAR.LEGACY_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), - CAR.OUTBACK_PREGLOBAL: dbc_dict('subaru_outback_2015_generated', None), - CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), -} +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 95a248a614..e460111e32 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,11 +1,12 @@ from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.frame = 0 @@ -59,7 +60,7 @@ class CarController: # TODO: HUD control - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steeringAngleDeg = self.apply_angle_last self.frame += 1 diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 2cb4f09d79..645ea46014 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -2,7 +2,7 @@ import copy from collections import deque from cereal import car from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine @@ -37,13 +37,15 @@ class CarState(CarStateBase): ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) # Steering wheel - self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"] - self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None) - steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None) + epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN else cp.vl["EPAS_sysStatus"] - ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"] + self.hands_on_level = epas_status["EPAS_handsOnLevel"] + self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None) + steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None) + + ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"] ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate - ret.steeringTorque = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"] + ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"] ret.steeringPressed = (self.hands_on_level > 0) ret.steerFaultPermanent = steer_status == "EAC_FAULT" ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON")) @@ -85,7 +87,10 @@ class CarState(CarStateBase): ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) # Seatbelt - ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) + if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: + ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1) + else: + ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1) # TODO: blindspot @@ -111,9 +116,14 @@ class CarState(CarStateBase): ("DI_state", 10), ("STW_ACTN_RQ", 10), ("GTW_carState", 10), - ("SDM1", 10), ("BrakeMessage", 50), ] + + if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: + messages.append(("DriverSeat", 20)) + else: + messages.append(("SDM1", 10)) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis) @staticmethod @@ -122,4 +132,8 @@ class CarState(CarStateBase): # sig_address, frequency ("DAS_control", 40), ] + + if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: + messages.append(("EPAS3P_sysStatus", 100)) + return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis) diff --git a/selfdrive/car/tesla/fingerprints.py b/selfdrive/car/tesla/fingerprints.py index 772ca59efa..68c50a62ed 100644 --- a/selfdrive/car/tesla/fingerprints.py +++ b/selfdrive/car/tesla/fingerprints.py @@ -1,17 +1,10 @@ -# ruff: noqa: E501 from cereal import car from openpilot.selfdrive.car.tesla.values import CAR Ecu = car.CarParams.Ecu -FINGERPRINTS = { - CAR.AP1_MODELS: [{ - 1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 6, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 325: 8, 328: 5, 336: 8, 341: 8, 360: 7, 373: 8, 389: 8, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 526: 8, 532: 3, 536: 8, 537: 3, 542: 8, 551: 5, 552: 2, 556: 8, 558: 8, 568: 8, 569: 8, 574: 8, 577: 8, 582: 5, 584: 4, 585: 8, 590: 8, 606: 8, 622: 8, 627: 6, 638: 8, 641: 8, 643: 8, 660: 5, 693: 8, 696: 8, 697: 8, 712: 8, 728: 8, 744: 8, 760: 8, 772: 8, 775: 8, 776: 8, 777: 8, 778: 8, 782: 8, 788: 8, 791: 8, 792: 8, 796: 2, 797: 8, 798: 6, 799: 8, 804: 8, 805: 8, 807: 8, 808: 1, 809: 8, 812: 8, 813: 8, 814: 5, 815: 8, 820: 8, 823: 8, 824: 8, 829: 8, 830: 5, 836: 8, 840: 8, 841: 8, 845: 8, 846: 5, 852: 8, 856: 4, 857: 6, 861: 8, 862: 5, 872: 8, 873: 8, 877: 8, 878: 8, 879: 8, 880: 8, 884: 8, 888: 8, 889: 8, 893: 8, 896: 8, 901: 6, 904: 3, 905: 8, 908: 2, 909: 8, 920: 8, 921: 8, 925: 4, 936: 8, 937: 8, 941: 8, 949: 8, 952: 8, 953: 6, 957: 8, 968: 8, 973: 8, 984: 8, 987: 8, 989: 8, 990: 8, 1000: 8, 1001: 8, 1006: 8, 1016: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1033: 1, 1034: 8, 1048: 1, 1064: 8, 1070: 8, 1080: 8, 1160: 4, 1281: 8, 1329: 8, 1332: 8, 1335: 8, 1337: 8, 1368: 8, 1412: 8, 1436: 8, 1465: 8, 1476: 8, 1497: 8, 1524: 8, 1527: 8, 1601: 8, 1605: 8, 1611: 8, 1614: 8, 1617: 8, 1621: 8, 1627: 8, 1630: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1828: 8, 1831: 8, 1832: 8, 1840: 8, 1848: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5, 2045: 4 - }], -} - FW_VERSIONS = { - CAR.AP2_MODELS: { + CAR.TESLA_AP2_MODELS: { (Ecu.adas, 0x649, None): [ b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11', ], @@ -25,4 +18,15 @@ FW_VERSIONS = { b'\x10#\x01', ], }, + CAR.TESLA_MODELS_RAVEN: { + (Ecu.electricBrakeBooster, 0x64d, None): [ + b'1037123-00-A', + ], + (Ecu.fwdRadar, 0x671, None): [ + b'\x01\x00\x99\x02\x01\x00\x10\x00\x00AP8.3.03\x00\x10', + ], + (Ecu.eps, 0x730, None): [ + b'SX_0.0.0 (99),SR013.7', + ], + }, } diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index e06139729c..e039859263 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -28,27 +28,20 @@ class CarInterface(CarInterfaceBase): # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus # If so, we assume that it is connected to the longitudinal harness. + flags = (Panda.FLAG_TESLA_RAVEN if candidate == CAR.TESLA_MODELS_RAVEN else 0) if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()): ret.openpilotLongitudinalControl = True + flags |= Panda.FLAG_TESLA_LONG_CONTROL ret.safetyConfigs = [ - get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL), - get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN), + get_safety_config(car.CarParams.SafetyModel.tesla, flags), + get_safety_config(car.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN), ] else: ret.openpilotLongitudinalControl = False - ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)] ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.25 - - if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS): - ret.mass = 2100. - ret.wheelbase = 2.959 - ret.centerToFront = ret.wheelbase * 0.5 - ret.steerRatio = 15.0 - else: - raise ValueError(f"Unsupported car: {candidate}") - return ret def _update(self, c): @@ -57,6 +50,3 @@ class CarInterface(CarInterfaceBase): ret.events = self.create_common_events(ret).to_msg() return ret - - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index b3e7c7fcb1..ae5077824b 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -1,38 +1,33 @@ #!/usr/bin/env python3 from cereal import car from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.tesla.values import DBC, CANBUS +from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS from openpilot.selfdrive.car.interfaces import RadarInterfaceBase -RADAR_MSGS_A = list(range(0x310, 0x36E, 3)) -RADAR_MSGS_B = list(range(0x311, 0x36F, 3)) -NUM_POINTS = len(RADAR_MSGS_A) - -def get_radar_can_parser(CP): - # Status messages - messages = [ - ('TeslaRadarSguInfo', 10), - ] - - # Radar tracks. There are also raw point clouds available, - # we don't use those. - for i in range(NUM_POINTS): - msg_id_a = RADAR_MSGS_A[i] - msg_id_b = RADAR_MSGS_B[i] - messages.extend([ - (msg_id_a, 8), - (msg_id_b, 8), - ]) - - return CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar) class RadarInterface(RadarInterfaceBase): def __init__(self, CP): super().__init__(CP) - self.rcp = get_radar_can_parser(CP) + self.CP = CP + + if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: + messages = [('RadarStatus', 16)] + self.num_points = 40 + self.trigger_msg = 1119 + else: + messages = [('TeslaRadarSguInfo', 10)] + self.num_points = 32 + self.trigger_msg = 878 + + for i in range(self.num_points): + messages.extend([ + (f'RadarPoint{i}_A', 16), + (f'RadarPoint{i}_B', 16), + ]) + + self.rcp = CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar) self.updated_messages = set() self.track_id = 0 - self.trigger_msg = RADAR_MSGS_B[-1] def update(self, can_strings): if self.rcp is None: @@ -48,17 +43,24 @@ class RadarInterface(RadarInterfaceBase): # Errors errors = [] - sgu_info = self.rcp.vl['TeslaRadarSguInfo'] if not self.rcp.can_valid: errors.append('canError') - if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']: - errors.append('fault') + + if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: + radar_status = self.rcp.vl['RadarStatus'] + if radar_status['sensorBlocked'] or radar_status['shortTermUnavailable'] or radar_status['vehDynamicsError']: + errors.append('fault') + else: + radar_status = self.rcp.vl['TeslaRadarSguInfo'] + if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail'] or radar_status['RADC_SensorDirty']: + errors.append('fault') + ret.errors = errors # Radar tracks - for i in range(NUM_POINTS): - msg_a = self.rcp.vl[RADAR_MSGS_A[i]] - msg_b = self.rcp.vl[RADAR_MSGS_B[i]] + for i in range(self.num_points): + msg_a = self.rcp.vl[f'RadarPoint{i}_A'] + msg_b = self.rcp.vl[f'RadarPoint{i}_B'] # Make sure msg A and B are together if msg_a['Index'] != msg_b['Index2']: diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 12877f1344..0f9cd00f63 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -1,32 +1,30 @@ from collections import namedtuple -from enum import StrEnum -from typing import Dict, List, Union from cereal import car -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarInfo +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.docs_definitions import CarDocs from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) - -class CAR(StrEnum): - AP1_MODELS = 'TESLA AP1 MODEL S' - AP2_MODELS = 'TESLA AP2 MODEL S' - - -CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { - CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"), - CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"), -} - - -DBC = { - CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), - CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'), -} +class CAR(Platforms): + TESLA_AP1_MODELS = PlatformConfig( + [CarDocs("Tesla AP1 Model S", "All")], + CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0), + dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can') + ) + TESLA_AP2_MODELS = PlatformConfig( + [CarDocs("Tesla AP2 Model S", "All")], + TESLA_AP1_MODELS.specs, + TESLA_AP1_MODELS.dbc_dict + ) + TESLA_MODELS_RAVEN = PlatformConfig( + [CarDocs("Tesla Model S Raven", "All")], + TESLA_AP1_MODELS.specs, + dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can') + ) FW_QUERY_CONFIG = FwQueryConfig( requests=[ @@ -37,6 +35,13 @@ FW_QUERY_CONFIG = FwQueryConfig( rx_offset=0x08, bus=0, ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE], + whitelist_ecus=[Ecu.eps], + rx_offset=0x08, + bus=0, + ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], @@ -47,7 +52,6 @@ FW_QUERY_CONFIG = FwQueryConfig( ] ) - class CANBUS: # Lateral harness chassis = 0 @@ -89,3 +93,6 @@ class CarControllerParams: def __init__(self, CP): pass + + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/tests/.gitignore b/selfdrive/car/tests/.gitignore new file mode 100644 index 0000000000..192fb0945e --- /dev/null +++ b/selfdrive/car/tests/.gitignore @@ -0,0 +1 @@ +*.bz2 diff --git a/selfdrive/car/tests/big_cars_test.sh b/selfdrive/car/tests/big_cars_test.sh new file mode 100755 index 0000000000..af45c9cd14 --- /dev/null +++ b/selfdrive/car/tests/big_cars_test.sh @@ -0,0 +1,12 @@ +#!/bin/bash + +SCRIPT_DIR=$(dirname "$0") +BASEDIR=$(realpath "$SCRIPT_DIR/../../../") +cd $BASEDIR + +MAX_EXAMPLES=300 +INTERNAL_SEG_CNT=300 +FILEREADER_CACHE=1 +INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt + +cd selfdrive/car/tests && pytest test_models.py test_car_interfaces.py \ No newline at end of file diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py new file mode 100755 index 0000000000..dd3a8f633d --- /dev/null +++ b/selfdrive/car/tests/routes.py @@ -0,0 +1,300 @@ +#!/usr/bin/env python3 +from typing import NamedTuple + +from openpilot.selfdrive.car.chrysler.values import CAR as CHRYSLER +from openpilot.selfdrive.car.gm.values import CAR as GM +from openpilot.selfdrive.car.ford.values import CAR as FORD +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI +from openpilot.selfdrive.car.nissan.values import CAR as NISSAN +from openpilot.selfdrive.car.mazda.values import CAR as MAZDA +from openpilot.selfdrive.car.subaru.values import CAR as SUBARU +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.values import Platform +from openpilot.selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN +from openpilot.selfdrive.car.tesla.values import CAR as TESLA +from openpilot.selfdrive.car.body.values import CAR as COMMA + +# TODO: add routes for these cars +non_tested_cars = [ + FORD.FORD_F_150_MK14, + GM.CADILLAC_ATS, + GM.HOLDEN_ASTRA, + GM.CHEVROLET_MALIBU, + HYUNDAI.GENESIS_G90, + HONDA.HONDA_ODYSSEY_CHN, + VOLKSWAGEN.VOLKSWAGEN_CRAFTER_MK2, # need a route from an ACC-equipped Crafter + SUBARU.SUBARU_FORESTER_HYBRID, +] + + +class CarTestRoute(NamedTuple): + route: str + car_model: Platform | None + segment: int | None = None + + +routes = [ + CarTestRoute("efdf9af95e71cd84|2022-05-13--19-03-31", COMMA.COMMA_BODY), + + CarTestRoute("0c94aa1e1296d7c6|2021-05-05--19-48-37", CHRYSLER.JEEP_GRAND_CHEROKEE), + CarTestRoute("91dfedae61d7bd75|2021-05-22--20-07-52", CHRYSLER.JEEP_GRAND_CHEROKEE_2019), + CarTestRoute("420a8e183f1aed48|2020-03-05--07-15-29", CHRYSLER.CHRYSLER_PACIFICA_2017_HYBRID), + CarTestRoute("43a685a66291579b|2021-05-27--19-47-29", CHRYSLER.CHRYSLER_PACIFICA_2018), + CarTestRoute("378472f830ee7395|2021-05-28--07-38-43", CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID), + CarTestRoute("8190c7275a24557b|2020-01-29--08-33-58", CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID), + CarTestRoute("3d84727705fecd04|2021-05-25--08-38-56", CHRYSLER.CHRYSLER_PACIFICA_2020), + CarTestRoute("221c253375af4ee9|2022-06-15--18-38-24", CHRYSLER.RAM_1500_5TH_GEN), + CarTestRoute("8fb5eabf914632ae|2022-08-04--17-28-53", CHRYSLER.RAM_HD_5TH_GEN, segment=6), + CarTestRoute("3379c85aeedc8285|2023-12-07--17-49-39", CHRYSLER.DODGE_DURANGO), + + CarTestRoute("54827bf84c38b14f|2023-01-25--14-14-11", FORD.FORD_BRONCO_SPORT_MK1), + CarTestRoute("f8eaaccd2a90aef8|2023-05-04--15-10-09", FORD.FORD_ESCAPE_MK4), + CarTestRoute("62241b0c7fea4589|2022-09-01--15-32-49", FORD.FORD_EXPLORER_MK6), + CarTestRoute("e886087f430e7fe7|2023-06-16--23-06-36", FORD.FORD_FOCUS_MK4), + CarTestRoute("bd37e43731e5964b|2023-04-30--10-42-26", FORD.FORD_MAVERICK_MK1), + CarTestRoute("112e4d6e0cad05e1|2023-11-14--08-21-43", FORD.FORD_F_150_LIGHTNING_MK1), + CarTestRoute("83a4e056c7072678|2023-11-13--16-51-33", FORD.FORD_MUSTANG_MACH_E_MK1), + CarTestRoute("37998aa0fade36ab/00000000--48f927c4f5", FORD.FORD_RANGER_MK2), + #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), + + CarTestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.GMC_ACADIA), + CarTestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL), + CarTestRoute("75a6bcb9b8b40373|2023-03-11--22-47-33", GM.BUICK_LACROSSE), + CarTestRoute("e746f59bc96fd789|2024-01-31--22-25-58", GM.CHEVROLET_EQUINOX), + CarTestRoute("ef8f2185104d862e|2023-02-09--18-37-13", GM.CADILLAC_ESCALADE), + CarTestRoute("46460f0da08e621e|2021-10-26--07-21-46", GM.CADILLAC_ESCALADE_ESV), + CarTestRoute("168f8b3be57f66ae|2023-09-12--21-44-42", GM.CADILLAC_ESCALADE_ESV_2019), + CarTestRoute("c950e28c26b5b168|2018-05-30--22-03-41", GM.CHEVROLET_VOLT), + CarTestRoute("f08912a233c1584f|2022-08-11--18-02-41", GM.CHEVROLET_BOLT_EUV, segment=1), + CarTestRoute("555d4087cf86aa91|2022-12-02--12-15-07", GM.CHEVROLET_BOLT_EUV, segment=14), # Bolt EV + CarTestRoute("38aa7da107d5d252|2022-08-15--16-01-12", GM.CHEVROLET_SILVERADO), + CarTestRoute("5085c761395d1fe6|2023-04-07--18-20-06", GM.CHEVROLET_TRAILBLAZER), + + CarTestRoute("0e7a2ba168465df5|2020-10-18--14-14-22", HONDA.ACURA_RDX_3G), + CarTestRoute("a74b011b32b51b56|2020-07-26--17-09-36", HONDA.HONDA_CIVIC), + CarTestRoute("a859a044a447c2b0|2020-03-03--18-42-45", HONDA.HONDA_CRV_EU), + CarTestRoute("68aac44ad69f838e|2021-05-18--20-40-52", HONDA.HONDA_CRV), + CarTestRoute("14fed2e5fa0aa1a5|2021-05-25--14-59-42", HONDA.HONDA_CRV_HYBRID), + CarTestRoute("52f3e9ae60c0d886|2021-05-23--15-59-43", HONDA.HONDA_FIT), + CarTestRoute("2c4292a5cd10536c|2021-08-19--21-32-15", HONDA.HONDA_FREED), + CarTestRoute("03be5f2fd5c508d1|2020-04-19--18-44-15", HONDA.HONDA_HRV), + CarTestRoute("320098ff6c5e4730|2023-04-13--17-47-46", HONDA.HONDA_HRV_3G), + CarTestRoute("147613502316e718/00000001--dd141a3140", HONDA.HONDA_HRV_3G), # Brazilian model + CarTestRoute("917b074700869333|2021-05-24--20-40-20", HONDA.ACURA_ILX), + CarTestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.HONDA_ACCORD), # 1.5T + CarTestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.HONDA_ACCORD), # 2.0T + CarTestRoute("085ac1d942c35910|2021-03-25--20-11-15", HONDA.HONDA_ACCORD), # 2021 with new style HUD msgs + CarTestRoute("07585b0da3c88459|2021-05-26--18-52-04", HONDA.HONDA_ACCORD), # hybrid + CarTestRoute("f29e2b57a55e7ad5|2021-03-24--20-52-38", HONDA.HONDA_ACCORD), # hybrid, 2021 with new style HUD msgs + CarTestRoute("1ad763dd22ef1a0e|2020-02-29--18-37-03", HONDA.HONDA_CRV_5G), + CarTestRoute("0a96f86fcfe35964|2020-02-05--07-25-51", HONDA.HONDA_ODYSSEY), + CarTestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.HONDA_CIVIC_BOSCH_DIESEL), + CarTestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.HONDA_INSIGHT), + CarTestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.HONDA_PILOT), + CarTestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.HONDA_PILOT), # Passport + CarTestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.HONDA_CIVIC_BOSCH), + CarTestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), + CarTestRoute("54fd8451b3974762|2021-04-01--14-50-10", HONDA.HONDA_RIDGELINE), + CarTestRoute("2d5808fae0b38ac6|2021-09-01--17-14-11", HONDA.HONDA_E), + CarTestRoute("f44aa96ace22f34a|2021-12-22--06-22-31", HONDA.HONDA_CIVIC_2022), + + CarTestRoute("87d7f06ade479c2e|2023-09-11--23-30-11", HYUNDAI.HYUNDAI_AZERA_6TH_GEN), + CarTestRoute("66189dd8ec7b50e6|2023-09-20--07-02-12", HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN), + CarTestRoute("6fe86b4e410e4c37|2020-07-22--16-27-13", HYUNDAI.HYUNDAI_GENESIS), + CarTestRoute("b5d6dc830ad63071|2022-12-12--21-28-25", HYUNDAI.GENESIS_GV60_EV_1ST_GEN, segment=12), + CarTestRoute("70c5bec28ec8e345|2020-08-08--12-22-23", HYUNDAI.GENESIS_G70), + CarTestRoute("ca4de5b12321bd98|2022-10-18--21-15-59", HYUNDAI.GENESIS_GV70_1ST_GEN), + CarTestRoute("6b301bf83f10aa90|2020-11-22--16-45-07", HYUNDAI.GENESIS_G80), + CarTestRoute("0bbe367c98fa1538|2023-09-16--00-16-49", HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN), + CarTestRoute("f0709d2bc6ca451f|2022-10-15--08-13-54", HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN), + CarTestRoute("4dbd55df87507948|2022-03-01--09-45-38", HYUNDAI.HYUNDAI_SANTA_FE), + CarTestRoute("bf43d9df2b660eb0|2021-09-23--14-16-37", HYUNDAI.HYUNDAI_SANTA_FE_2022), + CarTestRoute("37398f32561a23ad|2021-11-18--00-11-35", HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022), + CarTestRoute("656ac0d830792fcc|2021-12-28--14-45-56", HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022, segment=1), + CarTestRoute("de59124955b921d8|2023-06-24--00-12-50", HYUNDAI.KIA_CARNIVAL_4TH_GEN), + CarTestRoute("409c9409979a8abc|2023-07-11--09-06-44", HYUNDAI.KIA_CARNIVAL_4TH_GEN), # Chinese model + CarTestRoute("e0e98335f3ebc58f|2021-03-07--16-38-29", HYUNDAI.KIA_CEED), + CarTestRoute("7653b2bce7bcfdaa|2020-03-04--15-34-32", HYUNDAI.KIA_OPTIMA_G4), + CarTestRoute("018654717bc93d7d|2022-09-19--23-11-10", HYUNDAI.KIA_OPTIMA_G4_FL, segment=0), + CarTestRoute("f9716670b2481438|2023-08-23--14-49-50", HYUNDAI.KIA_OPTIMA_H), + CarTestRoute("6a42c1197b2a8179|2023-09-21--10-23-44", HYUNDAI.KIA_OPTIMA_H_G4_FL), + CarTestRoute("c75a59efa0ecd502|2021-03-11--20-52-55", HYUNDAI.KIA_SELTOS), + CarTestRoute("5b7c365c50084530|2020-04-15--16-13-24", HYUNDAI.HYUNDAI_SONATA), + CarTestRoute("b2a38c712dcf90bd|2020-05-18--18-12-48", HYUNDAI.HYUNDAI_SONATA_LF), + CarTestRoute("c344fd2492c7a9d2|2023-12-11--09-03-23", HYUNDAI.HYUNDAI_STARIA_4TH_GEN), + CarTestRoute("fb3fd42f0baaa2f8|2022-03-30--15-25-05", HYUNDAI.HYUNDAI_TUCSON), + CarTestRoute("db68bbe12250812c|2022-12-05--00-54-12", HYUNDAI.HYUNDAI_TUCSON_4TH_GEN), # 2023 + CarTestRoute("36e10531feea61a4|2022-07-25--13-37-42", HYUNDAI.HYUNDAI_TUCSON_4TH_GEN), # hybrid + CarTestRoute("5875672fc1d4bf57|2020-07-23--21-33-28", HYUNDAI.KIA_SORENTO), + CarTestRoute("1d0d000db3370fd0|2023-01-04--22-28-42", HYUNDAI.KIA_SORENTO_4TH_GEN, segment=5), + CarTestRoute("fc19648042eb6896|2023-08-16--11-43-27", HYUNDAI.KIA_SORENTO_HEV_4TH_GEN, segment=14), + CarTestRoute("628935d7d3e5f4f7|2022-11-30--01-12-46", HYUNDAI.KIA_SORENTO_HEV_4TH_GEN), # plug-in hybrid + CarTestRoute("9c917ba0d42ffe78|2020-04-17--12-43-19", HYUNDAI.HYUNDAI_PALISADE), + CarTestRoute("05a8f0197fdac372|2022-10-19--14-14-09", HYUNDAI.HYUNDAI_IONIQ_5), # HDA2 + CarTestRoute("eb4eae1476647463|2023-08-26--18-07-04", HYUNDAI.HYUNDAI_IONIQ_6, segment=6), # HDA2 + CarTestRoute("3f29334d6134fcd4|2022-03-30--22-00-50", HYUNDAI.HYUNDAI_IONIQ_PHEV_2019), + CarTestRoute("fa8db5869167f821|2021-06-10--22-50-10", HYUNDAI.HYUNDAI_IONIQ_PHEV), + CarTestRoute("e1107f9d04dfb1e2|2023-09-05--22-32-12", HYUNDAI.HYUNDAI_IONIQ_PHEV), # openpilot longitudinal enabled + CarTestRoute("2c5cf2dd6102e5da|2020-12-17--16-06-44", HYUNDAI.HYUNDAI_IONIQ_EV_2020), + CarTestRoute("610ebb9faaad6b43|2020-06-13--15-28-36", HYUNDAI.HYUNDAI_IONIQ_EV_LTD), + CarTestRoute("2c5cf2dd6102e5da|2020-06-26--16-00-08", HYUNDAI.HYUNDAI_IONIQ), + CarTestRoute("012c95f06918eca4|2023-01-15--11-19-36", HYUNDAI.HYUNDAI_IONIQ), # openpilot longitudinal enabled + CarTestRoute("ab59fe909f626921|2021-10-18--18-34-28", HYUNDAI.HYUNDAI_IONIQ_HEV_2022), + CarTestRoute("22d955b2cd499c22|2020-08-10--19-58-21", HYUNDAI.HYUNDAI_KONA), + CarTestRoute("efc48acf44b1e64d|2021-05-28--21-05-04", HYUNDAI.HYUNDAI_KONA_EV), + CarTestRoute("f90d3cd06caeb6fa|2023-09-06--17-15-47", HYUNDAI.HYUNDAI_KONA_EV), # openpilot longitudinal enabled + CarTestRoute("ff973b941a69366f|2022-07-28--22-01-19", HYUNDAI.HYUNDAI_KONA_EV_2022, segment=11), + CarTestRoute("1618132d68afc876|2023-08-27--09-32-14", HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN, segment=13), + CarTestRoute("49f3c13141b6bc87|2021-07-28--08-05-13", HYUNDAI.HYUNDAI_KONA_HEV), + CarTestRoute("5dddcbca6eb66c62|2020-07-26--13-24-19", HYUNDAI.KIA_STINGER), + CarTestRoute("5b50b883a4259afb|2022-11-09--15-00-42", HYUNDAI.KIA_STINGER_2022), + CarTestRoute("d624b3d19adce635|2020-08-01--14-59-12", HYUNDAI.HYUNDAI_VELOSTER), + CarTestRoute("d545129f3ca90f28|2022-10-19--09-22-54", HYUNDAI.KIA_EV6), # HDA2 + CarTestRoute("68d6a96e703c00c9|2022-09-10--16-09-39", HYUNDAI.KIA_EV6), # HDA1 + CarTestRoute("9b25e8c1484a1b67|2023-04-13--10-41-45", HYUNDAI.KIA_EV6), + CarTestRoute("007d5e4ad9f86d13|2021-09-30--15-09-23", HYUNDAI.KIA_K5_2021), + CarTestRoute("c58dfc9fc16590e0|2023-01-14--13-51-48", HYUNDAI.KIA_K5_HEV_2020), + CarTestRoute("78ad5150de133637|2023-09-13--16-15-57", HYUNDAI.KIA_K8_HEV_1ST_GEN), + CarTestRoute("50c6c9b85fd1ff03|2020-10-26--17-56-06", HYUNDAI.KIA_NIRO_EV), + CarTestRoute("b153671049a867b3|2023-04-05--10-00-30", HYUNDAI.KIA_NIRO_EV_2ND_GEN), + CarTestRoute("173219cf50acdd7b|2021-07-05--10-27-41", HYUNDAI.KIA_NIRO_PHEV), + CarTestRoute("23349923ba5c4e3b|2023-12-02--08-51-54", HYUNDAI.KIA_NIRO_PHEV_2022), + CarTestRoute("34a875f29f69841a|2021-07-29--13-02-09", HYUNDAI.KIA_NIRO_HEV_2021), + CarTestRoute("db04d2c63990e3ba|2023-02-08--16-52-39", HYUNDAI.KIA_NIRO_HEV_2ND_GEN), + CarTestRoute("50a2212c41f65c7b|2021-05-24--16-22-06", HYUNDAI.KIA_FORTE), + CarTestRoute("192283cdbb7a58c2|2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN), + CarTestRoute("09559f1fcaed4704|2023-11-16--02-24-57", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # openpilot longitudinal + CarTestRoute("b3537035ffe6a7d6|2022-10-17--15-23-49", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # hybrid + CarTestRoute("c5ac319aa9583f83|2021-06-01--18-18-31", HYUNDAI.HYUNDAI_ELANTRA), + CarTestRoute("734ef96182ddf940|2022-10-02--16-41-44", HYUNDAI.HYUNDAI_ELANTRA_GT_I30), + CarTestRoute("82e9cdd3f43bf83e|2021-05-15--02-42-51", HYUNDAI.HYUNDAI_ELANTRA_2021), + CarTestRoute("715ac05b594e9c59|2021-06-20--16-21-07", HYUNDAI.HYUNDAI_ELANTRA_HEV_2021), + CarTestRoute("7120aa90bbc3add7|2021-08-02--07-12-31", HYUNDAI.HYUNDAI_SONATA_HYBRID), + CarTestRoute("715ac05b594e9c59|2021-10-27--23-24-56", HYUNDAI.GENESIS_G70_2020), + CarTestRoute("6b0d44d22df18134|2023-05-06--10-36-55", HYUNDAI.GENESIS_GV80), + + CarTestRoute("00c829b1b7613dea|2021-06-24--09-10-10", TOYOTA.TOYOTA_ALPHARD_TSS2), + CarTestRoute("912119ebd02c7a42|2022-03-19--07-24-50", TOYOTA.TOYOTA_ALPHARD_TSS2), # hybrid + CarTestRoute("000cf3730200c71c|2021-05-24--10-42-05", TOYOTA.TOYOTA_AVALON), + CarTestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.TOYOTA_AVALON_2019), + CarTestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.TOYOTA_AVALON_2019), # hybrid + CarTestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.TOYOTA_AVALON_TSS2), + CarTestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.TOYOTA_AVALON_TSS2), # hybrid + CarTestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.TOYOTA_CAMRY), + CarTestRoute("2f37c007683e85ba|2023-09-02--14-39-44", TOYOTA.TOYOTA_CAMRY), # openpilot longitudinal, with radar CAN filter + CarTestRoute("54034823d30962f5|2021-05-24--06-37-34", TOYOTA.TOYOTA_CAMRY), # hybrid + CarTestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.TOYOTA_CAMRY_TSS2), + CarTestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.TOYOTA_CAMRY_TSS2), # hybrid + CarTestRoute("4e45c89c38e8ec4d|2021-05-02--02-49-28", TOYOTA.TOYOTA_COROLLA), + CarTestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.TOYOTA_COROLLA_TSS2), + CarTestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.TOYOTA_COROLLA_TSS2), # hybrid + CarTestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.TOYOTA_PRIUS), + CarTestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.TOYOTA_RAV4), + CarTestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.TOYOTA_RAV4H), + CarTestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.TOYOTA_RAV4_TSS2), + CarTestRoute("a5c341bb250ca2f0|2022-05-18--16-05-17", TOYOTA.TOYOTA_RAV4_TSS2_2022), + CarTestRoute("ad5a3fa719bc2f83|2023-10-17--19-48-42", TOYOTA.TOYOTA_RAV4_TSS2_2023), + CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.TOYOTA_RAV4_TSS2), # hybrid + CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022), # hybrid + CarTestRoute("7a31f030957b9c85|2023-04-01--14-12-51", TOYOTA.LEXUS_ES), + CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ES), # hybrid + CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), + CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ES_TSS2), # hybrid + CarTestRoute("da23c367491f53e2|2021-05-21--09-09-11", TOYOTA.LEXUS_CTH, segment=3), + CarTestRoute("32696cea52831b02|2021-11-19--18-13-30", TOYOTA.LEXUS_RC), + CarTestRoute("ab9b64a5e5960cba|2023-10-24--17-32-08", TOYOTA.LEXUS_GS_F), + CarTestRoute("886fcd8408d570e9|2020-01-29--02-18-55", TOYOTA.LEXUS_RX), + CarTestRoute("d27ad752e9b08d4f|2021-05-26--19-39-51", TOYOTA.LEXUS_RX), # hybrid + CarTestRoute("01b22eb2ed121565|2020-02-02--11-25-51", TOYOTA.LEXUS_RX_TSS2), + CarTestRoute("b74758c690a49668|2020-05-20--15-58-57", TOYOTA.LEXUS_RX_TSS2), # hybrid + CarTestRoute("964c09eb11ca8089|2020-11-03--22-04-00", TOYOTA.LEXUS_NX), + CarTestRoute("ec429c0f37564e3c|2020-02-01--17-28-12", TOYOTA.LEXUS_NX), # hybrid + CarTestRoute("3fd5305f8b6ca765|2021-04-28--19-26-49", TOYOTA.LEXUS_NX_TSS2), + CarTestRoute("09ae96064ed85a14|2022-06-09--12-22-31", TOYOTA.LEXUS_NX_TSS2), # hybrid + CarTestRoute("4765fbbf59e3cd88|2024-02-06--17-45-32", TOYOTA.LEXUS_LC_TSS2), + CarTestRoute("0a302ffddbb3e3d3|2020-02-08--16-19-08", TOYOTA.TOYOTA_HIGHLANDER_TSS2), + CarTestRoute("437e4d2402abf524|2021-05-25--07-58-50", TOYOTA.TOYOTA_HIGHLANDER_TSS2), # hybrid + CarTestRoute("3183cd9b021e89ce|2021-05-25--10-34-44", TOYOTA.TOYOTA_HIGHLANDER), + CarTestRoute("80d16a262e33d57f|2021-05-23--20-01-43", TOYOTA.TOYOTA_HIGHLANDER), # hybrid + CarTestRoute("eb6acd681135480d|2019-06-20--20-00-00", TOYOTA.TOYOTA_SIENNA), + CarTestRoute("2e07163a1ba9a780|2019-08-25--13-15-13", TOYOTA.LEXUS_IS), + CarTestRoute("649bf2997ada6e3a|2023-08-08--18-04-22", TOYOTA.LEXUS_IS_TSS2), + CarTestRoute("0a0de17a1e6a2d15|2020-09-21--21-24-41", TOYOTA.TOYOTA_PRIUS_TSS2), + CarTestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.TOYOTA_MIRAI), + CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.TOYOTA_CHR), + CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.TOYOTA_CHR), # hybrid + CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.TOYOTA_CHR_TSS2), + CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.TOYOTA_CHR_TSS2), # openpilot longitudinal, with smartDSU + CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid + CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled + CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.TOYOTA_PRIUS_V), + + CarTestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1), + CarTestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.VOLKSWAGEN_ATLAS_MK1), + CarTestRoute("ffcd23abbbd02219|2024-02-28--14-59-38", VOLKSWAGEN.VOLKSWAGEN_CADDY_MK3), + CarTestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # Stock ACC + CarTestRoute("3cfdec54aa035f3f|2022-10-13--14-58-58", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # openpilot longitudinal + CarTestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.VOLKSWAGEN_JETTA_MK7), + CarTestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.VOLKSWAGEN_PASSAT_MK8), + CarTestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.VOLKSWAGEN_PASSAT_NMS), + CarTestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.VOLKSWAGEN_POLO_MK6), + CarTestRoute("064d1816e448f8eb|2022-09-29--15-32-34", VOLKSWAGEN.VOLKSWAGEN_SHARAN_MK2), + CarTestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.VOLKSWAGEN_TAOS_MK1), + CarTestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.VOLKSWAGEN_TCROSS_MK1), + CarTestRoute("2cef8a0b898f331a|2021-03-25--20-13-57", VOLKSWAGEN.VOLKSWAGEN_TIGUAN_MK2), + CarTestRoute("a589dcc642fdb10a|2021-06-14--20-54-26", VOLKSWAGEN.VOLKSWAGEN_TOURAN_MK2), + CarTestRoute("a459f4556782eba1|2021-09-19--09-48-00", VOLKSWAGEN.VOLKSWAGEN_TRANSPORTER_T61), + CarTestRoute("0cd0b7f7e31a3853|2021-11-18--00-38-32", VOLKSWAGEN.VOLKSWAGEN_TROC_MK1), + CarTestRoute("07667b885add75fd|2021-01-23--19-48-42", VOLKSWAGEN.AUDI_A3_MK3), + CarTestRoute("6c6b466346192818|2021-06-06--14-17-47", VOLKSWAGEN.AUDI_Q2_MK1), + CarTestRoute("0cd0b7f7e31a3853|2021-12-03--03-12-05", VOLKSWAGEN.AUDI_Q3_MK2), + CarTestRoute("8f205bdd11bcbb65|2021-03-26--01-00-17", VOLKSWAGEN.SEAT_ATECA_MK1), + CarTestRoute("fc6b6c9a3471c846|2021-05-27--13-39-56", VOLKSWAGEN.SEAT_ATECA_MK1), # Leon + CarTestRoute("0bbe367c98fa1538|2023-03-04--17-46-11", VOLKSWAGEN.SKODA_FABIA_MK4), + CarTestRoute("12d6ae3057c04b0d|2021-09-15--00-04-07", VOLKSWAGEN.SKODA_KAMIQ_MK1), + CarTestRoute("12d6ae3057c04b0d|2021-09-04--21-21-21", VOLKSWAGEN.SKODA_KAROQ_MK1), + CarTestRoute("90434ff5d7c8d603|2021-03-15--12-07-31", VOLKSWAGEN.SKODA_KODIAQ_MK1), + CarTestRoute("66e5edc3a16459c5|2021-05-25--19-00-29", VOLKSWAGEN.SKODA_OCTAVIA_MK3), + CarTestRoute("026b6d18fba6417f|2021-03-26--09-17-04", VOLKSWAGEN.SKODA_KAMIQ_MK1), # Scala + CarTestRoute("b2e9858e29db492b|2021-03-26--16-58-42", VOLKSWAGEN.SKODA_SUPERB_MK3), + + CarTestRoute("3c8f0c502e119c1c|2020-06-30--12-58-02", SUBARU.SUBARU_ASCENT), + CarTestRoute("c321c6b697c5a5ff|2020-06-23--11-04-33", SUBARU.SUBARU_FORESTER), + CarTestRoute("791340bc01ed993d|2019-03-10--16-28-08", SUBARU.SUBARU_IMPREZA), + CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.SUBARU_IMPREZA_2020), + CarTestRoute("8de015561e1ea4a0|2023-08-29--17-08-31", SUBARU.SUBARU_IMPREZA), # openpilot longitudinal + # CarTestRoute("c3d1ccb52f5f9d65|2023-07-22--01-23-20", SUBARU.OUTBACK, segment=9), # gen2 longitudinal, eyesight disabled + CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.SUBARU_OUTBACK, segment=10), + CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.SUBARU_LEGACY, segment=3), + CarTestRoute("f4e3a0c511a076f4|2022-08-04--16-16-48", SUBARU.SUBARU_CROSSTREK_HYBRID, segment=2), + CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.SUBARU_FORESTER_2022, segment=4), + CarTestRoute("f3b34c0d2632aa83|2023-07-23--20-43-25", SUBARU.SUBARU_OUTBACK_2023, segment=7), + CarTestRoute("99437cef6d5ff2ee|2023-03-13--21-21-38", SUBARU.SUBARU_ASCENT_2023, segment=7), + # Pre-global, dashcam + CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.SUBARU_FORESTER_PREGLOBAL), + CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.SUBARU_LEGACY_PREGLOBAL), + CarTestRoute("5ab784f361e19b78|2020-06-08--16-30-41", SUBARU.SUBARU_OUTBACK_PREGLOBAL), + CarTestRoute("e19eb5d5353b1ac1|2020-08-09--14-37-56", SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018), + + CarTestRoute("fbbfa6af821552b9|2020-03-03--08-09-43", NISSAN.NISSAN_XTRAIL), + CarTestRoute("5b7c365c50084530|2020-03-25--22-10-13", NISSAN.NISSAN_LEAF), + CarTestRoute("22c3dcce2dd627eb|2020-12-30--16-38-48", NISSAN.NISSAN_LEAF_IC), + CarTestRoute("059ab9162e23198e|2020-05-30--09-41-01", NISSAN.NISSAN_ROGUE), + CarTestRoute("b72d3ec617c0a90f|2020-12-11--15-38-17", NISSAN.NISSAN_ALTIMA), + + CarTestRoute("32a319f057902bb3|2020-04-27--15-18-58", MAZDA.MAZDA_CX5), + CarTestRoute("10b5a4b380434151|2020-08-26--17-11-45", MAZDA.MAZDA_CX9), + CarTestRoute("74f1038827005090|2020-08-26--20-05-50", MAZDA.MAZDA_3), + CarTestRoute("fb53c640f499b73d|2021-06-01--04-17-56", MAZDA.MAZDA_6), + CarTestRoute("f6d5b1a9d7a1c92e|2021-07-08--06-56-59", MAZDA.MAZDA_CX9_2021), + CarTestRoute("a4af1602d8e668ac|2022-02-03--12-17-07", MAZDA.MAZDA_CX5_2022), + + CarTestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.TESLA_AP1_MODELS), + CarTestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.TESLA_AP2_MODELS), + CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.TESLA_MODELS_RAVEN), + + # Segments that test specific issues + # Controls mismatch due to standstill threshold + CarTestRoute("bec2dcfde6a64235|2022-04-08--14-21-32", HONDA.HONDA_CRV_HYBRID, segment=22), +] diff --git a/selfdrive/car/tests/test_can_fingerprint.py b/selfdrive/car/tests/test_can_fingerprint.py new file mode 100644 index 0000000000..f236986d8e --- /dev/null +++ b/selfdrive/car/tests/test_can_fingerprint.py @@ -0,0 +1,61 @@ +from parameterized import parameterized + +from cereal import log, messaging +from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, can_fingerprint +from openpilot.selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS + + +class TestCanFingerprint: + @parameterized.expand(list(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 + + assert car_fingerprint == car_model + assert finger[0] == fingerprint + assert finger[1] == fingerprint + assert finger[2] == {} + + def test_timing(self, subtests): + # just pick any CAN fingerprinting car + car_model = "CHEVROLET_BOLT_EUV" + fingerprint = FINGERPRINTS[car_model][0] + + cases = [] + + # case 1 - one match, make sure we keep going for 100 frames + 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)] + cases.append((FRAME_FINGERPRINT, car_model, can)) + + # case 2 - no matches, make sure we keep going for 100 frames + can = messaging.new_message('can', 1) + can.can = [log.CanData(address=1, dat=b'\x00' * 1, src=src) for src in (0, 1)] # uncommon address + cases.append((FRAME_FINGERPRINT, None, can)) + + # case 3 - multiple matches, make sure we keep going for 200 frames to try to eliminate some + can = messaging.new_message('can', 1) + can.can = [log.CanData(address=2016, dat=b'\x00' * 8, src=src) for src in (0, 1)] # common address + cases.append((FRAME_FINGERPRINT * 2, None, can)) + + for expected_frames, car_model, can in cases: + with subtests.test(expected_frames=expected_frames, car_model=car_model): + frames = 0 + + def test(): + nonlocal frames + frames += 1 + return can # noqa: B023 + + car_fingerprint, _ = can_fingerprint(test) + assert car_fingerprint == car_model + assert frames == expected_frames + 2# TODO: fix extra frames diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py old mode 100755 new mode 100644 index a454f616cb..19096c23e5 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -1,7 +1,5 @@ -#!/usr/bin/env python3 import os import math -import unittest import hypothesis.strategies as st from hypothesis import Phase, given, settings import importlib @@ -12,7 +10,7 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import gen_empty_fingerprint from openpilot.selfdrive.car.car_helpers import interfaces from openpilot.selfdrive.car.fingerprints import all_known_cars -from openpilot.selfdrive.car.fw_versions import FW_VERSIONS +from openpilot.selfdrive.car.fw_versions import FW_VERSIONS, FW_QUERY_CONFIGS from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -20,9 +18,12 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator -ALL_ECUS = list({ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()}) +ALL_ECUS = {ecu for ecus in FW_VERSIONS.values() for ecu in ecus.keys()} +ALL_ECUS |= {ecu for config in FW_QUERY_CONFIGS.values() for ecu in config.extra_ecus} -MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '20')) +ALL_REQUESTS = {tuple(r.request) for config in FW_QUERY_CONFIGS.values() for r in config.requests} + +MAX_EXAMPLES = int(os.environ.get('MAX_EXAMPLES', '40')) def get_fuzzy_car_interface_args(draw: DrawType) -> dict: @@ -32,7 +33,7 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: gen_empty_fingerprint()}) # only pick from possible ecus to reduce search space - car_fw_strategy = st.lists(st.sampled_from(ALL_ECUS)) + car_fw_strategy = st.lists(st.sampled_from(sorted(ALL_ECUS))) params_strategy = st.fixed_dictionaries({ 'fingerprints': fingerprint_strategy, @@ -41,11 +42,13 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict: }) params: dict = draw(params_strategy) - params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0) for fw in params['car_fw']] + params['car_fw'] = [car.CarParams.CarFw(ecu=fw[0], address=fw[1], subAddress=fw[2] or 0, + request=draw(st.sampled_from(sorted(ALL_REQUESTS)))) + for fw in params['car_fw']] return params -class TestCarInterfaces(unittest.TestCase): +class TestCarInterfaces: # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause # many generated examples to overrun when max_examples > ~20, don't use it @parameterized.expand([(car,) for car in sorted(all_known_cars())]) @@ -63,32 +66,28 @@ class TestCarInterfaces(unittest.TestCase): assert car_params assert car_interface - self.assertGreater(car_params.mass, 1) - self.assertGreater(car_params.wheelbase, 0) + assert car_params.mass > 1 + assert car_params.wheelbase > 0 # centerToFront is center of gravity to front wheels, assert a reasonable range - self.assertTrue(car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7) - self.assertGreater(car_params.maxLateralAccel, 0) + assert car_params.wheelbase * 0.3 < car_params.centerToFront < car_params.wheelbase * 0.7 + assert car_params.maxLateralAccel > 0 # Longitudinal sanity checks - self.assertEqual(len(car_params.longitudinalTuning.kpV), len(car_params.longitudinalTuning.kpBP)) - self.assertEqual(len(car_params.longitudinalTuning.kiV), len(car_params.longitudinalTuning.kiBP)) - self.assertEqual(len(car_params.longitudinalTuning.deadzoneV), len(car_params.longitudinalTuning.deadzoneBP)) - - # If we're using the interceptor for gasPressed, we should be commanding gas with it - if car_params.enableGasInterceptor: - self.assertTrue(car_params.openpilotLongitudinalControl) + assert len(car_params.longitudinalTuning.kpV) == len(car_params.longitudinalTuning.kpBP) + assert len(car_params.longitudinalTuning.kiV) == len(car_params.longitudinalTuning.kiBP) + assert len(car_params.longitudinalTuning.deadzoneV) == len(car_params.longitudinalTuning.deadzoneBP) # Lateral sanity checks if car_params.steerControlType != car.CarParams.SteerControlType.angle: tune = car_params.lateralTuning if tune.which() == 'pid': - self.assertTrue(not math.isnan(tune.pid.kf) and tune.pid.kf > 0) - self.assertTrue(len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP)) - self.assertTrue(len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP)) + assert not math.isnan(tune.pid.kf) and tune.pid.kf > 0 + assert len(tune.pid.kpV) > 0 and len(tune.pid.kpV) == len(tune.pid.kpBP) + assert len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP) elif tune.which() == 'torque': - self.assertTrue(not math.isnan(tune.torque.kf) and tune.torque.kf > 0) - self.assertTrue(not math.isnan(tune.torque.friction) and tune.torque.friction > 0) + assert not math.isnan(tune.torque.kf) and tune.torque.kf > 0 + assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0 cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) # Run car interface @@ -96,16 +95,14 @@ class TestCarInterfaces(unittest.TestCase): CC = car.CarControl.new_message(**cc_msg) for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC.as_reader(), now_nanos) now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) CC.enabled = True for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, now_nanos) - car_interface.apply(CC, now_nanos) + car_interface.apply(CC.as_reader(), now_nanos) now_nanos += DT_CTRL * 1e9 # 10ms # Test controller initialization @@ -134,33 +131,29 @@ class TestCarInterfaces(unittest.TestCase): if not car_params.radarUnavailable and radar_interface.rcp is not None: cans = [messaging.new_message('can', 1).to_bytes() for _ in range(5)] rr = radar_interface.update(cans) - self.assertTrue(rr is None or len(rr.errors) > 0) + assert rr is None or len(rr.errors) > 0 def test_interface_attrs(self): """Asserts basic behavior of interface attribute getter""" num_brands = len(get_interface_attr('CAR')) - self.assertGreaterEqual(num_brands, 13) + assert num_brands >= 13 # Should return value for all brands when not combining, even if attribute doesn't exist ret = get_interface_attr('FAKE_ATTR') - self.assertEqual(len(ret), num_brands) + assert len(ret) == num_brands # Make sure we can combine dicts ret = get_interface_attr('DBC', combine_brands=True) - self.assertGreaterEqual(len(ret), 160) + assert len(ret) >= 160 # We don't support combining non-dicts ret = get_interface_attr('CAR', combine_brands=True) - self.assertEqual(len(ret), 0) + assert len(ret) == 0 # If brand has None value, it shouldn't return when ignore_none=True is specified none_brands = {b for b, v in get_interface_attr('FINGERPRINTS').items() if v is None} - self.assertGreaterEqual(len(none_brands), 1) + assert len(none_brands) >= 1 ret = get_interface_attr('FINGERPRINTS', ignore_none=True) none_brands_in_ret = none_brands.intersection(ret) - self.assertEqual(len(none_brands_in_ret), 0, f'Brands with None values in ignore_none=True result: {none_brands_in_ret}') - - -if __name__ == "__main__": - unittest.main() + assert len(none_brands_in_ret) == 0, f'Brands with None values in ignore_none=True result: {none_brands_in_ret}' diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py new file mode 100644 index 0000000000..40ad07b283 --- /dev/null +++ b/selfdrive/car/tests/test_docs.py @@ -0,0 +1,91 @@ +from collections import defaultdict +import os +import pytest +import re + +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_docs +from openpilot.selfdrive.car.docs_definitions import Cable, Column, PartType, Star +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.values import PLATFORMS +from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs +from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff + + +class TestCarDocs: + @classmethod + def setup_class(cls): + cls.all_cars = get_all_car_docs() + + def test_generator(self): + generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) + with open(CARS_MD_OUT) as f: + current_cars_md = f.read() + + assert generated_cars_md == current_cars_md, "Run selfdrive/car/docs.py to update the compatibility documentation" + + def test_docs_diff(self): + dump_path = os.path.join(BASEDIR, "selfdrive", "car", "tests", "cars_dump") + dump_car_docs(dump_path) + print_car_docs_diff(dump_path) + os.remove(dump_path) + + def test_duplicate_years(self, subtests): + make_model_years = defaultdict(list) + for car in self.all_cars: + with subtests.test(car_docs_name=car.name): + make_model = (car.make, car.model) + for year in car.year_list: + assert year not in make_model_years[make_model], f"{car.name}: Duplicate model year" + make_model_years[make_model].append(year) + + def test_missing_car_docs(self, subtests): + all_car_docs_platforms = [name for name, config in PLATFORMS.items()] + for platform in sorted(interfaces.keys()): + with subtests.test(platform=platform): + assert platform in all_car_docs_platforms, f"Platform: {platform} doesn't have a CarDocs entry" + + def test_naming_conventions(self, subtests): + # Asserts market-standard car naming conventions by brand + for car in self.all_cars: + with subtests.test(car=car.name): + tokens = car.model.lower().split(" ") + if car.car_name == "hyundai": + assert "phev" not in tokens, "Use `Plug-in Hybrid`" + assert "hev" not in tokens, "Use `Hybrid`" + if "plug-in hybrid" in car.model.lower(): + assert "Plug-in Hybrid" in car.model, "Use correct capitalization" + if car.make != "Kia": + assert "ev" not in tokens, "Use `Electric`" + elif car.car_name == "toyota": + if "rav4" in tokens: + assert "RAV4" in car.model, "Use correct capitalization" + + def test_torque_star(self, subtests): + # Asserts brand-specific assumptions around steering torque star + for car in self.all_cars: + with subtests.test(car=car.name): + # honda sanity check, it's the definition of a no torque star + if car.car_fingerprint in (HONDA.HONDA_ACCORD, HONDA.HONDA_CIVIC, HONDA.HONDA_CRV, HONDA.HONDA_ODYSSEY, HONDA.HONDA_PILOT): + assert car.row[Column.STEERING_TORQUE] == Star.EMPTY, f"{car.name} has full torque star" + elif car.car_name in ("toyota", "hyundai"): + assert car.row[Column.STEERING_TORQUE] != Star.EMPTY, f"{car.name} has no torque star" + + def test_year_format(self, subtests): + for car in self.all_cars: + with subtests.test(car=car.name): + assert re.search(r"\d{4}-\d{4}", car.name) is None, f"Format years correctly: {car.name}" + + def test_harnesses(self, subtests): + for car in self.all_cars: + with subtests.test(car=car.name): + if car.name == "comma body": + pytest.skip() + + car_part_type = [p.part_type for p in car.car_parts.all_parts()] + car_parts = list(car.car_parts.all_parts()) + assert len(car_parts) > 0, f"Need to specify car parts: {car.name}" + assert car_part_type.count(PartType.connector) == 1, f"Need to specify one harness connector: {car.name}" + assert car_part_type.count(PartType.mount) == 1, f"Need to specify one mount: {car.name}" + assert Cable.right_angle_obd_c_cable_1_5ft in car_parts, f"Need to specify a right angle OBD-C cable (1.5ft): {car.name}" diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py new file mode 100644 index 0000000000..f872972542 --- /dev/null +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -0,0 +1,315 @@ +import pytest +import random +import time +from collections import defaultdict +from parameterized import parameterized + +from cereal import car +from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.car.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.fw_versions import ESSENTIAL_ECUS, FW_QUERY_CONFIGS, FUZZY_EXCLUDE_ECUS, VERSIONS, build_fw_dict, \ + match_fw_to_car, get_brand_ecu_matches, get_fw_versions, get_present_ecus +from openpilot.selfdrive.car.vin import get_vin + +CarFw = car.CarParams.CarFw +Ecu = car.CarParams.Ecu + +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +class FakeSocket: + def receive(self, non_blocking=False): + pass + + def send(self, msg): + pass + + +class TestFwFingerprint: + def assertFingerprints(self, candidates, expected): + candidates = list(candidates) + assert len(candidates) == 1, f"got more than one candidate: {candidates}" + assert candidates[0] == expected + + @parameterized.expand([(b, c, e[c], n) for b, e in VERSIONS.items() for c in e for n in (True, False)]) + def test_exact_match(self, brand, car_model, ecus, test_non_essential): + config = FW_QUERY_CONFIGS[brand] + CP = car.CarParams.new_message() + for _ in range(100): + fw = [] + for ecu, fw_versions in ecus.items(): + # Assume non-essential ECUs apply to all cars, so we catch cases where Car A with + # missing ECUs won't match to Car B where only Car B has labeled non-essential ECUs + if ecu[0] in config.non_essential_ecus and test_non_essential: + continue + + ecu_name, addr, sub_addr = ecu + fw.append({"ecu": ecu_name, "fwVersion": random.choice(fw_versions), 'brand': brand, + "address": addr, "subAddress": 0 if sub_addr is None else sub_addr}) + CP.carFw = fw + _, matches = match_fw_to_car(CP.carFw, CP.carVin, allow_fuzzy=False) + if not test_non_essential: + self.assertFingerprints(matches, car_model) + else: + # if we're removing ECUs we expect some match loss, but it shouldn't mismatch + if len(matches) != 0: + self.assertFingerprints(matches, car_model) + + @parameterized.expand([(b, c, e[c]) for b, e in VERSIONS.items() for c in e]) + def test_custom_fuzzy_match(self, brand, car_model, ecus): + # Assert brand-specific fuzzy fingerprinting function doesn't disagree with standard fuzzy function + config = FW_QUERY_CONFIGS[brand] + if config.match_fw_to_car_fuzzy is None: + pytest.skip("Brand does not implement custom fuzzy fingerprinting function") + + CP = car.CarParams.new_message() + for _ in range(5): + fw = [] + for ecu, fw_versions in ecus.items(): + ecu_name, addr, sub_addr = ecu + fw.append({"ecu": ecu_name, "fwVersion": random.choice(fw_versions), 'brand': brand, + "address": addr, "subAddress": 0 if sub_addr is None else sub_addr}) + CP.carFw = fw + _, matches = match_fw_to_car(CP.carFw, CP.carVin, allow_exact=False, log=False) + brand_matches = config.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, VERSIONS[brand]) + + # If both have matches, they must agree + if len(matches) == 1 and len(brand_matches) == 1: + assert matches == brand_matches + + @parameterized.expand([(b, c, e[c]) for b, e in VERSIONS.items() for c in e]) + def test_fuzzy_match_ecu_count(self, brand, car_model, ecus): + # Asserts that fuzzy matching does not count matching FW, but ECU address keys + valid_ecus = [e for e in ecus if e[0] not in FUZZY_EXCLUDE_ECUS] + if not len(valid_ecus): + pytest.skip("Car model has no compatible ECUs for fuzzy matching") + + fw = [] + for ecu in valid_ecus: + ecu_name, addr, sub_addr = ecu + for _ in range(5): + # Add multiple FW versions to simulate ECU returning to multiple queries in a brand + fw.append({"ecu": ecu_name, "fwVersion": random.choice(ecus[ecu]), 'brand': brand, + "address": addr, "subAddress": 0 if sub_addr is None else sub_addr}) + CP = car.CarParams.new_message(carFw=fw) + _, matches = match_fw_to_car(CP.carFw, CP.carVin, allow_exact=False, log=False) + + # Assert no match if there are not enough unique ECUs + unique_ecus = {(f['address'], f['subAddress']) for f in fw} + if len(unique_ecus) < 2: + assert len(matches) == 0, car_model + # There won't always be a match due to shared FW, but if there is it should be correct + elif len(matches): + self.assertFingerprints(matches, car_model) + + def test_fw_version_lists(self, subtests): + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for ecu, ecu_fw in ecus.items(): + with subtests.test(ecu): + duplicates = {fw for fw in ecu_fw if ecu_fw.count(fw) > 1} + assert not len(duplicates), f'{car_model}: Duplicate FW versions: Ecu.{ECU_NAME[ecu[0]]}, {duplicates}' + assert len(ecu_fw) > 0, f'{car_model}: No FW versions: Ecu.{ECU_NAME[ecu[0]]}' + + def test_all_addrs_map_to_one_ecu(self): + for brand, cars in VERSIONS.items(): + addr_to_ecu = defaultdict(set) + for ecus in cars.values(): + for ecu_type, addr, sub_addr in ecus.keys(): + addr_to_ecu[(addr, sub_addr)].add(ecu_type) + ecus_for_addr = addr_to_ecu[(addr, sub_addr)] + ecu_strings = ", ".join([f'Ecu.{ECU_NAME[ecu]}' for ecu in ecus_for_addr]) + assert len(ecus_for_addr) <= 1, f"{brand} has multiple ECUs that map to one address: {ecu_strings} -> ({hex(addr)}, {sub_addr})" + + def test_data_collection_ecus(self, subtests): + # Asserts no extra ECUs are in the fingerprinting database + for brand, config in FW_QUERY_CONFIGS.items(): + for car_model, ecus in VERSIONS[brand].items(): + bad_ecus = set(ecus).intersection(config.extra_ecus) + with subtests.test(car_model=car_model.value): + assert not len(bad_ecus), f'{car_model}: Fingerprints contain ECUs added for data collection: {bad_ecus}' + + def test_blacklisted_ecus(self, subtests): + blacklisted_addrs = (0x7c4, 0x7d0) # includes A/C ecu and an unknown ecu + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + CP = interfaces[car_model][0].get_non_essential_params(car_model) + if CP.carName == 'subaru': + for ecu in ecus.keys(): + assert ecu[1] not in blacklisted_addrs, f'{car_model}: Blacklisted ecu: (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])})' + + elif CP.carName == "chrysler": + # Some HD trucks have a combined TCM and ECM + if CP.carFingerprint.startswith("RAM HD"): + for ecu in ecus.keys(): + assert ecu[0] != Ecu.transmission, f"{car_model}: Blacklisted ecu: (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])})" + + def test_non_essential_ecus(self, subtests): + for brand, config in FW_QUERY_CONFIGS.items(): + with subtests.test(brand): + # These ECUs are already not in ESSENTIAL_ECUS which the fingerprint functions give a pass if missing + unnecessary_non_essential_ecus = set(config.non_essential_ecus) - set(ESSENTIAL_ECUS) + assert unnecessary_non_essential_ecus == set(), "Declaring non-essential ECUs non-essential is not required: " + \ + f"{', '.join([f'Ecu.{ECU_NAME[ecu]}' for ecu in unnecessary_non_essential_ecus])}" + + def test_missing_versions_and_configs(self, subtests): + brand_versions = set(VERSIONS.keys()) + brand_configs = set(FW_QUERY_CONFIGS.keys()) + if len(brand_configs - brand_versions): + with subtests.test(): + pytest.fail(f"Brands do not implement FW_VERSIONS: {brand_configs - brand_versions}") + + if len(brand_versions - brand_configs): + with subtests.test(): + pytest.fail(f"Brands do not implement FW_QUERY_CONFIG: {brand_versions - brand_configs}") + + # Ensure each brand has at least 1 ECU to query, and extra ECU retrieval + for brand, config in FW_QUERY_CONFIGS.items(): + assert len(config.get_all_ecus({}, include_extra_ecus=False)) == 0 + assert config.get_all_ecus({}) == set(config.extra_ecus) + assert len(config.get_all_ecus(VERSIONS[brand])) > 0 + + def test_fw_request_ecu_whitelist(self, subtests): + for brand, config in FW_QUERY_CONFIGS.items(): + with subtests.test(brand=brand): + whitelisted_ecus = {ecu for r in config.requests for ecu in r.whitelist_ecus} + brand_ecus = {fw[0] for car_fw in VERSIONS[brand].values() for fw in car_fw} + brand_ecus |= {ecu[0] for ecu in config.extra_ecus} + + # each ecu in brand's fw versions + extra ecus needs to be whitelisted at least once + ecus_not_whitelisted = brand_ecus - whitelisted_ecus + + ecu_strings = ", ".join([f'Ecu.{ECU_NAME[ecu]}' for ecu in ecus_not_whitelisted]) + assert not (len(whitelisted_ecus) and len(ecus_not_whitelisted)), \ + f'{brand.title()}: ECUs not in any FW query whitelists: {ecu_strings}' + + def test_fw_requests(self, subtests): + # Asserts equal length request and response lists + for brand, config in FW_QUERY_CONFIGS.items(): + with subtests.test(brand=brand): + for request_obj in config.requests: + assert len(request_obj.request) == len(request_obj.response) + + # No request on the OBD port (bus 1, multiplexed) should be run on an aux panda + assert not (request_obj.auxiliary and request_obj.bus == 1 and request_obj.obd_multiplexing), \ + f"{brand.title()}: OBD multiplexed request is marked auxiliary: {request_obj}" + + def test_brand_ecu_matches(self): + empty_response = {brand: set() for brand in FW_QUERY_CONFIGS} + assert get_brand_ecu_matches(set()) == empty_response + + # we ignore bus + expected_response = empty_response | {'toyota': {(0x750, 0xf)}} + assert get_brand_ecu_matches({(0x758, 0xf, 99)}) == expected_response + + +class TestFwFingerprintTiming: + N: int = 5 + TOL: float = 0.05 + + # for patched functions + current_obd_multiplexing: bool + total_time: float + + def fake_set_obd_multiplexing(self, _, obd_multiplexing): + """The 10Hz blocking params loop adds on average 50ms to the query time for each OBD multiplexing change""" + if obd_multiplexing != self.current_obd_multiplexing: + self.current_obd_multiplexing = obd_multiplexing + self.total_time += 0.1 / 2 + + def fake_get_data(self, timeout): + self.total_time += timeout + return {} + + def _benchmark_brand(self, brand, num_pandas, mocker): + fake_socket = FakeSocket() + self.total_time = 0 + mocker.patch("openpilot.selfdrive.car.fw_versions.set_obd_multiplexing", self.fake_set_obd_multiplexing) + mocker.patch("openpilot.selfdrive.car.isotp_parallel_query.IsoTpParallelQuery.get_data", self.fake_get_data) + for _ in range(self.N): + # Treat each brand as the most likely (aka, the first) brand with OBD multiplexing initially on + self.current_obd_multiplexing = True + + t = time.perf_counter() + get_fw_versions(fake_socket, fake_socket, brand, num_pandas=num_pandas) + self.total_time += time.perf_counter() - t + + return self.total_time / self.N + + def _assert_timing(self, avg_time, ref_time): + assert avg_time < ref_time + self.TOL + assert avg_time > ref_time - self.TOL, "Performance seems to have improved, update test refs." + + def test_startup_timing(self, subtests, mocker): + # Tests worse-case VIN query time and typical present ECU query time + vin_ref_times = {'worst': 1.4, 'best': 0.7} # best assumes we go through all queries to get a match + present_ecu_ref_time = 0.45 + + def fake_get_ecu_addrs(*_, timeout): + self.total_time += timeout + return set() + + fake_socket = FakeSocket() + self.total_time = 0.0 + mocker.patch("openpilot.selfdrive.car.fw_versions.set_obd_multiplexing", self.fake_set_obd_multiplexing) + mocker.patch("openpilot.selfdrive.car.fw_versions.get_ecu_addrs", fake_get_ecu_addrs) + for _ in range(self.N): + self.current_obd_multiplexing = True + get_present_ecus(fake_socket, fake_socket, num_pandas=2) + self._assert_timing(self.total_time / self.N, present_ecu_ref_time) + print(f'get_present_ecus, query time={self.total_time / self.N} seconds') + + for name, args in (('worst', {}), ('best', {'retry': 1})): + with subtests.test(name=name): + self.total_time = 0.0 + mocker.patch("openpilot.selfdrive.car.isotp_parallel_query.IsoTpParallelQuery.get_data", self.fake_get_data) + for _ in range(self.N): + get_vin(fake_socket, fake_socket, (0, 1), **args) + self._assert_timing(self.total_time / self.N, vin_ref_times[name]) + print(f'get_vin {name} case, query time={self.total_time / self.N} seconds') + + def test_fw_query_timing(self, subtests, mocker): + total_ref_time = {1: 7.2, 2: 7.8} + brand_ref_times = { + 1: { + 'gm': 1.0, + 'body': 0.1, + 'chrysler': 0.3, + 'ford': 1.5, + 'honda': 0.45, + 'hyundai': 0.65, + 'mazda': 0.1, + 'nissan': 0.8, + 'subaru': 0.65, + 'tesla': 0.3, + 'toyota': 0.7, + 'volkswagen': 0.65, + }, + 2: { + 'ford': 1.6, + 'hyundai': 1.15, + 'tesla': 0.3, + } + } + + total_times = {1: 0.0, 2: 0.0} + for num_pandas in (1, 2): + for brand, config in FW_QUERY_CONFIGS.items(): + with subtests.test(brand=brand, num_pandas=num_pandas): + avg_time = self._benchmark_brand(brand, num_pandas, mocker) + total_times[num_pandas] += avg_time + avg_time = round(avg_time, 2) + + ref_time = brand_ref_times[num_pandas].get(brand) + if ref_time is None: + # ref time should be same as 1 panda if no aux queries + ref_time = brand_ref_times[num_pandas - 1][brand] + + self._assert_timing(avg_time, ref_time) + print(f'{brand=}, {num_pandas=}, {len(config.requests)=}, avg FW query time={avg_time} seconds') + + for num_pandas in (1, 2): + with subtests.test(brand='all_brands', num_pandas=num_pandas): + total_time = round(total_times[num_pandas], 2) + self._assert_timing(total_time, total_ref_time[num_pandas]) + print(f'all brands, total FW query time={total_time} seconds') diff --git a/selfdrive/car/tests/test_lateral_limits.py b/selfdrive/car/tests/test_lateral_limits.py new file mode 100755 index 0000000000..e61d197f4b --- /dev/null +++ b/selfdrive/car/tests/test_lateral_limits.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python3 +from collections import defaultdict +import importlib +from parameterized import parameterized_class +import pytest +import sys + +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.car.fingerprints import all_known_cars +from openpilot.selfdrive.car.interfaces import get_torque_params + +CAR_MODELS = all_known_cars() + +# ISO 11270 - allowed up jerk is strictly lower than recommended limits +MAX_LAT_ACCEL = 3.0 # m/s^2 +MAX_LAT_JERK_UP = 2.5 # m/s^3 +MAX_LAT_JERK_DOWN = 5.0 # m/s^3 +MAX_LAT_JERK_UP_TOLERANCE = 0.5 # m/s^3 + +# jerk is measured over half a second +JERK_MEAS_T = 0.5 + + +@parameterized_class('car_model', [(c,) for c in sorted(CAR_MODELS)]) +class TestLateralLimits: + car_model: str + + @classmethod + def setup_class(cls): + CarInterface, _, _ = interfaces[cls.car_model] + CP = CarInterface.get_non_essential_params(cls.car_model) + + if CP.dashcamOnly: + pytest.skip("Platform is behind dashcamOnly") + + # TODO: test all platforms + if CP.lateralTuning.which() != 'torque': + pytest.skip() + + if CP.notCar: + pytest.skip() + + CarControllerParams = importlib.import_module(f'selfdrive.car.{CP.carName}.values').CarControllerParams + cls.control_params = CarControllerParams(CP) + cls.torque_params = get_torque_params()[cls.car_model] + + @staticmethod + def calculate_0_5s_jerk(control_params, torque_params): + steer_step = control_params.STEER_STEP + max_lat_accel = torque_params['MAX_LAT_ACCEL_MEASURED'] + + # Steer up/down delta per 10ms frame, in percentage of max torque + steer_up_per_frame = control_params.STEER_DELTA_UP / control_params.STEER_MAX / steer_step + steer_down_per_frame = control_params.STEER_DELTA_DOWN / control_params.STEER_MAX / steer_step + + # Lateral acceleration reached in 0.5 seconds, clipping to max torque + accel_up_0_5_sec = min(steer_up_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel + accel_down_0_5_sec = min(steer_down_per_frame * JERK_MEAS_T / DT_CTRL, 1.0) * max_lat_accel + + # Convert to m/s^3 + return accel_up_0_5_sec / JERK_MEAS_T, accel_down_0_5_sec / JERK_MEAS_T + + def test_jerk_limits(self): + up_jerk, down_jerk = self.calculate_0_5s_jerk(self.control_params, self.torque_params) + assert up_jerk <= MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE + assert down_jerk <= MAX_LAT_JERK_DOWN + + def test_max_lateral_accel(self): + assert self.torque_params["MAX_LAT_ACCEL_MEASURED"] <= MAX_LAT_ACCEL + + +class LatAccelReport: + car_model_jerks: defaultdict[str, dict[str, float]] = defaultdict(dict) + + def pytest_sessionfinish(self): + print(f"\n\n---- Lateral limit report ({len(CAR_MODELS)} cars) ----\n") + + max_car_model_len = max([len(car_model) for car_model in self.car_model_jerks]) + for car_model, _jerks in sorted(self.car_model_jerks.items(), key=lambda i: i[1]['up_jerk'], reverse=True): + violation = _jerks["up_jerk"] > MAX_LAT_JERK_UP + MAX_LAT_JERK_UP_TOLERANCE or \ + _jerks["down_jerk"] > MAX_LAT_JERK_DOWN + violation_str = " - VIOLATION" if violation else "" + + print(f"{car_model:{max_car_model_len}} - up jerk: {round(_jerks['up_jerk'], 2):5} " + + f"m/s^3, down jerk: {round(_jerks['down_jerk'], 2):5} m/s^3{violation_str}") + + @pytest.fixture(scope="class", autouse=True) + def class_setup(self, request): + yield + cls = request.cls + if hasattr(cls, "control_params"): + up_jerk, down_jerk = TestLateralLimits.calculate_0_5s_jerk(cls.control_params, cls.torque_params) + self.car_model_jerks[cls.car_model] = {"up_jerk": up_jerk, "down_jerk": down_jerk} + + +if __name__ == '__main__': + sys.exit(pytest.main([__file__, '-n0', '--no-summary'], plugins=[LatAccelReport()])) # noqa: TID251 diff --git a/selfdrive/car/tests/test_platform_configs.py b/selfdrive/car/tests/test_platform_configs.py new file mode 100644 index 0000000000..31e4be4fe6 --- /dev/null +++ b/selfdrive/car/tests/test_platform_configs.py @@ -0,0 +1,17 @@ +from openpilot.selfdrive.car.values import PLATFORMS + + +class TestPlatformConfigs: + def test_configs(self, subtests): + + for name, platform in PLATFORMS.items(): + with subtests.test(platform=str(platform)): + assert platform.config._frozen + + if platform != "MOCK": + assert "pt" in platform.config.dbc_dict + assert len(platform.config.platform_str) > 0 + + assert name == platform.config.platform_str + + assert platform.config.specs is not None diff --git a/selfdrive/car/torque_data/neural_ff_weights.json b/selfdrive/car/torque_data/neural_ff_weights.json index c526f07fa2..251b66efb0 100644 --- a/selfdrive/car/torque_data/neural_ff_weights.json +++ b/selfdrive/car/torque_data/neural_ff_weights.json @@ -1 +1 @@ -{"CHEVROLET BOLT EUV 2022": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.043946366757154465], [-0.10138195008039474, -0.040128443390131, -0.08937158435583115, -0.0048376512713730335], [-0.0028251828625798225, -0.04743027314543724, 0.06340016424655914, 0.07277824729681015], [0.49482327699661255, -0.06410001963376999, -0.0999293103814125, -0.14250673353672028], [0.042802367359399796, 0.0015462725423276424, -0.05991362780332565, 0.1022040992975235], [0.3523194193840027, 0.07343732565641403, 0.04157765582203865, -0.12358107417821884]], "b_3": [0.2653026282787323, -0.058485131710767746, -0.0744510293006897, 0.012550175189971924], "w_4": [[0.5988775491714478, 0.09668736904859543], [-0.04360569268465042, 0.06491032242774963], [-0.11868984252214432, -0.09601487964391708], [-0.06554870307445526, -0.14189276099205017]], "b_4": [-0.08148707449436188, -2.8251802921295166], "input_norm_mat": [[-3.0, 3.0], [-3.0, 3.0], [0.0, 40.0], [-3.0, 3.0]], "output_norm_mat": [-1.0, 1.0], "temperature": 100.0}} \ No newline at end of file +{"CHEVROLET_BOLT_EUV": {"w_1": [[0.3452189564704895, -0.15614677965641022, -0.04062516987323761, -0.5960758328437805, 0.3211185932159424, 0.31732726097106934, -0.04430829733610153, -0.37327295541763306, -0.14118380844593048, 0.12712529301643372, 0.2641555070877075, -0.3451094627380371, -0.005127656273543835, 0.6185108423233032, 0.03725295141339302, 0.3763789236545563], [-0.0708412230014801, 0.3667356073856354, 0.031383827328681946, 0.1740853488445282, -0.04695861041545868, 0.018055908381938934, 0.009072160348296165, -0.23640218377113342, -0.10362917929887772, 0.022628149017691612, -0.224413201212883, 0.20718418061733246, -0.016947750002145767, -0.3872031271457672, -0.15500062704086304, -0.06375953555107117], [-0.0838046595454216, -0.0242826659232378, -0.07765661180019379, 0.028858814388513565, -0.09516210108995438, 0.008368706330657005, 0.1689300835132599, 0.015036891214549541, -0.15121428668498993, 0.1388195902109146, 0.11486363410949707, 0.0651545450091362, 0.13559958338737488, 0.04300367832183838, -0.13856294751167297, -0.058136988431215286], [-0.006249868310987949, 0.08809533715248108, -0.040690965950489044, 0.02359287068247795, -0.00766348373144865, 0.24816390872001648, -0.17360293865203857, -0.03676899895071983, -0.17564819753170013, 0.18998438119888306, -0.050583917647600174, -0.006488069426268339, 0.10649101436138153, -0.024557121098041534, -0.103276826441288, 0.18448011577129364]], "b_1": [0.2935388386249542, 0.10967712104320526, -0.014007942751049995, 0.211833655834198, 0.33605605363845825, 0.37722209095954895, -0.16615016758441925, 0.3134673535823822, 0.06695777177810669, 0.3425212800502777, 0.3769673705101013, 0.23186539113521576, 0.5770409107208252, -0.05929069593548775, 0.01839117519557476, 0.03828774020075798], "w_2": [[-0.06261160969734192, 0.010185074992477894, -0.06083013117313385, -0.04531499370932579, -0.08979734033346176, 0.3432150185108185, -0.019801849499344826, 0.3010321259498596], [0.19698476791381836, -0.009238275699317455, 0.08842222392559052, -0.09516377002000809, -0.05022778362035751, 0.13626104593276978, -0.052890390157699585, 0.15569131076335907], [0.0724768117070198, -0.09018408507108688, 0.06850195676088333, -0.025572121143341064, 0.0680626779794693, -0.07648195326328278, 0.07993496209383011, -0.059752143919467926], [1.267876386642456, -0.05755887180566788, -0.08429178595542908, 0.021366603672504425, -0.0006479775765910745, -1.4292563199996948, -0.08077696710824966, -1.414825439453125], [0.04535430669784546, 0.06555880606174469, -0.027145234867930412, -0.07661093026399612, -0.05702832341194153, 0.23650476336479187, 0.0024587824009358883, 0.20126521587371826], [0.006042032968252897, 0.042880818247795105, 0.002187949838116765, -0.017126334831118584, -0.08352015167474747, 0.19801731407642365, -0.029196614399552345, 0.23713473975658417], [-0.01644900068640709, -0.04358499124646187, 0.014584392309188843, 0.07155826687812805, -0.09354910999536514, -0.033351872116327286, 0.07138452678918839, -0.04755295440554619], [-1.1012420654296875, -0.03534531593322754, 0.02167935110628605, -0.01116552110761404, -0.08436500281095505, 1.1038788557052612, 0.027903547510504723, 1.0676132440567017], [0.03843916580080986, -0.0952216386795044, 0.039226632565259933, 0.002778085647150874, -0.020275786519050598, -0.07848760485649109, 0.04803166165947914, 0.015538203530013561], [0.018385495990514755, -0.025189843028783798, 0.0036680365446954966, -0.02105865254998207, 0.04808586835861206, 0.1575016975402832, 0.02703506126999855, 0.23039312660694122], [-0.0033881019335240126, -0.10210853815078735, -0.04877309128642082, 0.006989633198827505, 0.046798162162303925, 0.38676899671554565, -0.032304272055625916, 0.2345031052827835], [0.22092825174331665, -0.09642873704433441, 0.04499409720301628, 0.05108088254928589, -0.10191166400909424, 0.12818090617656708, -0.021021494641900063, 0.09440375864505768], [0.1212429478764534, -0.028194155544042587, -0.0981956496834755, 0.08226924389600754, 0.055346839129924774, 0.27067816257476807, -0.09064067900180817, 0.12580905854701996], [-1.6740131378173828, -0.02066155895590782, -0.05924689769744873, 0.06347910314798355, -0.07821853458881378, 1.2807466983795166, 0.04589352011680603, 1.310766577720642], [-0.09893272817134857, -0.04093599319458008, -0.02502273954451084, 0.09490344673395157, -0.0211324505507946, -0.09021010994911194, 0.07936318963766098, -0.03593116253614426], [-0.08490308374166489, -0.015558987855911255, -0.048692114651203156, -0.007421435788273811, -0.040531404316425323, 0.25889304280281067, 0.06012800335884094, 0.27946868538856506]], "b_2": [0.07973937690258026, -0.010446485131978989, -0.003066520905122161, -0.031895797699689865, 0.006032303906977177, 0.24106740951538086, -0.008969511836767197, 0.2872662842273712], "w_3": [[-1.364486813545227, -0.11682678014039993, 0.01764785870909691, 0.03926877677440643], [-0.05695437639951706, 0.05472218990325928, 0.1266128271818161, 0.09950875490903854], [0.11415273696184158, -0.10069356113672256, 0.0864749327301979, -0.043946366757154465], [-0.10138195008039474, -0.040128443390131, -0.08937158435583115, -0.0048376512713730335], [-0.0028251828625798225, -0.04743027314543724, 0.06340016424655914, 0.07277824729681015], [0.49482327699661255, -0.06410001963376999, -0.0999293103814125, -0.14250673353672028], [0.042802367359399796, 0.0015462725423276424, -0.05991362780332565, 0.1022040992975235], [0.3523194193840027, 0.07343732565641403, 0.04157765582203865, -0.12358107417821884]], "b_3": [0.2653026282787323, -0.058485131710767746, -0.0744510293006897, 0.012550175189971924], "w_4": [[0.5988775491714478, 0.09668736904859543], [-0.04360569268465042, 0.06491032242774963], [-0.11868984252214432, -0.09601487964391708], [-0.06554870307445526, -0.14189276099205017]], "b_4": [-0.08148707449436188, -2.8251802921295166], "input_norm_mat": [[-3.0, 3.0], [-3.0, 3.0], [0.0, 40.0], [-3.0, 3.0]], "output_norm_mat": [-1.0, 1.0], "temperature": 100.0}} \ No newline at end of file diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index 339fc533e5..993eb3fb3c 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -1,74 +1,77 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] ### angle control # Nissan appears to have torque -"NISSAN X-TRAIL 2017" = [nan, 1.5, nan] -"NISSAN ALTIMA 2020" = [nan, 1.5, nan] -"NISSAN LEAF 2018 Instrument Cluster" = [nan, 1.5, nan] -"NISSAN LEAF 2018" = [nan, 1.5, nan] -"NISSAN ROGUE 2019" = [nan, 1.5, nan] +"NISSAN_XTRAIL" = [nan, 1.5, nan] +"NISSAN_ALTIMA" = [nan, 1.5, nan] +"NISSAN_LEAF_IC" = [nan, 1.5, nan] +"NISSAN_LEAF" = [nan, 1.5, nan] +"NISSAN_ROGUE" = [nan, 1.5, nan] # New subarus angle based controllers -"SUBARU FORESTER 2022" = [nan, 3.0, nan] -"SUBARU OUTBACK 7TH GEN" = [nan, 3.0, nan] -"SUBARU ASCENT 2023" = [nan, 3.0, nan] +"SUBARU_FORESTER_2022" = [nan, 3.0, nan] +"SUBARU_OUTBACK_2023" = [nan, 3.0, nan] +"SUBARU_ASCENT_2023" = [nan, 3.0, nan] # Toyota LTA also has torque -"TOYOTA RAV4 2023" = [nan, 3.0, nan] +"TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan] # Tesla has high torque -"TESLA AP1 MODEL S" = [nan, 2.5, nan] -"TESLA AP2 MODEL S" = [nan, 2.5, nan] +"TESLA_AP1_MODELS" = [nan, 2.5, nan] +"TESLA_AP2_MODELS" = [nan, 2.5, nan] +"TESLA_MODELS_RAVEN" = [nan, 2.5, nan] # Guess -"FORD BRONCO SPORT 1ST GEN" = [nan, 1.5, nan] -"FORD ESCAPE 4TH GEN" = [nan, 1.5, nan] -"FORD EXPLORER 6TH GEN" = [nan, 1.5, nan] -"FORD F-150 14TH GEN" = [nan, 1.5, nan] -"FORD FOCUS 4TH GEN" = [nan, 1.5, nan] -"FORD MAVERICK 1ST GEN" = [nan, 1.5, nan] -"FORD F-150 LIGHTNING 1ST GEN" = [nan, 1.5, nan] -"FORD MUSTANG MACH-E 1ST GEN" = [nan, 1.5, nan] +"FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan] +"FORD_ESCAPE_MK4" = [nan, 1.5, nan] +"FORD_EXPLORER_MK6" = [nan, 1.5, nan] +"FORD_F_150_MK14" = [nan, 1.5, nan] +"FORD_FOCUS_MK4" = [nan, 1.5, nan] +"FORD_MAVERICK_MK1" = [nan, 1.5, nan] +"FORD_F_150_LIGHTNING_MK1" = [nan, 1.5, nan] +"FORD_MUSTANG_MACH_E_MK1" = [nan, 1.5, nan] +"FORD_RANGER_MK2" = [nan, 1.5, nan] ### # No steering wheel -"COMMA BODY" = [nan, 1000, nan] +"COMMA_BODY" = [nan, 1000, nan] # Totally new cars -"RAM 1500 5TH GEN" = [2.0, 2.0, 0.05] -"RAM HD 5TH GEN" = [1.4, 1.4, 0.05] -"SUBARU OUTBACK 6TH GEN" = [2.0, 2.0, 0.2] -"CADILLAC ESCALADE 2017" = [1.899999976158142, 1.842270016670227, 0.1120000034570694] -"CADILLAC ESCALADE ESV 2019" = [1.15, 1.3, 0.2] -"CHEVROLET BOLT EUV 2022" = [2.0, 2.0, 0.05] -"CHEVROLET SILVERADO 1500 2020" = [1.9, 1.9, 0.112] -"CHEVROLET TRAILBLAZER 2021" = [1.33, 1.9, 0.16] -"CHEVROLET EQUINOX 2019" = [2.5, 2.5, 0.05] -"VOLKSWAGEN PASSAT NMS" = [2.5, 2.5, 0.1] -"VOLKSWAGEN SHARAN 2ND GEN" = [2.5, 2.5, 0.1] -"HYUNDAI SANTA CRUZ 1ST GEN" = [2.7, 2.7, 0.1] -"KIA SPORTAGE 5TH GEN" = [2.6, 2.6, 0.1] -"GENESIS GV70 1ST GEN" = [2.42, 2.42, 0.1] -"GENESIS GV60 ELECTRIC 1ST GEN" = [2.5, 2.5, 0.1] -"KIA SORENTO 4TH GEN" = [2.5, 2.5, 0.1] -"KIA SORENTO HYBRID 4TH GEN" = [2.5, 2.5, 0.1] -"KIA NIRO HYBRID 2ND GEN" = [2.42, 2.5, 0.12] -"KIA NIRO EV 2ND GEN" = [2.05, 2.5, 0.14] -"GENESIS GV80 2023" = [2.5, 2.5, 0.1] -"KIA CARNIVAL 4TH GEN" = [1.75, 1.75, 0.15] -"GMC ACADIA DENALI 2018" = [1.6, 1.6, 0.2] -"LEXUS IS 2023" = [2.0, 2.0, 0.1] -"HYUNDAI KONA ELECTRIC 2ND GEN" = [2.5, 2.5, 0.1] -"HYUNDAI IONIQ 6 2023" = [2.5, 2.5, 0.1] -"HYUNDAI AZERA 6TH GEN" = [1.8, 1.8, 0.1] -"HYUNDAI AZERA HYBRID 6TH GEN" = [1.8, 1.8, 0.1] -"KIA K8 HYBRID 1ST GEN" = [2.5, 2.5, 0.1] -"HYUNDAI CUSTIN 1ST GEN" = [2.5, 2.5, 0.1] -"LEXUS GS F 2016" = [2.5, 2.5, 0.08] -"HYUNDAI STARIA 4TH GEN" = [1.8, 2.0, 0.15] +"RAM_1500_5TH_GEN" = [2.0, 2.0, 0.05] +"RAM_HD_5TH_GEN" = [1.4, 1.4, 0.05] +"SUBARU_OUTBACK" = [2.0, 1.5, 0.2] +"CADILLAC_ESCALADE" = [1.899999976158142, 1.842270016670227, 0.1120000034570694] +"CADILLAC_ESCALADE_ESV_2019" = [1.15, 1.3, 0.2] +"CHEVROLET_BOLT_EUV" = [2.0, 2.0, 0.05] +"CHEVROLET_SILVERADO" = [1.9, 1.9, 0.112] +"CHEVROLET_TRAILBLAZER" = [1.33, 1.9, 0.16] +"CHEVROLET_EQUINOX" = [2.5, 2.5, 0.05] +"VOLKSWAGEN_CADDY_MK3" = [1.2, 1.2, 0.1] +"VOLKSWAGEN_PASSAT_NMS" = [2.5, 2.5, 0.1] +"VOLKSWAGEN_SHARAN_MK2" = [2.5, 2.5, 0.1] +"HYUNDAI_SANTA_CRUZ_1ST_GEN" = [2.7, 2.7, 0.1] +"KIA_SPORTAGE_5TH_GEN" = [2.6, 2.6, 0.1] +"GENESIS_GV70_1ST_GEN" = [2.42, 2.42, 0.1] +"GENESIS_GV60_EV_1ST_GEN" = [2.5, 2.5, 0.1] +"KIA_SORENTO_4TH_GEN" = [2.5, 2.5, 0.1] +"KIA_SORENTO_HEV_4TH_GEN" = [2.5, 2.5, 0.1] +"KIA_NIRO_HEV_2ND_GEN" = [2.42, 2.5, 0.12] +"KIA_NIRO_EV_2ND_GEN" = [2.05, 2.5, 0.14] +"GENESIS_GV80" = [2.5, 2.5, 0.1] +"KIA_CARNIVAL_4TH_GEN" = [1.75, 1.75, 0.15] +"GMC_ACADIA" = [1.6, 1.6, 0.2] +"LEXUS_IS_TSS2" = [2.0, 2.0, 0.1] +"HYUNDAI_KONA_EV_2ND_GEN" = [2.5, 2.5, 0.1] +"HYUNDAI_IONIQ_6" = [2.5, 2.5, 0.005] +"HYUNDAI_AZERA_6TH_GEN" = [1.8, 1.8, 0.1] +"HYUNDAI_AZERA_HEV_6TH_GEN" = [1.8, 1.8, 0.1] +"KIA_K8_HEV_1ST_GEN" = [2.5, 2.5, 0.1] +"HYUNDAI_CUSTIN_1ST_GEN" = [2.5, 2.5, 0.1] +"LEXUS_GS_F" = [2.5, 2.5, 0.08] +"HYUNDAI_STARIA_4TH_GEN" = [1.8, 2.0, 0.15] # Dashcam or fallback configured as ideal car -"mock" = [10.0, 10, 0.0] +"MOCK" = [10.0, 10, 0.0] # Manually checked -"HONDA CIVIC 2022" = [2.5, 1.2, 0.15] -"HONDA HR-V 2023" = [2.5, 1.2, 0.2] +"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15] +"HONDA_HRV_3G" = [2.5, 1.2, 0.2] diff --git a/selfdrive/car/torque_data/params.toml b/selfdrive/car/torque_data/params.toml index 568646c84c..34cfd0e066 100644 --- a/selfdrive/car/torque_data/params.toml +++ b/selfdrive/car/torque_data/params.toml @@ -1,86 +1,82 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] -"ACURA ILX 2016" = [1.524988973896102, 0.519011053086259, 0.34236219253028] -"ACURA RDX 2018" = [0.9987728568686902, 0.5323765166196301, 0.303218805715844] -"ACURA RDX 2020" = [1.4314459806646749, 0.33874701282109954, 0.18048847083897598] -"AUDI A3 3RD GEN" = [1.5122414863077502, 1.7443517531719404, 0.15194151892450905] -"AUDI Q3 2ND GEN" = [1.4439223359448605, 1.2254955789112076, 0.1413798895978097] -"CHEVROLET VOLT PREMIER 2017" = [1.5961527626411784, 1.8422651988094612, 0.1572393918005158] -"CHRYSLER PACIFICA 2018" = [2.07140, 1.3366521181047952, 0.13776367250652022] -"CHRYSLER PACIFICA 2020" = [1.86206, 1.509076559398423, 0.14328246159386085] -"CHRYSLER PACIFICA HYBRID 2017" = [1.79422, 1.06831764583744, 0.116237] -"CHRYSLER PACIFICA HYBRID 2018" = [2.08887, 1.2943025830995154, 0.114818] -"CHRYSLER PACIFICA HYBRID 2019" = [1.90120, 1.1958788168371808, 0.131520] -"GENESIS G70 2018" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221] -"HONDA ACCORD 2018" = [1.7135052593468778, 0.3461280068322071, 0.21579936052863807] -"HONDA ACCORD HYBRID 2018" = [1.6651615004829625, 0.30322180951193245, 0.2083000440586149] -"HONDA CIVIC (BOSCH) 2019" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094] -"HONDA CIVIC 2016" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544] -"HONDA CR-V 2016" = [0.7667141440182675, 0.5927571534745969, 0.40909087636157127] -"HONDA CR-V 2017" = [2.01323205142022, 0.2700612209345081, 0.2238412881331528] -"HONDA CR-V HYBRID 2019" = [2.072034634644233, 0.7152085160516978, 0.20237105008376083] -"HONDA FIT 2018" = [1.5719981427109775, 0.5712761407108976, 0.110773383324281] -"HONDA HRV 2019" = [2.0661212805710205, 0.7521343418694775, 0.17760375789242094] -"HONDA INSIGHT 2019" = [1.5201671214069354, 0.5660229120683284, 0.25808042580281876] -"HONDA ODYSSEY 2018" = [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] -"HONDA PILOT 2017" = [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] -"HONDA RIDGELINE 2017" = [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] -"HYUNDAI ELANTRA 2021" = [3.169, 2.1259108157250735, 0.0819] -"HYUNDAI GENESIS 2015-2016" = [2.7807965280270794, 2.325, 0.0984484465421171] -"HYUNDAI IONIQ 5 2022" = [3.172929, 2.713050, 0.096019] -"HYUNDAI IONIQ ELECTRIC LIMITED 2019" = [1.7662975472852054, 1.613755614526594, 0.17087579756306276] -"HYUNDAI IONIQ PHEV 2020" = [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] -"HYUNDAI IONIQ PLUG-IN HYBRID 2019" = [2.970807902012267, 1.6312321830002083, 0.1088964990357482] -"HYUNDAI KONA ELECTRIC 2019" = [3.078814714619148, 2.307336938253934, 0.12359762054065548] -"HYUNDAI PALISADE 2020" = [2.544642494803999, 1.8721703683337008, 0.1301424599248651] -"HYUNDAI SANTA FE 2019" = [3.0787027729757632, 2.6173437483495565, 0.1207019341823945] -"HYUNDAI SANTA FE HYBRID 2022" = [3.501877602644835, 2.729064118456137, 0.10384068104538963] -"HYUNDAI SANTA FE PlUG-IN HYBRID 2022" = [1.6953050513611045, 1.5837614296206861, 0.12672855941458458] -"HYUNDAI SONATA 2019" = [2.2200457811703953, 1.2967330275895228, 0.14039920986586393] -"HYUNDAI SONATA 2020" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] -"HYUNDAI SONATA HYBRID 2021" = [2.8990264092395734, 2.061410192222139, 0.0899805488717382] -"HYUNDAI TUCSON 4TH GEN" = [2.960174, 2.860284, 0.108745] -"JEEP GRAND CHEROKEE 2019" = [2.30972, 1.289689569171081, 0.117048] -"JEEP GRAND CHEROKEE V6 2018" = [2.27116, 1.4057367824262523, 0.11725947414922003] -"KIA EV6 2022" = [3.2, 2.093457, 0.05] -"KIA K5 2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716] -"KIA NIRO EV 2020" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] -"KIA SORENTO GT LINE 2018" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558] -"KIA STINGER GT2 2018" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202] -"LEXUS ES 2019" = [2.0357564999999997, 1.999082295195227, 0.101533] -"LEXUS NX 2018" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927] -"LEXUS NX 2020" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067] -"LEXUS RX 2016" = [1.6430539050086406, 1.181960058934143, 0.19768806040843034] -"LEXUS RX 2020" = [1.5375561442049257, 1.343166476215164, 0.1931062001527557] -"MAZDA CX-9 2021" = [1.7601682915983443, 1.0889677335154337, 0.17713792194297195] -"SKODA SUPERB 3RD GEN" = [1.166437404652981, 1.1686163012668165, 0.12194533036948708] -"SUBARU FORESTER 2019" = [3.6617001649776793, 2.342197172531713, 0.11075960785398745] -"SUBARU IMPREZA LIMITED 2019" = [1.0670704910352047, 0.8234374840709592, 0.20986563268614938] -"SUBARU IMPREZA SPORT 2020" = [2.6068223389108303, 2.134872342760203, 0.15261513193561627] -"TOYOTA AVALON 2016" = [2.5185770183845646, 1.7153346784214922, 0.10603968787111022] -"TOYOTA AVALON 2019" = [1.7036141952825095, 1.239619084240008, 0.08459830394899492] -"TOYOTA AVALON 2022" = [2.3154403649717357, 2.7777922854327124, 0.11453999639164605] -"TOYOTA C-HR 2018" = [1.5591084333664578, 1.271271459066948, 0.20259087058453193] -"TOYOTA C-HR 2021" = [1.7678810166088303, 1.3742176337919942, 0.2319674583741509] -"TOYOTA CAMRY 2018" = [2.0568162685952505, 1.7576185169559122, 0.108878753] -"TOYOTA CAMRY 2021" = [2.3548324999999997, 2.368900128946771, 0.118436] -"TOYOTA COROLLA 2017" = [3.117154369115421, 1.8438132575043773, 0.12289685869250652] -"TOYOTA COROLLA TSS2 2019" = [1.991132339206426, 1.868866242720403, 0.19570063298031432] -"TOYOTA HIGHLANDER 2017" = [1.8108348718624456, 1.6348421600679828, 0.15972686105120398] -"TOYOTA HIGHLANDER 2020" = [1.9617570834136164, 1.8611643317268927, 0.14519673256119725] -"TOYOTA MIRAI 2021" = [2.506899832157829, 1.7417213930750164, 0.20182618449440565] -"TOYOTA PRIUS 2017" = [1.60, 1.5023147650693636, 0.151515] -"TOYOTA PRIUS TSS2 2021" = [1.972600, 1.9104337425537743, 0.170968] -"TOYOTA RAV4 2017" = [2.085695074355425, 2.2142832316984733, 0.13339165270103975] -"TOYOTA RAV4 2019" = [2.279239424615458, 2.087101966779332, 0.13682208413446817] -"TOYOTA RAV4 2019 8965" = [2.3080951748210854, 2.1189367835820603, 0.12942102328134028] -"TOYOTA RAV4 2019 x02" = [2.762293266024922, 2.243615865975329, 0.11113568178327986] -"TOYOTA RAV4 HYBRID 2017" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] -"TOYOTA RAV4 2022" = [2.241883248393209, 1.9304407208090029, 0.112174] -"TOYOTA RAV4 2022 x02" = [3.044930631831037, 2.3979189796380918, 0.14023209146703736] -"TOYOTA SIENNA 2018" = [1.689726, 1.3208264576110418, 0.140456] -"VOLKSWAGEN ARTEON 1ST GEN" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032] -"VOLKSWAGEN ATLAS 1ST GEN" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] -"VOLKSWAGEN GOLF 7TH GEN" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586] -"VOLKSWAGEN JETTA 7TH GEN" = [1.2271623034089392, 1.216955117387, 0.19437384688370712] -"VOLKSWAGEN PASSAT 8TH GEN" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647] -"VOLKSWAGEN TIGUAN 2ND GEN" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916] +"ACURA_ILX" = [1.524988973896102, 0.519011053086259, 0.34236219253028] +"ACURA_RDX" = [0.9987728568686902, 0.5323765166196301, 0.303218805715844] +"ACURA_RDX_3G" = [1.4314459806646749, 0.33874701282109954, 0.18048847083897598] +"AUDI_A3_MK3" = [1.5122414863077502, 1.7443517531719404, 0.15194151892450905] +"AUDI_Q3_MK2" = [1.4439223359448605, 1.2254955789112076, 0.1413798895978097] +"CHEVROLET_VOLT" = [1.5961527626411784, 1.8422651988094612, 0.1572393918005158] +"CHRYSLER_PACIFICA_2018" = [2.07140, 1.3366521181047952, 0.13776367250652022] +"CHRYSLER_PACIFICA_2020" = [1.86206, 1.509076559398423, 0.14328246159386085] +"CHRYSLER_PACIFICA_2017_HYBRID" = [1.79422, 1.06831764583744, 0.116237] +"CHRYSLER_PACIFICA_2018_HYBRID" = [2.08887, 1.2943025830995154, 0.114818] +"CHRYSLER_PACIFICA_2019_HYBRID" = [1.90120, 1.1958788168371808, 0.131520] +"GENESIS_G70" = [3.8520195946707947, 2.354697063349854, 0.06830285485626221] +"HONDA_ACCORD" = [1.6893333799149202, 0.3246749081720698, 0.2120497022936265] +"HONDA_CIVIC_BOSCH" = [1.691708637466905, 0.40132900729454185, 0.25460295304024094] +"HONDA_CIVIC" = [1.6528895627785531, 0.4018518740819229, 0.25458812851328544] +"HONDA_CRV" = [0.7667141440182675, 0.5927571534745969, 0.40909087636157127] +"HONDA_CRV_5G" = [2.01323205142022, 0.2700612209345081, 0.2238412881331528] +"HONDA_CRV_HYBRID" = [2.072034634644233, 0.7152085160516978, 0.20237105008376083] +"HONDA_FIT" = [1.5719981427109775, 0.5712761407108976, 0.110773383324281] +"HONDA_HRV" = [2.0661212805710205, 0.7521343418694775, 0.17760375789242094] +"HONDA_INSIGHT" = [1.5201671214069354, 0.5660229120683284, 0.25808042580281876] +"HONDA_ODYSSEY" = [1.8774809275211801, 0.8394431662987996, 0.2096978613792822] +"HONDA_PILOT" = [1.7262026201812795, 0.9470005614967523, 0.21351430733218763] +"HONDA_RIDGELINE" = [1.4146525028237624, 0.7356572861629564, 0.23307177552211328] +"HYUNDAI_ELANTRA_2021" = [3.169, 2.1259108157250735, 0.0819] +"HYUNDAI_GENESIS" = [2.7807965280270794, 2.325, 0.0984484465421171] +"HYUNDAI_IONIQ_5" = [3.172929, 2.713050, 0.096019] +"HYUNDAI_IONIQ_EV_LTD" = [1.7662975472852054, 1.613755614526594, 0.17087579756306276] +"HYUNDAI_IONIQ_PHEV" = [3.2928700076638537, 2.1193482926455656, 0.12463700961468778] +"HYUNDAI_IONIQ_PHEV_2019" = [2.970807902012267, 1.6312321830002083, 0.1088964990357482] +"HYUNDAI_KONA_EV" = [3.078814714619148, 2.307336938253934, 0.12359762054065548] +"HYUNDAI_PALISADE" = [2.544642494803999, 1.8721703683337008, 0.1301424599248651] +"HYUNDAI_SANTA_FE" = [3.0787027729757632, 2.6173437483495565, 0.1207019341823945] +"HYUNDAI_SANTA_FE_HEV_2022" = [3.501877602644835, 2.729064118456137, 0.10384068104538963] +"HYUNDAI_SANTA_FE_PHEV_2022" = [1.6953050513611045, 1.5837614296206861, 0.12672855941458458] +"HYUNDAI_SONATA_LF" = [2.2200457811703953, 1.2967330275895228, 0.14039920986586393] +"HYUNDAI_SONATA" = [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] +"HYUNDAI_SONATA_HYBRID" = [2.8990264092395734, 2.061410192222139, 0.0899805488717382] +"HYUNDAI_TUCSON_4TH_GEN" = [2.960174, 2.860284, 0.108745] +"JEEP_GRAND_CHEROKEE_2019" = [2.30972, 1.289689569171081, 0.117048] +"JEEP_GRAND_CHEROKEE" = [2.27116, 1.4057367824262523, 0.11725947414922003] +"KIA_EV6" = [3.2, 2.093457, 0.005] +"KIA_K5_2021" = [2.405339728085138, 1.460032270828705, 0.11650989850813716] +"KIA_NIRO_EV" = [2.9215954981365337, 2.1500583840260044, 0.09236802474810267] +"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558] +"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202] +"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533] +"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927] +"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067] +"LEXUS_RX" = [1.6430539050086406, 1.181960058934143, 0.19768806040843034] +"LEXUS_RX_TSS2" = [1.5375561442049257, 1.343166476215164, 0.1931062001527557] +"MAZDA_CX9_2021" = [1.7601682915983443, 1.0889677335154337, 0.17713792194297195] +"SKODA_SUPERB_MK3" = [1.166437404652981, 1.1686163012668165, 0.12194533036948708] +"SUBARU_FORESTER" = [3.6617001649776793, 2.342197172531713, 0.11075960785398745] +"SUBARU_IMPREZA" = [1.0670704910352047, 0.8234374840709592, 0.20986563268614938] +"SUBARU_IMPREZA_2020" = [2.6068223389108303, 2.134872342760203, 0.15261513193561627] +"TOYOTA_AVALON" = [2.5185770183845646, 1.7153346784214922, 0.10603968787111022] +"TOYOTA_AVALON_2019" = [1.7036141952825095, 1.239619084240008, 0.08459830394899492] +"TOYOTA_AVALON_TSS2" = [2.3154403649717357, 2.7777922854327124, 0.11453999639164605] +"TOYOTA_CHR" = [1.5591084333664578, 1.271271459066948, 0.20259087058453193] +"TOYOTA_CHR_TSS2" = [1.7678810166088303, 1.3742176337919942, 0.2319674583741509] +"TOYOTA_CAMRY" = [2.0568162685952505, 1.7576185169559122, 0.108878753] +"TOYOTA_CAMRY_TSS2" = [2.3548324999999997, 2.368900128946771, 0.118436] +"TOYOTA_COROLLA" = [3.117154369115421, 1.8438132575043773, 0.12289685869250652] +"TOYOTA_COROLLA_TSS2" = [1.991132339206426, 1.868866242720403, 0.19570063298031432] +"TOYOTA_HIGHLANDER" = [1.8108348718624456, 1.6348421600679828, 0.15972686105120398] +"TOYOTA_HIGHLANDER_TSS2" = [1.9617570834136164, 1.8611643317268927, 0.14519673256119725] +"TOYOTA_MIRAI" = [2.506899832157829, 1.7417213930750164, 0.20182618449440565] +"TOYOTA_PRIUS" = [1.60, 1.5023147650693636, 0.151515] +"TOYOTA_PRIUS_TSS2" = [1.972600, 1.9104337425537743, 0.170968] +"TOYOTA_RAV4" = [2.085695074355425, 2.2142832316984733, 0.13339165270103975] +"TOYOTA_RAV4_TSS2" = [2.279239424615458, 2.087101966779332, 0.13682208413446817] +"TOYOTA_RAV4H" = [1.9796257271652042, 1.7503987331707576, 0.14628860048885406] +"TOYOTA_RAV4_TSS2_2022" = [2.241883248393209, 1.9304407208090029, 0.112174] +"TOYOTA_SIENNA" = [1.689726, 1.3208264576110418, 0.140456] +"VOLKSWAGEN_ARTEON_MK1" = [1.45136518053819, 1.3639364049316804, 0.23806361745695032] +"VOLKSWAGEN_ATLAS_MK1" = [1.4677006726964945, 1.6733266634075656, 0.12959584092073367] +"VOLKSWAGEN_GOLF_MK7" = [1.3750394140491293, 1.5814743077200641, 0.2018321939386586] +"VOLKSWAGEN_JETTA_MK7" = [1.2271623034089392, 1.216955117387, 0.19437384688370712] +"VOLKSWAGEN_PASSAT_MK8" = [1.3432120736752917, 1.7087275587362314, 0.19444383787326647] +"VOLKSWAGEN_TIGUAN_MK2" = [0.9711965500094828, 1.0001565939459098, 0.1465626137072916] diff --git a/selfdrive/car/torque_data/substitute.toml b/selfdrive/car/torque_data/substitute.toml index 6822ef437b..8724a08010 100644 --- a/selfdrive/car/torque_data/substitute.toml +++ b/selfdrive/car/torque_data/substitute.toml @@ -1,85 +1,83 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] -"MAZDA 3" = "MAZDA CX-9 2021" -"MAZDA 6" = "MAZDA CX-9 2021" -"MAZDA CX-5" = "MAZDA CX-9 2021" -"MAZDA CX-5 2022" = "MAZDA CX-9 2021" -"MAZDA CX-9" = "MAZDA CX-9 2021" +"MAZDA_3" = "MAZDA_CX9_2021" +"MAZDA_6" = "MAZDA_CX9_2021" +"MAZDA_CX5" = "MAZDA_CX9_2021" +"MAZDA_CX5_2022" = "MAZDA_CX9_2021" +"MAZDA_CX9" = "MAZDA_CX9_2021" -"DODGE DURANGO 2021" = "CHRYSLER PACIFICA 2020" +"DODGE_DURANGO" = "CHRYSLER_PACIFICA_2020" -"TOYOTA ALPHARD 2020" = "TOYOTA SIENNA 2018" -"TOYOTA PRIUS v 2017" = "TOYOTA PRIUS 2017" -"LEXUS IS 2018" = "LEXUS NX 2018" -"LEXUS CT HYBRID 2018" = "LEXUS NX 2018" -"LEXUS ES 2018" = "TOYOTA CAMRY 2018" -"LEXUS RC 2020" = "LEXUS NX 2020" -"LEXUS LC 2024" = "LEXUS NX 2020" +"TOYOTA_ALPHARD_TSS2" = "TOYOTA_SIENNA" +"TOYOTA_PRIUS_V" = "TOYOTA_PRIUS" +"LEXUS_IS" = "LEXUS_NX" +"LEXUS_CTH" = "LEXUS_NX" +"LEXUS_ES" = "TOYOTA_CAMRY" +"LEXUS_RC" = "LEXUS_NX_TSS2" +"LEXUS_LC_TSS2" = "LEXUS_NX_TSS2" -"KIA OPTIMA 4TH GEN" = "HYUNDAI SONATA 2020" -"KIA OPTIMA 4TH GEN FACELIFT" = "HYUNDAI SONATA 2020" -"KIA OPTIMA HYBRID 2017 & SPORTS 2019" = "HYUNDAI SONATA 2020" -"KIA OPTIMA HYBRID 4TH GEN FACELIFT" = "HYUNDAI SONATA 2020" -"KIA FORTE E 2018 & GT 2021" = "HYUNDAI SONATA 2020" -"KIA CEED INTRO ED 2019" = "HYUNDAI SONATA 2020" -"KIA SELTOS 2021" = "HYUNDAI SONATA 2020" -"KIA NIRO HYBRID 2019" = "KIA NIRO EV 2020" -"KIA NIRO PLUG-IN HYBRID 2022" = "KIA NIRO EV 2020" -"KIA NIRO HYBRID 2021" = "KIA NIRO EV 2020" -"HYUNDAI VELOSTER 2019" = "HYUNDAI SONATA 2019" -"HYUNDAI KONA 2020" = "HYUNDAI KONA ELECTRIC 2019" -"HYUNDAI KONA HYBRID 2020" = "HYUNDAI KONA ELECTRIC 2019" -"HYUNDAI KONA ELECTRIC 2022" = "HYUNDAI KONA ELECTRIC 2019" -"HYUNDAI IONIQ HYBRID 2017-2019" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" -"HYUNDAI IONIQ HYBRID 2020-2022" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" -"HYUNDAI IONIQ ELECTRIC 2020" = "HYUNDAI IONIQ PLUG-IN HYBRID 2019" -"HYUNDAI ELANTRA 2017" = "HYUNDAI SONATA 2019" -"HYUNDAI I30 N LINE 2019 & GT 2018 DCT" = "HYUNDAI SONATA 2019" -"HYUNDAI ELANTRA HYBRID 2021" = "HYUNDAI SONATA 2020" -"HYUNDAI TUCSON 2019" = "HYUNDAI SANTA FE 2019" -"HYUNDAI SANTA FE 2022" = "HYUNDAI SANTA FE HYBRID 2022" -"KIA K5 HYBRID 2020" = "KIA K5 2021" -"KIA STINGER 2022" = "KIA STINGER GT2 2018" -"GENESIS G90 2017" = "GENESIS G70 2018" -"GENESIS G80 2017" = "GENESIS G70 2018" -"GENESIS G70 2020" = "HYUNDAI SONATA 2020" +"KIA_OPTIMA_G4" = "HYUNDAI_SONATA" +"KIA_OPTIMA_G4_FL" = "HYUNDAI_SONATA" +"KIA_OPTIMA_H" = "HYUNDAI_SONATA" +"KIA_OPTIMA_H_G4_FL" = "HYUNDAI_SONATA" +"KIA_FORTE" = "HYUNDAI_SONATA" +"KIA_CEED" = "HYUNDAI_SONATA" +"KIA_SELTOS" = "HYUNDAI_SONATA" +"KIA_NIRO_PHEV" = "KIA_NIRO_EV" +"KIA_NIRO_PHEV_2022" = "KIA_NIRO_EV" +"KIA_NIRO_HEV_2021" = "KIA_NIRO_EV" +"HYUNDAI_VELOSTER" = "HYUNDAI_SONATA_LF" +"HYUNDAI_KONA" = "HYUNDAI_KONA_EV" +"HYUNDAI_KONA_HEV" = "HYUNDAI_KONA_EV" +"HYUNDAI_KONA_EV_2022" = "HYUNDAI_KONA_EV" +"HYUNDAI_IONIQ" = "HYUNDAI_IONIQ_PHEV_2019" +"HYUNDAI_IONIQ_HEV_2022" = "HYUNDAI_IONIQ_PHEV_2019" +"HYUNDAI_IONIQ_EV_2020" = "HYUNDAI_IONIQ_PHEV_2019" +"HYUNDAI_ELANTRA" = "HYUNDAI_SONATA_LF" +"HYUNDAI_ELANTRA_GT_I30" = "HYUNDAI_SONATA_LF" +"HYUNDAI_ELANTRA_HEV_2021" = "HYUNDAI_SONATA" +"HYUNDAI_TUCSON" = "HYUNDAI_SANTA_FE" +"HYUNDAI_SANTA_FE_2022" = "HYUNDAI_SANTA_FE_HEV_2022" +"KIA_K5_HEV_2020" = "KIA_K5_2021" +"KIA_STINGER_2022" = "KIA_STINGER" +"GENESIS_G90" = "GENESIS_G70" +"GENESIS_G80" = "GENESIS_G70" +"GENESIS_G70_2020" = "HYUNDAI_SONATA" -"HONDA FREED 2020" = "HONDA ODYSSEY 2018" -"HONDA CR-V EU 2016" = "HONDA CR-V 2016" -"HONDA CIVIC SEDAN 1.6 DIESEL 2019" = "HONDA CIVIC (BOSCH) 2019" -"HONDA E 2020" = "HONDA CIVIC (BOSCH) 2019" -"HONDA ODYSSEY CHN 2019" = "HONDA ODYSSEY 2018" +"HONDA_FREED" = "HONDA_ODYSSEY" +"HONDA_CRV_EU" = "HONDA_CRV" +"HONDA_CIVIC_BOSCH_DIESEL" = "HONDA_CIVIC_BOSCH" +"HONDA_E" = "HONDA_CIVIC_BOSCH" +"HONDA_ODYSSEY_CHN" = "HONDA_ODYSSEY" -"BUICK LACROSSE 2017" = "CHEVROLET VOLT PREMIER 2017" -"BUICK REGAL ESSENCE 2018" = "CHEVROLET VOLT PREMIER 2017" -"CADILLAC ESCALADE ESV 2016" = "CHEVROLET VOLT PREMIER 2017" -"CADILLAC ATS Premium Performance 2018" = "CHEVROLET VOLT PREMIER 2017" -"CHEVROLET MALIBU PREMIER 2017" = "CHEVROLET VOLT PREMIER 2017" -"HOLDEN ASTRA RS-V BK 2017" = "CHEVROLET VOLT PREMIER 2017" +"BUICK_LACROSSE" = "CHEVROLET_VOLT" +"BUICK_REGAL" = "CHEVROLET_VOLT" +"CADILLAC_ESCALADE_ESV" = "CHEVROLET_VOLT" +"CADILLAC_ATS" = "CHEVROLET_VOLT" +"CHEVROLET_MALIBU" = "CHEVROLET_VOLT" +"HOLDEN_ASTRA" = "CHEVROLET_VOLT" -"SKODA FABIA 4TH GEN" = "VOLKSWAGEN GOLF 7TH GEN" -"SKODA OCTAVIA 3RD GEN" = "SKODA SUPERB 3RD GEN" -"SKODA SCALA 1ST GEN" = "SKODA SUPERB 3RD GEN" -"SKODA KODIAQ 1ST GEN" = "SKODA SUPERB 3RD GEN" -"SKODA KAROQ 1ST GEN" = "SKODA SUPERB 3RD GEN" -"SKODA KAMIQ 1ST GEN" = "SKODA SUPERB 3RD GEN" -"VOLKSWAGEN CRAFTER 2ND GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN T-ROC 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN T-CROSS 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN TOURAN 2ND GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN TRANSPORTER T6.1" = "VOLKSWAGEN TIGUAN 2ND GEN" -"AUDI Q2 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN TAOS 1ST GEN" = "VOLKSWAGEN TIGUAN 2ND GEN" -"VOLKSWAGEN POLO 6TH GEN" = "VOLKSWAGEN GOLF 7TH GEN" -"SEAT LEON 3RD GEN" = "VOLKSWAGEN GOLF 7TH GEN" -"SEAT ATECA 1ST GEN" = "VOLKSWAGEN GOLF 7TH GEN" +"SKODA_FABIA_MK4" = "VOLKSWAGEN_GOLF_MK7" +"SKODA_OCTAVIA_MK3" = "SKODA_SUPERB_MK3" +"SKODA_KODIAQ_MK1" = "SKODA_SUPERB_MK3" +"SKODA_KAROQ_MK1" = "SKODA_SUPERB_MK3" +"SKODA_KAMIQ_MK1" = "SKODA_SUPERB_MK3" +"VOLKSWAGEN_CRAFTER_MK2" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_TROC_MK1" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_TCROSS_MK1" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_TOURAN_MK2" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_TRANSPORTER_T61" = "VOLKSWAGEN_TIGUAN_MK2" +"AUDI_Q2_MK1" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_TAOS_MK1" = "VOLKSWAGEN_TIGUAN_MK2" +"VOLKSWAGEN_POLO_MK6" = "VOLKSWAGEN_GOLF_MK7" +"SEAT_ATECA_MK1" = "VOLKSWAGEN_GOLF_MK7" -"SUBARU CROSSTREK HYBRID 2020" = "SUBARU IMPREZA SPORT 2020" -"SUBARU FORESTER HYBRID 2020" = "SUBARU IMPREZA SPORT 2020" -"SUBARU LEGACY 7TH GEN" = "SUBARU OUTBACK 6TH GEN" +"SUBARU_CROSSTREK_HYBRID" = "SUBARU_IMPREZA_2020" +"SUBARU_FORESTER_HYBRID" = "SUBARU_IMPREZA_2020" +"SUBARU_LEGACY" = "SUBARU_OUTBACK" # Old subarus don't have much data guessing it's like low torque impreza" -"SUBARU OUTBACK 2018 - 2019" = "SUBARU IMPREZA LIMITED 2019" -"SUBARU OUTBACK 2015 - 2017" = "SUBARU IMPREZA LIMITED 2019" -"SUBARU FORESTER 2017 - 2018" = "SUBARU IMPREZA LIMITED 2019" -"SUBARU LEGACY 2015 - 2018" = "SUBARU IMPREZA LIMITED 2019" -"SUBARU ASCENT LIMITED 2019" = "SUBARU FORESTER 2019" +"SUBARU_OUTBACK_PREGLOBAL_2018" = "SUBARU_IMPREZA" +"SUBARU_OUTBACK_PREGLOBAL" = "SUBARU_IMPREZA" +"SUBARU_FORESTER_PREGLOBAL" = "SUBARU_IMPREZA" +"SUBARU_LEGACY_PREGLOBAL" = "SUBARU_IMPREZA" +"SUBARU_ASCENT" = "SUBARU_FORESTER" diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 343b1d3031..f9b7a478e0 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,10 +1,10 @@ from cereal import car -from openpilot.common.numpy_fast import clip, interp -from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ - create_gas_interceptor_command, make_can_msg +from openpilot.common.numpy_fast import clip +from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ - MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ + CarControllerParams, ToyotaFlags, \ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker @@ -25,7 +25,7 @@ MAX_LTA_ANGLE = 94.9461 # deg MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.params = CarControllerParams(self.CP) @@ -36,6 +36,7 @@ class CarController: self.last_standstill = False self.standstill_req = False self.steer_rate_counter = 0 + self.distance_button = 0 self.packer = CANPacker(dbc_name) self.gas = 0 @@ -99,30 +100,10 @@ class CarController: lta_active, self.frame // 2, torque_wind_down)) # *** gas and brake *** - if self.CP.enableGasInterceptor and CC.longActive: - MAX_INTERCEPTOR_GAS = 0.5 - # RAV4 has very sensitive gas pedal - if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) - elif self.CP.carFingerprint in (CAR.COROLLA,): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) - else: - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) - # offset for creep and windbrake - pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) - pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) - interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) - else: - interceptor_gas_cmd = 0. pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) - # TODO: probably can delete this. CS.pcm_acc_status uses a different signal - # than CS.cruiseState.enabled. confirm they're not meaningfully different - if not CC.enabled and CS.pcm_acc_status: - pcm_cancel_cmd = 1 - # on entering standstill, send standstill request - if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): + if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR): self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled @@ -138,23 +119,26 @@ class CarController: if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged + # Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup + if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl: + desired_distance = 4 - hud_control.leadDistanceBars + if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance: + self.distance_button = not self.distance_button + else: + self.distance_button = 0 + # Lexus IS uses a different cancellation message if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(toyotacan.create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert)) + can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert, + self.distance_button)) self.accel = pcm_accel_cmd else: - can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False)) - - if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: - # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. - # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) - self.gas = interceptor_gas_cmd + can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, self.distance_button)) # *** hud ui *** - if self.CP.carFingerprint != CAR.PRIUS_V: + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: # ui mesg is at 1Hz but we send asap if: # - there is something to display # - there is something to stop displaying @@ -184,7 +168,7 @@ class CarController: if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0]) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer new_actuators.steeringAngleDeg = self.last_angle diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e4ea0d30f9..8315f24ae4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -40,6 +40,11 @@ class CarState(CarStateBase): self.accurate_steer_angle_seen = False self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False) + self.prev_distance_button = 0 + self.distance_button = 0 + + self.pcm_follow_distance = 0 + self.low_speed_lockout = False self.acc_type = 1 self.lkas_hud = {} @@ -54,12 +59,8 @@ class CarState(CarStateBase): ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1 - if self.CP.enableGasInterceptor: - ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2 - ret.gasPressed = ret.gas > 805 - else: - # TODO: find a common gas pedal percentage signal - ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 + + ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 ret.wheelSpeeds = self.get_wheel_speeds( cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"], @@ -95,7 +96,7 @@ class CarState(CarStateBase): ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1 ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 - if self.CP.carFingerprint != CAR.MIRAI: + if self.CP.carFingerprint != CAR.TOYOTA_MIRAI: ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] @@ -148,7 +149,7 @@ class CarState(CarStateBase): # ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request ret.cruiseState.standstill = self.pcm_acc_status == 7 ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"]) - ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) + ret.cruiseState.nonAdaptive = self.pcm_acc_status in (1, 2, 3, 4, 5, 6) ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 @@ -160,9 +161,20 @@ class CarState(CarStateBase): ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) - if self.CP.carFingerprint != CAR.PRIUS_V: + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: + self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] + + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): + # distance button is wired to the ACC module (camera or radar) + self.prev_distance_button = self.distance_button + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + else: + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] + return ret @staticmethod @@ -183,7 +195,7 @@ class CarState(CarStateBase): ("STEER_TORQUE_SENSOR", 50), ] - if CP.carFingerprint != CAR.MIRAI: + if CP.carFingerprint != CAR.TOYOTA_MIRAI: messages.append(("ENGINE_RPM", 42)) if CP.carFingerprint in UNSUPPORTED_DSU_CAR: @@ -192,10 +204,6 @@ class CarState(CarStateBase): else: messages.append(("PCM_CRUISE_2", 33)) - # add gas interceptor reading if we are using it - if CP.enableGasInterceptor: - messages.append(("GAS_SENSOR", 50)) - if CP.enableBsm: messages.append(("BSM", 1)) @@ -213,13 +221,18 @@ class CarState(CarStateBase): ("PRE_COLLISION", 33), ] + if CP.flags & ToyotaFlags.SMART_DSU and not CP.flags & ToyotaFlags.RADAR_CAN_FILTER: + messages += [ + ("SDSU", 100), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod def get_cam_can_parser(CP): messages = [] - if CP.carFingerprint != CAR.PRIUS_V: + if CP.carFingerprint != CAR.TOYOTA_PRIUS_V: messages += [ ("LKAS_HUD", 1), ] diff --git a/selfdrive/car/toyota/fingerprints.py b/selfdrive/car/toyota/fingerprints.py index 12a1d46aaf..0866a4d43c 100644 --- a/selfdrive/car/toyota/fingerprints.py +++ b/selfdrive/car/toyota/fingerprints.py @@ -4,7 +4,7 @@ from openpilot.selfdrive.car.toyota.values import CAR Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.AVALON: { + CAR.TOYOTA_AVALON: { (Ecu.abs, 0x7b0, None): [ b'F152607060\x00\x00\x00\x00\x00\x00', ], @@ -30,7 +30,7 @@ FW_VERSIONS = { b'8646F0703000\x00\x00\x00\x00', ], }, - CAR.AVALON_2019: { + CAR.TOYOTA_AVALON_2019: { (Ecu.abs, 0x7b0, None): [ b'F152607110\x00\x00\x00\x00\x00\x00', b'F152607140\x00\x00\x00\x00\x00\x00', @@ -71,7 +71,7 @@ FW_VERSIONS = { b'8646F0702100\x00\x00\x00\x00', ], }, - CAR.AVALON_TSS2: { + CAR.TOYOTA_AVALON_TSS2: { (Ecu.abs, 0x7b0, None): [ b'\x01F152607240\x00\x00\x00\x00\x00\x00', b'\x01F152607250\x00\x00\x00\x00\x00\x00', @@ -95,7 +95,7 @@ FW_VERSIONS = { b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', ], }, - CAR.CAMRY: { + CAR.TOYOTA_CAMRY: { (Ecu.engine, 0x700, None): [ b'\x018966306L3100\x00\x00\x00\x00', b'\x018966306L4200\x00\x00\x00\x00', @@ -214,6 +214,7 @@ FW_VERSIONS = { b'8646F0601400 ', b'8646F0603400 ', b'8646F0603500 ', + b'8646F0604000 ', b'8646F0604100 ', b'8646F0605000 ', b'8646F0606000 ', @@ -222,7 +223,7 @@ FW_VERSIONS = { b'8646F0607100 ', ], }, - CAR.CAMRY_TSS2: { + CAR.TOYOTA_CAMRY_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B33630\x00\x00\x00\x00\x00\x00', b'8965B33640\x00\x00\x00\x00\x00\x00', @@ -268,7 +269,7 @@ FW_VERSIONS = { b'\x028646F3305500\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, - CAR.CHR: { + CAR.TOYOTA_CHR: { (Ecu.engine, 0x700, None): [ b'\x01896631017100\x00\x00\x00\x00', b'\x01896631017200\x00\x00\x00\x00', @@ -353,7 +354,7 @@ FW_VERSIONS = { b'8646FF407100 ', ], }, - CAR.CHR_TSS2: { + CAR.TOYOTA_CHR_TSS2: { (Ecu.abs, 0x7b0, None): [ b'F152610041\x00\x00\x00\x00\x00\x00', b'F152610260\x00\x00\x00\x00\x00\x00', @@ -377,6 +378,7 @@ FW_VERSIONS = { (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821FF410200\x00\x00\x00\x00', b'\x018821FF410300\x00\x00\x00\x00', + b'\x018821FF410400\x00\x00\x00\x00', b'\x018821FF410500\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ @@ -385,7 +387,7 @@ FW_VERSIONS = { b'\x028646FF413100\x00\x00\x00\x008646GF411100\x00\x00\x00\x00', ], }, - CAR.COROLLA: { + CAR.TOYOTA_COROLLA: { (Ecu.engine, 0x7e0, None): [ b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0230ZC2100\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -419,7 +421,7 @@ FW_VERSIONS = { b'8646F0201200\x00\x00\x00\x00', ], }, - CAR.COROLLA_TSS2: { + CAR.TOYOTA_COROLLA_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630A22000\x00\x00\x00\x00', b'\x01896630ZG2000\x00\x00\x00\x00', @@ -520,6 +522,7 @@ FW_VERSIONS = { ], (Ecu.abs, 0x7b0, None): [ b'\x01F152602280\x00\x00\x00\x00\x00\x00', + b'\x01F152602281\x00\x00\x00\x00\x00\x00', b'\x01F152602470\x00\x00\x00\x00\x00\x00', b'\x01F152602560\x00\x00\x00\x00\x00\x00', b'\x01F152602590\x00\x00\x00\x00\x00\x00', @@ -573,6 +576,7 @@ FW_VERSIONS = { b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F12010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F1201200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', @@ -593,7 +597,7 @@ FW_VERSIONS = { b'\x028646F7605100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, - CAR.HIGHLANDER: { + CAR.TOYOTA_HIGHLANDER: { (Ecu.engine, 0x700, None): [ b'\x01896630E09000\x00\x00\x00\x00', b'\x01896630E43000\x00\x00\x00\x00', @@ -638,6 +642,7 @@ FW_VERSIONS = { (Ecu.dsu, 0x791, None): [ b'881510E01100\x00\x00\x00\x00', b'881510E01200\x00\x00\x00\x00', + b'881510E02200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'8821F4702100\x00\x00\x00\x00', @@ -648,7 +653,7 @@ FW_VERSIONS = { b'8646F0E01300\x00\x00\x00\x00', ], }, - CAR.HIGHLANDER_TSS2: { + CAR.TOYOTA_HIGHLANDER_TSS2: { (Ecu.eps, 0x7a1, None): [ b'8965B48241\x00\x00\x00\x00\x00\x00', b'8965B48310\x00\x00\x00\x00\x00\x00', @@ -685,6 +690,7 @@ FW_VERSIONS = { b'\x01896630EB1000\x00\x00\x00\x00', b'\x01896630EB1100\x00\x00\x00\x00', b'\x01896630EB1200\x00\x00\x00\x00', + b'\x01896630EB1300\x00\x00\x00\x00', b'\x01896630EB2000\x00\x00\x00\x00', b'\x01896630EB2100\x00\x00\x00\x00', b'\x01896630EB2200\x00\x00\x00\x00', @@ -724,6 +730,7 @@ FW_VERSIONS = { b'\x018966353M7000\x00\x00\x00\x00', b'\x018966353M7100\x00\x00\x00\x00', b'\x018966353Q2000\x00\x00\x00\x00', + b'\x018966353Q2100\x00\x00\x00\x00', b'\x018966353Q2300\x00\x00\x00\x00', b'\x018966353Q4000\x00\x00\x00\x00', b'\x018966353R1100\x00\x00\x00\x00', @@ -771,23 +778,29 @@ FW_VERSIONS = { b'\x018966353S1000\x00\x00\x00\x00', b'\x018966353S2000\x00\x00\x00\x00', ], + (Ecu.engine, 0x7e0, None): [ + b'\x02353U0000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00', + ], (Ecu.abs, 0x7b0, None): [ b'\x01F15265337200\x00\x00\x00\x00', b'\x01F15265342000\x00\x00\x00\x00', + b'\x01F15265343000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B53450\x00\x00\x00\x00\x00\x00', + b'8965B53800\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', b'\x018821F6201300\x00\x00\x00\x00', + b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ b'\x028646F5303300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F5303400\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, - CAR.PRIUS: { + CAR.TOYOTA_PRIUS: { (Ecu.engine, 0x700, None): [ b'\x02896634761000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896634761100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -843,6 +856,7 @@ FW_VERSIONS = { b'8965B47023\x00\x00\x00\x00\x00\x00', b'8965B47050\x00\x00\x00\x00\x00\x00', b'8965B47060\x00\x00\x00\x00\x00\x00', + b'8965B47070\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'F152647290\x00\x00\x00\x00\x00\x00', @@ -884,7 +898,7 @@ FW_VERSIONS = { b'8646F4705200\x00\x00\x00\x00', ], }, - CAR.PRIUS_V: { + CAR.TOYOTA_PRIUS_V: { (Ecu.abs, 0x7b0, None): [ b'F152647280\x00\x00\x00\x00\x00\x00', ], @@ -901,7 +915,7 @@ FW_VERSIONS = { b'8646F4703300\x00\x00\x00\x00', ], }, - CAR.RAV4: { + CAR.TOYOTA_RAV4: { (Ecu.engine, 0x7e0, None): [ b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02342Q1100\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -942,7 +956,7 @@ FW_VERSIONS = { b'8646F4204000\x00\x00\x00\x00', ], }, - CAR.RAV4H: { + CAR.TOYOTA_RAV4H: { (Ecu.engine, 0x7e0, None): [ b'\x02342N9000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02342N9100\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -979,7 +993,7 @@ FW_VERSIONS = { b'8646F4204000\x00\x00\x00\x00', ], }, - CAR.RAV4_TSS2: { + CAR.TOYOTA_RAV4_TSS2: { (Ecu.engine, 0x700, None): [ b'\x01896630R58000\x00\x00\x00\x00', b'\x01896630R58100\x00\x00\x00\x00', @@ -1024,6 +1038,8 @@ FW_VERSIONS = { b'\x02896634A13000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A13201\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', + b'\x02896634A14001\x00\x00\x00\x00897CF0R01000\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -1088,7 +1104,7 @@ FW_VERSIONS = { b'\x028646F4203800\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, - CAR.RAV4_TSS2_2022: { + CAR.TOYOTA_RAV4_TSS2_2022: { (Ecu.abs, 0x7b0, None): [ b'\x01F15260R350\x00\x00\x00\x00\x00\x00', b'\x01F15260R361\x00\x00\x00\x00\x00\x00', @@ -1124,26 +1140,32 @@ FW_VERSIONS = { b'\x028646F0R02100\x00\x00\x00\x008646G0R01100\x00\x00\x00\x00', ], }, - CAR.RAV4_TSS2_2023: { + CAR.TOYOTA_RAV4_TSS2_2023: { (Ecu.abs, 0x7b0, None): [ b'\x01F15260R450\x00\x00\x00\x00\x00\x00', + b'\x01F15260R50000\x00\x00\x00\x00', b'\x01F15260R51000\x00\x00\x00\x00', b'\x01F15264283200\x00\x00\x00\x00', b'\x01F15264283300\x00\x00\x00\x00', b'\x01F152642F1000\x00\x00\x00\x00', b'\x01F152642F8000\x00\x00\x00\x00', + b'\x01F152642F8100\x00\x00\x00\x00', + b'\x01F152642F9000\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x028965B0R11000\x00\x00\x00\x008965B0R12000\x00\x00\x00\x00', b'8965B42371\x00\x00\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896634A61000\x00\x00\x00\x00', b'\x01896634A88100\x00\x00\x00\x00', b'\x01896634A89100\x00\x00\x00\x00', b'\x01896634AE1001\x00\x00\x00\x00', b'\x01896634AF0000\x00\x00\x00\x00', b'\x01896634AJ2000\x00\x00\x00\x00', b'\x01896634AL5000\x00\x00\x00\x00', + b'\x01896634AL6000\x00\x00\x00\x00', + b'\x01896634AL8000\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R03100\x00\x00\x00\x00', @@ -1154,7 +1176,7 @@ FW_VERSIONS = { b'\x028646F0R11000\x00\x00\x00\x008646G0R04000\x00\x00\x00\x00', ], }, - CAR.SIENNA: { + CAR.TOYOTA_SIENNA: { (Ecu.engine, 0x700, None): [ b'\x01896630832100\x00\x00\x00\x00', b'\x01896630832200\x00\x00\x00\x00', @@ -1222,6 +1244,7 @@ FW_VERSIONS = { b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x028966333W1000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', + b'\x02896633T10000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00', ], (Ecu.abs, 0x7b0, None): [ b'\x01F152606281\x00\x00\x00\x00\x00\x00', @@ -1263,6 +1286,7 @@ FW_VERSIONS = { }, CAR.LEXUS_ES: { (Ecu.engine, 0x7e0, None): [ + b'\x02333M4100\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02333M4200\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x02333R0000\x00\x00\x00\x00\x00\x00\x00\x00A0C01000\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -1274,6 +1298,7 @@ FW_VERSIONS = { b'881513309400\x00\x00\x00\x00', b'881513309500\x00\x00\x00\x00', b'881513310400\x00\x00\x00\x00', + b'881513310500\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B33502\x00\x00\x00\x00\x00\x00', @@ -1318,6 +1343,7 @@ FW_VERSIONS = { b'\x01896637854000\x00\x00\x00\x00', b'\x01896637873000\x00\x00\x00\x00', b'\x01896637878000\x00\x00\x00\x00', + b'\x01896637878100\x00\x00\x00\x00', ], (Ecu.engine, 0x7e0, None): [ b'\x0237841000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1403,6 +1429,7 @@ FW_VERSIONS = { b'\x018821F6201400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F1104200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', b'\x028646F1105200\x00\x00\x00\x008646G3304000\x00\x00\x00\x00', ], }, @@ -1451,6 +1478,7 @@ FW_VERSIONS = { b'\x01896630E41200\x00\x00\x00\x00', b'\x01896630E41500\x00\x00\x00\x00', b'\x01896630EA3100\x00\x00\x00\x00', + b'\x01896630EA3300\x00\x00\x00\x00', b'\x01896630EA3400\x00\x00\x00\x00', b'\x01896630EA4100\x00\x00\x00\x00', b'\x01896630EA4300\x00\x00\x00\x00', @@ -1533,6 +1561,7 @@ FW_VERSIONS = { b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', b'\x018966348X0000\x00\x00\x00\x00', + b'\x01896634D11000\x00\x00\x00\x00', b'\x01896634D12000\x00\x00\x00\x00', b'\x01896634D12100\x00\x00\x00\x00', b'\x01896634D43000\x00\x00\x00\x00', @@ -1576,7 +1605,7 @@ FW_VERSIONS = { b'\x028646F4810400\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, - CAR.PRIUS_TSS2: { + CAR.TOYOTA_PRIUS_TSS2: { (Ecu.engine, 0x700, None): [ b'\x028966347B1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x028966347C4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -1607,7 +1636,7 @@ FW_VERSIONS = { b'\x028646F4712000\x00\x00\x00\x008646G2601500\x00\x00\x00\x00', ], }, - CAR.MIRAI: { + CAR.TOYOTA_MIRAI: { (Ecu.abs, 0x7d1, None): [ b'\x01898A36203000\x00\x00\x00\x00', ], @@ -1625,7 +1654,7 @@ FW_VERSIONS = { b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', ], }, - CAR.ALPHARD_TSS2: { + CAR.TOYOTA_ALPHARD_TSS2: { (Ecu.engine, 0x7e0, None): [ b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', b'\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 988b1b4547..3ea05f9fef 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,13 +1,13 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV from panda import Panda from panda.python import uds from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR -from openpilot.selfdrive.car import get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.interfaces import CarInterfaceBase +ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName SteerControlType = car.CarParams.SteerControlType @@ -44,82 +44,37 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate in TSS2_CAR - if candidate == CAR.PRIUS: + # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it + # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs + if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): + ret.flags |= ToyotaFlags.SMART_DSU.value + + if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR: + ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value + + # In TSS2 cars, the camera does long control + found_ecus = [fw.ecu for fw in car_fw] + ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ + and not (ret.flags & ToyotaFlags.SMART_DSU) + + if candidate == CAR.TOYOTA_PRIUS: stop_and_go = True - ret.wheelbase = 2.70 - ret.steerRatio = 15.74 # unknown end-to-end spec - 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': ret.steerActuatorDelay = 0.25 CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) - elif candidate == CAR.PRIUS_V: - stop_and_go = True - ret.wheelbase = 2.78 - ret.steerRatio = 17.4 - 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 - 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 - 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_RX_TSS2): stop_and_go = True - ret.wheelbase = 2.79 - ret.steerRatio = 16. # 14.8 is spec end-to-end ret.wheelSpeedFactor = 1.035 - ret.tireStiffnessFactor = 0.5533 - ret.mass = 4481. * CV.LB_TO_KG # mean between min and max - - elif candidate in (CAR.CHR, CAR.CHR_TSS2): - stop_and_go = True - ret.wheelbase = 2.63906 - ret.steerRatio = 13.6 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3300. * CV.LB_TO_KG - elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2): - stop_and_go = True - ret.wheelbase = 2.82448 - ret.steerRatio = 13.7 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid - - elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDER_TSS2): - # TODO: TSS-P models can do stop and go, but unclear if it requires sDSU or unplugging DSU - stop_and_go = True - ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in - ret.steerRatio = 16.0 - 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.AVALON_TSS2): + elif candidate in (CAR.TOYOTA_AVALON, CAR.TOYOTA_AVALON_2019, CAR.TOYOTA_AVALON_TSS2): # starting from 2019, all Avalon variants have stop and go # https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf - 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 - 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.RAV4_TSS2_2023): - ret.wheelbase = 2.68986 - ret.steerRatio = 14.3 - ret.tireStiffnessFactor = 0.7933 - ret.mass = 3585. * CV.LB_TO_KG # Average between ICE and Hybrid + stop_and_go = candidate != CAR.TOYOTA_AVALON + + elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023): ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP = [0.0] ret.lateralTuning.pid.kpBP = [0.0] @@ -136,75 +91,14 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00004 break - elif candidate == CAR.COROLLA_TSS2: - ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback - ret.steerRatio = 13.9 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 3060. * CV.LB_TO_KG - - elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ES_TSS2): - ret.wheelbase = 2.8702 - ret.steerRatio = 16.0 # not optimized - 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 - 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 - ret.tireStiffnessFactor = 0.444 - ret.mass = 3736.8 * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_GS_F: - ret.wheelbase = 2.84988 - ret.steerRatio = 13.3 - ret.tireStiffnessFactor = 0.444 - ret.mass = 4034. * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_CTH: - stop_and_go = True - ret.wheelbase = 2.60 - ret.steerRatio = 18.6 - ret.tireStiffnessFactor = 0.517 - ret.mass = 3108 * CV.LB_TO_KG # mean between min and max - - elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2): + elif candidate in (CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX): + # TODO: Some of these platforms are not advertised to have full range ACC, are they similar to SNG_WITHOUT_DSU cars? stop_and_go = True - ret.wheelbase = 2.66 - ret.steerRatio = 14.7 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 4070 * CV.LB_TO_KG - - elif candidate == CAR.LEXUS_LC_TSS2: - ret.wheelbase = 2.87 - ret.steerRatio = 13.0 - ret.tireStiffnessFactor = 0.444 # not optimized yet - ret.mass = 4500 * 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 - 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 - ret.tireStiffnessFactor = 0.8 - ret.mass = 4300. * CV.LB_TO_KG - elif candidate == CAR.ALPHARD_TSS2: - ret.wheelbase = 3.00 - ret.steerRatio = 14.2 - ret.tireStiffnessFactor = 0.444 - ret.mass = 4305. * CV.LB_TO_KG + # TODO: these models can do stop and go, but unclear if it requires sDSU or unplugging DSU. + # For now, don't list stop and go functionality in the docs + if ret.flags & ToyotaFlags.SNG_WITHOUT_DSU: + stop_and_go = stop_and_go or bool(ret.flags & ToyotaFlags.SMART_DSU.value) or (ret.enableDsu and not docs) ret.centerToFront = ret.wheelbase * 0.44 @@ -212,20 +106,10 @@ class CarInterface(CarInterfaceBase): # Detect flipped signals and enable for C-HR and others 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 - # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs - if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): - ret.flags |= ToyotaFlags.SMART_DSU.value - # No radar dbc for cars without DSU which are not TSS 2.0 # TODO: make an adas dbc file for dsu-less models ret.radarUnavailable = DBC[candidate]['radar'] is None or candidate in (NO_DSU_CAR - TSS2_CAR) - # In TSS2 cars, the camera does long control - found_ecus = [fw.ecu for fw in car_fw] - ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ - and not (ret.flags & ToyotaFlags.SMART_DSU) - # if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar. # since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU) @@ -249,22 +133,18 @@ class CarInterface(CarInterfaceBase): # - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet) ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR - ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl if not ret.openpilotLongitudinalControl: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL - if ret.enableGasInterceptor: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR - # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED + ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED tune = ret.longitudinalTuning tune.deadzoneBP = [0., 9.] tune.deadzoneV = [.0, .15] - if candidate in TSS2_CAR or ret.enableGasInterceptor: + if candidate in TSS2_CAR: tune.kpBP = [0., 5., 20.] tune.kpV = [1.3, 1.0, 0.7] tune.kiBP = [0., 5., 12., 20., 27.] @@ -292,6 +172,9 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): + ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) + # events events = self.create_common_events(ret) @@ -301,7 +184,7 @@ class CarInterface(CarInterfaceBase): events.add(EventName.vehicleSensorsInvalid) if self.CP.openpilotLongitudinalControl: - if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor: + if ret.cruiseState.standstill and not ret.brakePressed: events.add(EventName.resumeRequired) if self.CS.low_speed_lockout: events.add(EventName.lowSpeedLockout) @@ -317,8 +200,3 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() return ret - - # pass in a car.CarControl - # to be called @ 100hz - def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, now_nanos) diff --git a/selfdrive/car/toyota/tests/__init__.py b/selfdrive/car/toyota/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/car/toyota/tests/print_platform_codes.py b/selfdrive/car/toyota/tests/print_platform_codes.py new file mode 100755 index 0000000000..9ec7a14cd3 --- /dev/null +++ b/selfdrive/car/toyota/tests/print_platform_codes.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +from collections import defaultdict +from cereal import car +from openpilot.selfdrive.car.toyota.values import PLATFORM_CODE_ECUS, get_platform_codes +from openpilot.selfdrive.car.toyota.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + +if __name__ == "__main__": + parts_for_ecu: dict = defaultdict(set) + cars_for_code: dict = defaultdict(lambda: defaultdict(set)) + for car_model, ecus in FW_VERSIONS.items(): + print() + print(car_model) + for ecu in sorted(ecus, key=lambda x: int(x[0])): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + platform_codes = get_platform_codes(ecus[ecu]) + parts_for_ecu[ecu] |= {code.split(b'-')[0] for code in platform_codes if code.count(b'-') > 1} + for code in platform_codes: + cars_for_code[ecu][b'-'.join(code.split(b'-')[:2])] |= {car_model} + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + print(f' Codes: {platform_codes}') + + print('\nECU parts:') + for ecu, parts in parts_for_ecu.items(): + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}): {parts}') + + print('\nCar models vs. platform codes (no major versions):') + for ecu, codes in cars_for_code.items(): + print(f' (Ecu.{ECU_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') + for code, cars in codes.items(): + print(f' {code!r}: {sorted(cars)}') diff --git a/selfdrive/car/toyota/tests/test_toyota.py b/selfdrive/car/toyota/tests/test_toyota.py new file mode 100644 index 0000000000..0217a0fbc1 --- /dev/null +++ b/selfdrive/car/toyota/tests/test_toyota.py @@ -0,0 +1,166 @@ +from hypothesis import given, settings, strategies as st + +from cereal import car +from openpilot.selfdrive.car.fw_versions import build_fw_dict +from openpilot.selfdrive.car.toyota.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, \ + FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ + get_platform_codes + +Ecu = car.CarParams.Ecu +ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} + + +def check_fw_version(fw_version: bytes) -> bool: + return b'?' not in fw_version + + +class TestToyotaInterfaces: + def test_car_sets(self): + assert len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0 + assert len(RADAR_ACC_CAR - TSS2_CAR) == 0 + + def test_lta_platforms(self): + # At this time, only RAV4 2023 is expected to use LTA/angle control + assert ANGLE_CONTROL_CAR == {CAR.TOYOTA_RAV4_TSS2_2023} + + def test_tss2_dbc(self): + # We make some assumptions about TSS2 platforms, + # like looking up certain signals only in this DBC + for car_model, dbc in DBC.items(): + if car_model in TSS2_CAR: + assert dbc["pt"] == "toyota_nodsu_pt_generated" + + def test_essential_ecus(self, subtests): + # Asserts standard ECUs exist for each platform + common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera} + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + present_ecus = {ecu[0] for ecu in ecus} + missing_ecus = common_ecus - present_ecus + assert len(missing_ecus) == 0 + + # Some exceptions for other common ECUs + if car_model not in (CAR.TOYOTA_ALPHARD_TSS2,): + assert Ecu.abs in present_ecus + + if car_model not in (CAR.TOYOTA_MIRAI,): + assert Ecu.engine in present_ecus + + if car_model not in (CAR.TOYOTA_PRIUS_V, CAR.LEXUS_CTH): + assert Ecu.eps in present_ecus + + +class TestToyotaFingerprint: + def test_non_essential_ecus(self, subtests): + # Ensures only the cars that have multiple engine ECUs are in the engine non-essential ECU list + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + engine_ecus = {ecu for ecu in ecus if ecu[0] == Ecu.engine} + assert (len(engine_ecus) > 1) == (car_model in FW_QUERY_CONFIG.non_essential_ecus[Ecu.engine]), \ + f"Car model unexpectedly {'not ' if len(engine_ecus) > 1 else ''}in non-essential list" + + def test_valid_fw_versions(self, subtests): + # Asserts all FW versions are valid + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for fws in ecus.values(): + for fw in fws: + assert check_fw_version(fw), fw + + # Tests for part numbers, platform codes, and sub-versions which Toyota will use to fuzzy + # fingerprint in the absence of full FW matches: + @settings(max_examples=100) + @given(data=st.data()) + def test_platform_codes_fuzzy_fw(self, data): + fw_strategy = st.lists(st.binary()) + fws = data.draw(fw_strategy) + get_platform_codes(fws) + + def test_platform_code_ecus_available(self, subtests): + # Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for platform_code_ecu in PLATFORM_CODE_ECUS: + if platform_code_ecu == Ecu.eps and car_model in (CAR.TOYOTA_PRIUS_V, CAR.LEXUS_CTH,): + continue + if platform_code_ecu == Ecu.abs and car_model in (CAR.TOYOTA_ALPHARD_TSS2,): + continue + assert platform_code_ecu in [e[0] for e in ecus] + + def test_fw_format(self, subtests): + # Asserts: + # - every supported ECU FW version returns one platform code + # - every supported ECU FW version has a part number + # - expected parsing of ECU sub-versions + + for car_model, ecus in FW_VERSIONS.items(): + with subtests.test(car_model=car_model.value): + for ecu, fws in ecus.items(): + if ecu[0] not in PLATFORM_CODE_ECUS: + continue + + codes = dict() + for fw in fws: + result = get_platform_codes([fw]) + # Check only one platform code and sub-version + assert 1 == len(result), f"Unable to parse FW: {fw}" + assert 1 == len(list(result.values())[0]), f"Unable to parse FW: {fw}" + codes |= result + + # Toyota places the ECU part number in their FW versions, assert all parsable + # Note that there is only one unique part number per ECU across the fleet, so this + # is not important for identification, just a sanity check. + assert all(code.count(b"-") > 1 for code in codes), f"FW does not have part number: {fw} {codes}" + + def test_platform_codes_spot_check(self): + # Asserts basic platform code parsing behavior for a few cases + results = get_platform_codes([ + b"F152607140\x00\x00\x00\x00\x00\x00", + b"F152607171\x00\x00\x00\x00\x00\x00", + b"F152607110\x00\x00\x00\x00\x00\x00", + b"F152607180\x00\x00\x00\x00\x00\x00", + ]) + assert results == {b"F1526-07-1": {b"10", b"40", b"71", b"80"}} + + results = get_platform_codes([ + b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00", + b"\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00", + ]) + assert results == {b"8646F-41-04": {b"100"}} + + # Short version has no part number + results = get_platform_codes([ + b"\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00", + b"\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00", + ]) + assert results == {b"58-70": {b"000"}, b"58-83": {b"000"}} + + results = get_platform_codes([ + b"F152607110\x00\x00\x00\x00\x00\x00", + b"F152607140\x00\x00\x00\x00\x00\x00", + b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00", + b"\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00", + ]) + assert results == {b"F1526-07-1": {b"10", b"40"}, b"8646F-41-04": {b"100"}, b"58-79": {b"000"}} + + def test_fuzzy_excluded_platforms(self): + # Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared. + platforms_with_shared_codes = set() + for platform, fw_by_addr in FW_VERSIONS.items(): + car_fw = [] + for ecu, fw_versions in fw_by_addr.items(): + ecu_name, addr, sub_addr = ecu + for fw in fw_versions: + car_fw.append({"ecu": ecu_name, "fwVersion": fw, "address": addr, + "subAddress": 0 if sub_addr is None else sub_addr}) + + CP = car.CarParams.new_message(carFw=car_fw) + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS) + if len(matches) == 1: + assert list(matches)[0] == platform + else: + # If a platform has multiple matches, add it and its matches + platforms_with_shared_codes |= {str(platform), *matches} + + assert platforms_with_shared_codes == FUZZY_EXCLUDED_PLATFORMS, (len(platforms_with_shared_codes), len(FW_VERSIONS)) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index e14e3e53a0..1cc99b41b5 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -33,12 +33,12 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert, distance): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, "ACC_TYPE": acc_type, - "DISTANCE": 0, + "DISTANCE": distance, "MINI_CAR": lead, "PERMIT_BRAKING": 1, "RELEASE_STANDSTILL": not standstill_req, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ed2b022f43..2e781ad0aa 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,13 +1,13 @@ import re from collections import defaultdict from dataclasses import dataclass, field -from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Set, Union +from enum import Enum, IntFlag from cereal import car from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms from openpilot.selfdrive.car import AngleRateLimit, dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, CarParts, CarHarness +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarDocs, Column, CarParts, CarHarness from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries Ecu = car.CarParams.Ecu @@ -42,50 +42,22 @@ class CarControllerParams: class ToyotaFlags(IntFlag): + # Detected flags HYBRID = 1 SMART_DSU = 2 DISABLE_RADAR = 4 + RADAR_CAN_FILTER = 1024 - -class CAR(StrEnum): - # Toyota - ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" - AVALON = "TOYOTA AVALON 2016" - AVALON_2019 = "TOYOTA AVALON 2019" - AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5 - CAMRY = "TOYOTA CAMRY 2018" - CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5 - CHR = "TOYOTA C-HR 2018" - CHR_TSS2 = "TOYOTA C-HR 2021" - COROLLA = "TOYOTA COROLLA 2017" - # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid - COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019" - HIGHLANDER = "TOYOTA HIGHLANDER 2017" - HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020" - PRIUS = "TOYOTA PRIUS 2017" - PRIUS_V = "TOYOTA PRIUS v 2017" - PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" - RAV4 = "TOYOTA RAV4 2017" - RAV4H = "TOYOTA RAV4 HYBRID 2017" - RAV4_TSS2 = "TOYOTA RAV4 2019" - RAV4_TSS2_2022 = "TOYOTA RAV4 2022" - RAV4_TSS2_2023 = "TOYOTA RAV4 2023" - MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5 - SIENNA = "TOYOTA SIENNA 2018" - - # Lexus - LEXUS_CTH = "LEXUS CT HYBRID 2018" - LEXUS_ES = "LEXUS ES 2018" - LEXUS_ES_TSS2 = "LEXUS ES 2019" - LEXUS_IS = "LEXUS IS 2018" - LEXUS_IS_TSS2 = "LEXUS IS 2023" - LEXUS_NX = "LEXUS NX 2018" - LEXUS_NX_TSS2 = "LEXUS NX 2020" - LEXUS_LC_TSS2 = "LEXUS LC 2024" - LEXUS_RC = "LEXUS RC 2020" - LEXUS_RX = "LEXUS RX 2016" - LEXUS_RX_TSS2 = "LEXUS RX 2020" - LEXUS_GS_F = "LEXUS GS F 2016" + # Static flags + TSS2 = 8 + NO_DSU = 16 + UNSUPPORTED_DSU = 32 + RADAR_ACC = 64 + # these cars use the Lane Tracing Assist (LTA) message for lateral control + ANGLE_CONTROL = 128 + NO_STOP_TIMER = 256 + # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU + SNG_WITHOUT_DSU = 512 class Footnote(Enum): @@ -95,165 +67,311 @@ class Footnote(Enum): @dataclass -class ToyotaCarInfo(CarInfo): +class ToyotaCarDocs(CarDocs): package: str = "All" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota_a])) -CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = { +@dataclass +class ToyotaTSS2PlatformConfig(PlatformConfig): + dbc_dict: dict = field(default_factory=lambda: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas')) + + def init(self): + self.flags |= ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU + + if self.flags & ToyotaFlags.RADAR_ACC: + self.dbc_dict = dbc_dict('toyota_nodsu_pt_generated', None) + + +class CAR(Platforms): # Toyota - CAR.ALPHARD_TSS2: [ - ToyotaCarInfo("Toyota Alphard 2019-20"), - ToyotaCarInfo("Toyota Alphard Hybrid 2021"), - ], - CAR.AVALON: [ - ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"), - ToyotaCarInfo("Toyota Avalon 2017-18"), - ], - CAR.AVALON_2019: [ - ToyotaCarInfo("Toyota Avalon 2019-21"), - ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"), - ], - CAR.AVALON_TSS2: [ - ToyotaCarInfo("Toyota Avalon 2022"), - ToyotaCarInfo("Toyota Avalon Hybrid 2022"), - ], - CAR.CAMRY: [ - ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), - ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), - ], - CAR.CAMRY_TSS2: [ - ToyotaCarInfo("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]), - ToyotaCarInfo("Toyota Camry Hybrid 2021-24"), - ], - CAR.CHR: [ - ToyotaCarInfo("Toyota C-HR 2017-20"), - ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"), - ], - CAR.CHR_TSS2: [ - ToyotaCarInfo("Toyota C-HR 2021"), - ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"), - ], - CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"), - CAR.COROLLA_TSS2: [ - ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), - ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5), - ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), - # Hybrid platforms - ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"), - ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5), - ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), - ToyotaCarInfo("Lexus UX Hybrid 2019-23"), - ], - CAR.HIGHLANDER: [ - ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), - ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"), - ], - CAR.HIGHLANDER_TSS2: [ - ToyotaCarInfo("Toyota Highlander 2020-23"), - ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"), - ], - CAR.PRIUS: [ - ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), - ], - CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED), - CAR.PRIUS_TSS2: [ - ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), - ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), - ], - CAR.RAV4: [ - ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P"), - ToyotaCarInfo("Toyota RAV4 2017-18") - ], - CAR.RAV4H: [ - ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26") - ], - CAR.RAV4_TSS2: [ - ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"), - ], - CAR.RAV4_TSS2_2022: [ - ToyotaCarInfo("Toyota RAV4 2022"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), - ], - CAR.RAV4_TSS2_2023: [ - ToyotaCarInfo("Toyota RAV4 2023-24"), - ToyotaCarInfo("Toyota RAV4 Hybrid 2023-24"), - ], - CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"), - CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED), + TOYOTA_ALPHARD_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota Alphard 2019-20"), + ToyotaCarDocs("Toyota Alphard Hybrid 2021"), + ], + CarSpecs(mass=4305. * CV.LB_TO_KG, wheelbase=3.0, steerRatio=14.2, tireStiffnessFactor=0.444), + ) + TOYOTA_AVALON = PlatformConfig( + [ + ToyotaCarDocs("Toyota Avalon 2016", "Toyota Safety Sense P"), + ToyotaCarDocs("Toyota Avalon 2017-18"), + ], + CarSpecs(mass=3505. * CV.LB_TO_KG, wheelbase=2.82, steerRatio=14.8, tireStiffnessFactor=0.7983), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + TOYOTA_AVALON_2019 = PlatformConfig( + [ + ToyotaCarDocs("Toyota Avalon 2019-21"), + ToyotaCarDocs("Toyota Avalon Hybrid 2019-21"), + ], + TOYOTA_AVALON.specs, + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + ) + TOYOTA_AVALON_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5 + [ + ToyotaCarDocs("Toyota Avalon 2022"), + ToyotaCarDocs("Toyota Avalon Hybrid 2022"), + ], + TOYOTA_AVALON.specs, + ) + TOYOTA_CAMRY = PlatformConfig( + [ + ToyotaCarDocs("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]), + ToyotaCarDocs("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"), + ], + CarSpecs(mass=3400. * CV.LB_TO_KG, wheelbase=2.82448, steerRatio=13.7, tireStiffnessFactor=0.7933), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_DSU, + ) + TOYOTA_CAMRY_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5 + [ + ToyotaCarDocs("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]), + ToyotaCarDocs("Toyota Camry Hybrid 2021-24"), + ], + TOYOTA_CAMRY.specs, + ) + TOYOTA_CHR = PlatformConfig( + [ + ToyotaCarDocs("Toyota C-HR 2017-20"), + ToyotaCarDocs("Toyota C-HR Hybrid 2017-20"), + ], + CarSpecs(mass=3300. * CV.LB_TO_KG, wheelbase=2.63906, steerRatio=13.6, tireStiffnessFactor=0.7933), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_DSU, + ) + TOYOTA_CHR_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota C-HR 2021"), + ToyotaCarDocs("Toyota C-HR Hybrid 2021-22"), + ], + TOYOTA_CHR.specs, + flags=ToyotaFlags.RADAR_ACC, + ) + TOYOTA_COROLLA = PlatformConfig( + [ToyotaCarDocs("Toyota Corolla 2017-19")], + CarSpecs(mass=2860. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=18.27, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid + TOYOTA_COROLLA_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + ToyotaCarDocs("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5), + ToyotaCarDocs("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"), + # Hybrid platforms + ToyotaCarDocs("Toyota Corolla Hybrid 2020-22"), + ToyotaCarDocs("Toyota Corolla Hybrid (South America only) 2020-23", min_enable_speed=7.5), + ToyotaCarDocs("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5), + ToyotaCarDocs("Lexus UX Hybrid 2019-23"), + ], + CarSpecs(mass=3060. * CV.LB_TO_KG, wheelbase=2.67, steerRatio=13.9, tireStiffnessFactor=0.444), + ) + TOYOTA_HIGHLANDER = PlatformConfig( + [ + ToyotaCarDocs("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"), + ToyotaCarDocs("Toyota Highlander Hybrid 2017-19"), + ], + CarSpecs(mass=4516. * CV.LB_TO_KG, wheelbase=2.8194, steerRatio=16.0, tireStiffnessFactor=0.8), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU, + ) + TOYOTA_HIGHLANDER_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota Highlander 2020-23"), + ToyotaCarDocs("Toyota Highlander Hybrid 2020-23"), + ], + TOYOTA_HIGHLANDER.specs, + ) + TOYOTA_PRIUS = PlatformConfig( + [ + ToyotaCarDocs("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ToyotaCarDocs("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ToyotaCarDocs("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"), + ], + CarSpecs(mass=3045. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.74, tireStiffnessFactor=0.6371), + dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + ) + TOYOTA_PRIUS_V = PlatformConfig( + [ToyotaCarDocs("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED)], + CarSpecs(mass=3340. * CV.LB_TO_KG, wheelbase=2.78, steerRatio=17.4, tireStiffnessFactor=0.5533), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU, + ) + TOYOTA_PRIUS_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ToyotaCarDocs("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"), + ], + CarSpecs(mass=3115. * CV.LB_TO_KG, wheelbase=2.70002, steerRatio=13.4, tireStiffnessFactor=0.6371), + ) + TOYOTA_RAV4 = PlatformConfig( + [ + ToyotaCarDocs("Toyota RAV4 2016", "Toyota Safety Sense P"), + ToyotaCarDocs("Toyota RAV4 2017-18") + ], + CarSpecs(mass=3650. * CV.LB_TO_KG, wheelbase=2.65, steerRatio=16.88, tireStiffnessFactor=0.5533), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + TOYOTA_RAV4H = PlatformConfig( + [ + ToyotaCarDocs("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26") + ], + TOYOTA_RAV4.specs, + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + # Note that the ICE RAV4 does not respect positive acceleration commands under 19 mph + flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU, + ) + TOYOTA_RAV4_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2019-21"), + ], + CarSpecs(mass=3585. * CV.LB_TO_KG, wheelbase=2.68986, steerRatio=14.3, tireStiffnessFactor=0.7933), + ) + TOYOTA_RAV4_TSS2_2022 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota RAV4 2022"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"), + ], + TOYOTA_RAV4_TSS2.specs, + flags=ToyotaFlags.RADAR_ACC, + ) + TOYOTA_RAV4_TSS2_2023 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Toyota RAV4 2023-24"), + ToyotaCarDocs("Toyota RAV4 Hybrid 2023-24"), + ], + TOYOTA_RAV4_TSS2.specs, + flags=ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL, + ) + TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( # TSS 2.5 + [ToyotaCarDocs("Toyota Mirai 2021")], + CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8), + ) + TOYOTA_SIENNA = PlatformConfig( + [ToyotaCarDocs("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED)], + CarSpecs(mass=4590. * CV.LB_TO_KG, wheelbase=3.03, steerRatio=15.5, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.NO_STOP_TIMER, + ) # Lexus - CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"), - CAR.LEXUS_ES: [ - ToyotaCarInfo("Lexus ES 2017-18"), - ToyotaCarInfo("Lexus ES Hybrid 2017-18"), - ], - CAR.LEXUS_ES_TSS2: [ - ToyotaCarInfo("Lexus ES 2019-24"), - ToyotaCarInfo("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), - ], - CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"), - CAR.LEXUS_IS_TSS2: ToyotaCarInfo("Lexus IS 2022-23"), - CAR.LEXUS_GS_F: ToyotaCarInfo("Lexus GS F 2016"), - CAR.LEXUS_NX: [ - ToyotaCarInfo("Lexus NX 2018-19"), - ToyotaCarInfo("Lexus NX Hybrid 2018-19"), - ], - CAR.LEXUS_NX_TSS2: [ - ToyotaCarInfo("Lexus NX 2020-21"), - ToyotaCarInfo("Lexus NX Hybrid 2020-21"), - ], - CAR.LEXUS_LC_TSS2: ToyotaCarInfo("Lexus LC 2024"), - CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"), - CAR.LEXUS_RX: [ - ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"), - ToyotaCarInfo("Lexus RX 2017-19"), - # Hybrid platforms - ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"), - ToyotaCarInfo("Lexus RX Hybrid 2017-19"), - ], - CAR.LEXUS_RX_TSS2: [ - ToyotaCarInfo("Lexus RX 2020-22"), - ToyotaCarInfo("Lexus RX Hybrid 2020-22"), - ], -} + LEXUS_CTH = PlatformConfig( + [ToyotaCarDocs("Lexus CT Hybrid 2017-18", "Lexus Safety System+")], + CarSpecs(mass=3108. * CV.LB_TO_KG, wheelbase=2.6, steerRatio=18.6, tireStiffnessFactor=0.517), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + LEXUS_ES = PlatformConfig( + [ + ToyotaCarDocs("Lexus ES 2017-18"), + ToyotaCarDocs("Lexus ES Hybrid 2017-18"), + ], + CarSpecs(mass=3677. * CV.LB_TO_KG, wheelbase=2.8702, steerRatio=16.0, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + ) + LEXUS_ES_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Lexus ES 2019-24"), + ToyotaCarDocs("Lexus ES Hybrid 2019-24", video_link="https://youtu.be/BZ29osRVJeg?t=12"), + ], + LEXUS_ES.specs, + ) + LEXUS_IS = PlatformConfig( + [ToyotaCarDocs("Lexus IS 2017-19")], + CarSpecs(mass=3736.8 * CV.LB_TO_KG, wheelbase=2.79908, steerRatio=13.3, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + LEXUS_IS_TSS2 = ToyotaTSS2PlatformConfig( + [ToyotaCarDocs("Lexus IS 2022-23")], + LEXUS_IS.specs, + ) + LEXUS_NX = PlatformConfig( + [ + ToyotaCarDocs("Lexus NX 2018-19"), + ToyotaCarDocs("Lexus NX Hybrid 2018-19"), + ], + CarSpecs(mass=4070. * CV.LB_TO_KG, wheelbase=2.66, steerRatio=14.7, tireStiffnessFactor=0.444), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + LEXUS_NX_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Lexus NX 2020-21"), + ToyotaCarDocs("Lexus NX Hybrid 2020-21"), + ], + LEXUS_NX.specs, + ) + LEXUS_LC_TSS2 = ToyotaTSS2PlatformConfig( + [ToyotaCarDocs("Lexus LC 2024")], + CarSpecs(mass=4500. * CV.LB_TO_KG, wheelbase=2.87, steerRatio=13.0, tireStiffnessFactor=0.444), + ) + LEXUS_RC = PlatformConfig( + [ToyotaCarDocs("Lexus RC 2018-20")], + LEXUS_IS.specs, + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + LEXUS_RX = PlatformConfig( + [ + ToyotaCarDocs("Lexus RX 2016", "Lexus Safety System+"), + ToyotaCarDocs("Lexus RX 2017-19"), + # Hybrid platforms + ToyotaCarDocs("Lexus RX Hybrid 2016", "Lexus Safety System+"), + ToyotaCarDocs("Lexus RX Hybrid 2017-19"), + ], + CarSpecs(mass=4481. * CV.LB_TO_KG, wheelbase=2.79, steerRatio=16., tireStiffnessFactor=0.5533), + dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + ) + LEXUS_RX_TSS2 = ToyotaTSS2PlatformConfig( + [ + ToyotaCarDocs("Lexus RX 2020-22"), + ToyotaCarDocs("Lexus RX Hybrid 2020-22"), + ], + LEXUS_RX.specs, + ) + LEXUS_GS_F = PlatformConfig( + [ToyotaCarDocs("Lexus GS F 2016")], + CarSpecs(mass=4034. * CV.LB_TO_KG, wheelbase=2.84988, steerRatio=13.3, tireStiffnessFactor=0.444), + dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + flags=ToyotaFlags.UNSUPPORTED_DSU, + ) + # (addr, cars, bus, 1/freq*100, vl) STATIC_DSU_MSGS = [ - (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), - (0x128, (CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, - CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), - (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, - CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.PRIUS_V), + (0x128, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON), \ + 1, 3, b'\xf4\x01\x90\x83\x00\x37'), + (0x128, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 3, b'\x03\x00\x20\x00\x00\x52'), + (0x141, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), + (0x160, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), - (0X161, (CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, - CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), - (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), - (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), - (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, - CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), - (0x365, (CAR.PRIUS, CAR.LEXUS_NX, CAR.HIGHLANDER), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), - (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_RX, - CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), - (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.HIGHLANDER), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, (CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), + (0X161, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), + (0x283, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x2E6, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), + (0x2E7, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), + (0x33E, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), + (0x344, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x365, (CAR.TOYOTA_PRIUS, CAR.LEXUS_NX, CAR.TOYOTA_HIGHLANDER), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), + (0x365, (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_RX, + CAR.TOYOTA_PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x366, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_HIGHLANDER), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), + (0x366, (CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), - (0x470, (CAR.PRIUS, CAR.LEXUS_RX), 1, 100, b'\x00\x00\x02\x7a'), - (0x470, (CAR.HIGHLANDER, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, - CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, (CAR.TOYOTA_PRIUS, CAR.LEXUS_RX), 1, 100, b'\x00\x00\x02\x7a'), + (0x470, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_RAV4H, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON, + CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] -def get_platform_codes(fw_versions: List[bytes]) -> Dict[bytes, Set[bytes]]: +def get_platform_codes(fw_versions: list[bytes]) -> dict[bytes, set[bytes]]: # Returns sub versions in a dict so comparisons can be made within part-platform-major_version combos codes = defaultdict(set) # Optional[part]-platform-major_version: set of sub_version for fw in fw_versions: @@ -297,7 +415,7 @@ def get_platform_codes(fw_versions: List[bytes]) -> Dict[bytes, Set[bytes]]: return dict(codes) -def match_fw_to_car_fuzzy(live_fw_versions, offline_fw_versions) -> Set[str]: +def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]: candidates = set() for candidate, fws in offline_fw_versions.items(): @@ -376,62 +494,56 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST_KWP], [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE_KWP], - whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps, Ecu.epb, Ecu.telematics, - Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, Ecu.hvac], + whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps, Ecu.srs, Ecu.transmission, Ecu.hvac], bus=0, ), Request( [StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST], [StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.epb, Ecu.telematics, Ecu.hybrid, Ecu.srs, Ecu.combinationMeter, Ecu.transmission, - Ecu.gateway, Ecu.hvac], + whitelist_ecus=[Ecu.engine, Ecu.hybrid, Ecu.srs, Ecu.transmission, Ecu.hvac], bus=0, ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE], - whitelist_ecus=[Ecu.engine, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.abs, Ecu.eps, Ecu.epb, Ecu.telematics, - Ecu.hybrid, Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, Ecu.hvac], + whitelist_ecus=[Ecu.engine, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.abs, Ecu.eps, + Ecu.hybrid, Ecu.srs, Ecu.transmission, Ecu.hvac], bus=0, ), ], non_essential_ecus={ # FIXME: On some models, abs can sometimes be missing - Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2], + Ecu.abs: [CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_IS, CAR.TOYOTA_ALPHARD_TSS2], # On some models, the engine can show on two different addresses - Ecu.engine: [CAR.HIGHLANDER, CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, - CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2], + Ecu.engine: [CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_CAMRY, CAR.TOYOTA_COROLLA_TSS2, CAR.TOYOTA_CHR, CAR.TOYOTA_CHR_TSS2, CAR.LEXUS_IS, + CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2], }, extra_ecus=[ # All known ECUs on a late-model Toyota vehicle not queried here: # Responds to UDS: + # - Combination Meter (0x7c0) # - HV Battery (0x713, 0x747) # - Motor Generator (0x716, 0x724) # - 2nd ABS "Brake/EPB" (0x730) + # - Electronic Parking Brake ((0x750, 0x2c)) + # - Telematics ((0x750, 0xc7)) # Responds to KWP (0x1a8801): # - Steering Angle Sensor (0x7b3) # - EPS/EMPS (0x7a0, 0x7a1) + # - 2nd SRS Airbag (0x784) + # - Central Gateway ((0x750, 0x5f)) + # - Telematics ((0x750, 0xc7)) # Responds to KWP (0x1a8881): # - Body Control Module ((0x750, 0x40)) + # - Telematics ((0x750, 0xc7)) # Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform (Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer - # TODO: if these duplicate ECUs always exist together, remove one (Ecu.srs, 0x780, None), # SRS Airbag - (Ecu.srs, 0x784, None), # SRS Airbag 2 - # Likely only exists on cars where EPB isn't standard (e.g. Camry, Avalon (/Hybrid)) - # On some cars, EPB is controlled by the ABS module - (Ecu.epb, 0x750, 0x2c), # Electronic Parking Brake - # This isn't accessible on all cars - (Ecu.gateway, 0x750, 0x5f), - # On some cars, this only responds to b'\x1a\x88\x81', which is reflected by the b'\x1a\x88\x00' query - (Ecu.telematics, 0x750, 0xc7), # Transmission is combined with engine on some platforms, such as TSS-P RAV4 (Ecu.transmission, 0x701, None), # A few platforms have a tester present response on this address, add to log (Ecu.transmission, 0x7e1, None), - # On some cars, this only responds to b'\x1a\x88\x80' - (Ecu.combinationMeter, 0x7c0, None), (Ecu.hvac, 0x7c4, None), ], match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, @@ -440,62 +552,24 @@ FW_QUERY_CONFIG = FwQueryConfig( STEER_THRESHOLD = 100 -DBC = { - CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_LC_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None), - CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ES: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_GS_F: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), -} - # These cars have non-standard EPS torque scale factors. All others are 73 -EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) +EPS_SCALE = defaultdict(lambda: 73, + {CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.TOYOTA_PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 -TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2, - CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2, - CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.LEXUS_LC_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, - CAR.CHR_TSS2} +TSS2_CAR = CAR.with_flags(ToyotaFlags.TSS2) -NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY} +NO_DSU_CAR = CAR.with_flags(ToyotaFlags.NO_DSU) # the DSU uses the AEB message for longitudinal on these cars -UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC, CAR.LEXUS_GS_F} +UNSUPPORTED_DSU_CAR = CAR.with_flags(ToyotaFlags.UNSUPPORTED_DSU) # these cars have a radar which sends ACC messages instead of the camera -RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2} +RADAR_ACC_CAR = CAR.with_flags(ToyotaFlags.RADAR_ACC) -# these cars use the Lane Tracing Assist (LTA) message for lateral control -ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023} +ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL) # no resume button press required -NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDER, CAR.SIENNA} +NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/car/values.py b/selfdrive/car/values.py new file mode 100644 index 0000000000..bf5d378ab4 --- /dev/null +++ b/selfdrive/car/values.py @@ -0,0 +1,19 @@ +from typing import get_args +from openpilot.selfdrive.car.body.values import CAR as BODY +from openpilot.selfdrive.car.chrysler.values import CAR as CHRYSLER +from openpilot.selfdrive.car.ford.values import CAR as FORD +from openpilot.selfdrive.car.gm.values import CAR as GM +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.hyundai.values import CAR as HYUNDAI +from openpilot.selfdrive.car.mazda.values import CAR as MAZDA +from openpilot.selfdrive.car.mock.values import CAR as MOCK +from openpilot.selfdrive.car.nissan.values import CAR as NISSAN +from openpilot.selfdrive.car.subaru.values import CAR as SUBARU +from openpilot.selfdrive.car.tesla.values import CAR as TESLA +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN + +Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN +BRANDS = get_args(Platform) + +PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in brand} diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index e668c35f7d..e8c9c58042 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -23,6 +23,7 @@ def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): (StdQueries.OBD_VIN_REQUEST, StdQueries.OBD_VIN_RESPONSE, (0, 1), STANDARD_VIN_ADDRS, FUNCTIONAL_ADDRS, 0x8), (StdQueries.GM_VIN_REQUEST, StdQueries.GM_VIN_RESPONSE, (0,), [0x24b], None, 0x400), # Bolt fwdCamera (StdQueries.KWP_VIN_REQUEST, StdQueries.KWP_VIN_RESPONSE, (0,), [0x797], None, 0x3), # Nissan Leaf VCM + (StdQueries.UDS_VIN_REQUEST, StdQueries.UDS_VIN_RESPONSE, (0,), [0x74f], None, 0x6a), # Volkswagen fwdCamera ): if bus not in valid_buses: continue diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 0cee2c7fce..a4e0c8946a 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -4,19 +4,21 @@ from openpilot.common.numpy_fast import clip from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_driver_steer_torque_limits +from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan -from openpilot.selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams, VolkswagenFlags +from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState -class CarController: +class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): self.CP = CP self.CCP = CarControllerParams(CP) - self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan + self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan self.packer_pt = CANPacker(dbc_name) + self.ext_bus = CANBUS.pt if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera else CANBUS.cam self.apply_steer_last = 0 self.gra_acc_counter_last = None @@ -25,7 +27,7 @@ class CarController: self.hca_frame_timer_running = 0 self.hca_frame_same_torque = 0 - def update(self, CC, CS, ext_bus, now_nanos): + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl can_sends = [] @@ -90,7 +92,7 @@ class CarController: hud_alert = 0 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"] - can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, + can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.latActive, CS.out.steeringPressed, hud_alert, hud_control)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: @@ -101,19 +103,19 @@ class CarController: # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? set_speed = hud_control.setSpeed * CV.MS_TO_KPH can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - lead_distance)) + lead_distance, hud_control.leadDistanceBars)) # **** Stock ACC Button Controls **************************************** # gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume): - can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, + can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.ext_bus, CS.gra_stock_values, cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume)) - new_actuators = actuators.copy() + new_actuators = actuators.as_builder() new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 - return new_actuators, can_sends, self.eps_timer_soft_disable_alert + return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 25c5bc04bc..ec6403496f 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -3,13 +3,15 @@ from cereal import car from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ +from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ CarControllerParams, VolkswagenFlags class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + self.frame = 0 + self.eps_init_complete = False self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False @@ -31,7 +33,7 @@ class CarState(CarStateBase): return button_events def update(self, pt_cp, cam_cp, ext_cp, trans_type): - if self.CP.carFingerprint in PQ_CARS: + if self.CP.flags & VolkswagenFlags.PQ: return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) ret = car.CarState.new_message() @@ -47,18 +49,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # VW Emergency Assist status tracking and mitigation self.eps_stock_values = pt_cp.vl["LH_EPS_03"] @@ -116,21 +114,24 @@ class CarState(CarStateBase): # Update ACC radar status. self.acc_type = ext_cp.vl["ACC_06"]["ACC_Typ"] - if pt_cp.vl["TSK_06"]["TSK_Status"] == 2: - # ACC okay and enabled, but not currently engaged - ret.cruiseState.available = True - ret.cruiseState.enabled = False - elif pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5): - # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or brake only (5) - ret.cruiseState.available = True - ret.cruiseState.enabled = True + + # ACC okay but disabled (1), ACC ready (2), a radar visibility or other fault/disruption (6 or 7) + # currently regulating speed (3), driver accel override (4), brake only (5) + ret.cruiseState.available = pt_cp.vl["TSK_06"]["TSK_Status"] in (2, 3, 4, 5) + ret.cruiseState.enabled = pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5) + + if self.CP.pcmCruise: + # Cruise Control mode; check for distance UI setting from the radar. + # ECM does not manage this, so do not need to check for openpilot longitudinal + ret.cruiseState.nonAdaptive = ext_cp.vl["ACC_02"]["ACC_Gesetzte_Zeitluecke"] == 0 else: - # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) - ret.cruiseState.available = False - ret.cruiseState.enabled = False + # Speed limiter mode; ECM faults if we command ACC while not pcmCruise + ret.cruiseState.nonAdaptive = bool(pt_cp.vl["TSK_06"]["TSK_Limiter_ausgewaehlt"]) + + ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) + self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation - ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. @@ -151,6 +152,7 @@ class CarState(CarStateBase): # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + self.frame += 1 return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): @@ -168,18 +170,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["Lenkhilfe_3"]["LH3_BLW"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"])] ret.steeringRateDeg = pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])] ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # Update gas, brakes, and gearshift. ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 @@ -253,11 +251,20 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + self.frame += 1 return ret + def update_hca_state(self, hca_status): + # Treat INITIALIZING and FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist + # DISABLED means the EPS hasn't been configured to support Lane Assist + self.eps_init_complete = self.eps_init_complete or (hca_status in ("DISABLED", "READY", "ACTIVE") or self.frame > 600) + perm_fault = hca_status == "DISABLED" or (self.eps_init_complete and hca_status in ("INITIALIZING", "FAULT")) + temp_fault = hca_status in ("REJECTED", "PREEMPTED") or not self.eps_init_complete + return temp_fault, perm_fault + @staticmethod def get_can_parser(CP): - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: return CarState.get_can_parser_pq(CP) messages = [ @@ -294,7 +301,7 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: return CarState.get_cam_can_parser_pq(CP) messages = [] diff --git a/selfdrive/car/volkswagen/fingerprints.py b/selfdrive/car/volkswagen/fingerprints.py index eab2bc0090..fea530b29b 100644 --- a/selfdrive/car/volkswagen/fingerprints.py +++ b/selfdrive/car/volkswagen/fingerprints.py @@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu FW_VERSIONS = { - CAR.ARTEON_MK1: { + CAR.VOLKSWAGEN_ARTEON_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x873G0906259AH\xf1\x890001', b'\xf1\x873G0906259F \xf1\x890004', @@ -49,7 +49,7 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, - CAR.ATLAS_MK1: { + CAR.VOLKSWAGEN_ATLAS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703H906026AA\xf1\x899970', b'\xf1\x8703H906026AG\xf1\x899973', @@ -60,6 +60,7 @@ FW_VERSIONS = { b'\xf1\x8703H906026F \xf1\x896696', b'\xf1\x8703H906026F \xf1\x899970', b'\xf1\x8703H906026J \xf1\x896026', + b'\xf1\x8703H906026J \xf1\x899970', b'\xf1\x8703H906026J \xf1\x899971', b'\xf1\x8703H906026S \xf1\x896693', b'\xf1\x8703H906026S \xf1\x899970', @@ -80,6 +81,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655BN\xf1\x890713\xf1\x82\x0e2214152212001105141122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\x0e1114151112001105111122052900', b'\xf1\x873Q0959655DB\xf1\x890720\xf1\x82\x0e2214152212001105141122052900', + b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105111122052J00', b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1114151112001105161122052J00', b'\xf1\x873Q0959655DM\xf1\x890732\xf1\x82\x0e1115151112001105171122052J00', ], @@ -87,6 +89,7 @@ FW_VERSIONS = { b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B60924A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6G920A1', b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6M921A1', + b'\xf1\x873QF909144B \xf1\x891582\xf1\x82\x0571B6N920A1', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6080105', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820528B6090105', ], @@ -97,13 +100,26 @@ FW_VERSIONS = { b'\xf1\x875Q0907572H \xf1\x890620', b'\xf1\x875Q0907572J \xf1\x890654', b'\xf1\x875Q0907572P \xf1\x890682', + b'\xf1\x875Q0907572S \xf1\x890780', + ], + }, + CAR.VOLKSWAGEN_CADDY_MK3: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906027T \xf1\x892363', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x872K5959655E \xf1\x890018\xf1\x82\x05000P037605', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0155', ], }, - CAR.CRAFTER_MK2: { + CAR.VOLKSWAGEN_CRAFTER_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906056BP\xf1\x894729', b'\xf1\x8704L906056EK\xf1\x896391', b'\xf1\x8705L906023BC\xf1\x892688', + b'\xf1\x8705L906023MH\xf1\x892588', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AL\xf1\x890505\xf1\x82\x0e1411001413001203151311031100', @@ -113,6 +129,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x872N0909143D\x00\xf1\x897010\xf1\x82\x05183AZ306A2', b'\xf1\x872N0909143E \xf1\x897021\xf1\x82\x05163AZ306A2', + b'\xf1\x872N0909143H \xf1\x897045\xf1\x82\x05263AZ309A2', b'\xf1\x872N0909144K \xf1\x897045\xf1\x82\x05233AZ810A2', ], (Ecu.fwdRadar, 0x757, None): [ @@ -121,7 +138,7 @@ FW_VERSIONS = { b'\xf1\x872Q0907572M \xf1\x890233', ], }, - CAR.GOLF_MK7: { + CAR.VOLKSWAGEN_GOLF_MK7: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906016A \xf1\x897697', b'\xf1\x8704E906016AD\xf1\x895758', @@ -148,6 +165,7 @@ FW_VERSIONS = { b'\xf1\x8704L906056HE\xf1\x893758', b'\xf1\x8704L906056HN\xf1\x896590', b'\xf1\x8704L906056HT\xf1\x896591', + b'\xf1\x8704L997022N \xf1\x899459', b'\xf1\x870EA906016A \xf1\x898343', b'\xf1\x870EA906016E \xf1\x894219', b'\xf1\x870EA906016F \xf1\x894238', @@ -164,6 +182,7 @@ FW_VERSIONS = { b'\xf1\x875G0906259T \xf1\x890003', b'\xf1\x878V0906259H \xf1\x890002', b'\xf1\x878V0906259J \xf1\x890003', + b'\xf1\x878V0906259J \xf1\x890103', b'\xf1\x878V0906259K \xf1\x890001', b'\xf1\x878V0906259K \xf1\x890003', b'\xf1\x878V0906259P \xf1\x890001', @@ -198,17 +217,21 @@ FW_VERSIONS = { b'\xf1\x870D9300012 \xf1\x895046', b'\xf1\x870D9300014M \xf1\x895004', b'\xf1\x870D9300014Q \xf1\x895006', + b'\xf1\x870D9300018 \xf1\x895201', b'\xf1\x870D9300020J \xf1\x894902', b'\xf1\x870D9300020Q \xf1\x895201', b'\xf1\x870D9300020S \xf1\x895201', b'\xf1\x870D9300040A \xf1\x893613', b'\xf1\x870D9300040S \xf1\x894311', b'\xf1\x870D9300041H \xf1\x895220', + b'\xf1\x870D9300041N \xf1\x894512', b'\xf1\x870D9300041P \xf1\x894507', b'\xf1\x870DD300045K \xf1\x891120', b'\xf1\x870DD300046F \xf1\x891601', b'\xf1\x870GC300012A \xf1\x891401', b'\xf1\x870GC300012A \xf1\x891403', + b'\xf1\x870GC300012A \xf1\x891422', + b'\xf1\x870GC300012M \xf1\x892301', b'\xf1\x870GC300014B \xf1\x892401', b'\xf1\x870GC300014B \xf1\x892403', b'\xf1\x870GC300014B \xf1\x892405', @@ -227,6 +250,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655AR\xf1\x890317\xf1\x82\x13141500111233003142114A2131219333313100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333423100', b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\x1314160011123300314211012230229333463100', + b'\xf1\x875Q0959655BJ\xf1\x890339\xf1\x82\x13141600111233003142115A2232229333463100', b'\xf1\x875Q0959655BS\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2251229333463100', b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x13141600111233003142404A2252229333463100', @@ -235,6 +259,7 @@ FW_VERSIONS = { b'\xf1\x875Q0959655C \xf1\x890361\xf1\x82\x111413001112120004110415121610169112', b'\xf1\x875Q0959655CA\xf1\x890403\xf1\x82\x1314160011123300314240012250229333463100', b'\xf1\x875Q0959655D \xf1\x890388\xf1\x82\x111413001113120006110417121A101A9113', + b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\x13271112111312--071104171825102591131211', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13271112111312--071104171825102591131211', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13271212111312--071104171838103891131211', b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\x13272512111312--07110417182C102C91131211', @@ -285,6 +310,7 @@ FW_VERSIONS = { b'\xf1\x875QD909144B \xf1\x891072\xf1\x82\x0521A00507A1', b'\xf1\x875QM909144A \xf1\x891072\xf1\x82\x0521A20B03A1', b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A00442A1', + b'\xf1\x875QM909144B \xf1\x891081\xf1\x82\x0521A00642A1', b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A16A1', b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A17A1', b'\xf1\x875QN909144A \xf1\x895081\xf1\x82\x0571A01A18A1', @@ -309,7 +335,7 @@ FW_VERSIONS = { b'\xf1\x875Q0907572S \xf1\x890780', ], }, - CAR.JETTA_MK7: { + CAR.VOLKSWAGEN_JETTA_MK7: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906024AK\xf1\x899937', b'\xf1\x8704E906024AS\xf1\x899912', @@ -317,6 +343,7 @@ FW_VERSIONS = { b'\xf1\x8704E906024BC\xf1\x899971', b'\xf1\x8704E906024BG\xf1\x891057', b'\xf1\x8704E906024C \xf1\x899970', + b'\xf1\x8704E906024C \xf1\x899971', b'\xf1\x8704E906024L \xf1\x895595', b'\xf1\x8704E906024L \xf1\x899970', b'\xf1\x8704E906027MS\xf1\x896223', @@ -360,7 +387,7 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, - CAR.PASSAT_MK8: { + CAR.VOLKSWAGEN_PASSAT_MK8: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703N906026E \xf1\x892114', b'\xf1\x8704E906023AH\xf1\x893379', @@ -369,12 +396,15 @@ FW_VERSIONS = { b'\xf1\x8704L906026ET\xf1\x891990', b'\xf1\x8704L906026FP\xf1\x892012', b'\xf1\x8704L906026GA\xf1\x892013', + b'\xf1\x8704L906026GK\xf1\x899971', b'\xf1\x8704L906026KD\xf1\x894798', + b'\xf1\x8705L906022A \xf1\x890827', b'\xf1\x873G0906259 \xf1\x890004', b'\xf1\x873G0906259B \xf1\x890002', b'\xf1\x873G0906264 \xf1\x890004', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300041E \xf1\x891006', b'\xf1\x870CW300042H \xf1\x891601', b'\xf1\x870CW300042H \xf1\x891607', b'\xf1\x870CW300043H \xf1\x891601', @@ -389,6 +419,7 @@ FW_VERSIONS = { b'\xf1\x870DL300011H \xf1\x895201', b'\xf1\x870GC300042H \xf1\x891404', b'\xf1\x870GC300043 \xf1\x892301', + b'\xf1\x870GC300046P \xf1\x892805', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AE\xf1\x890195\xf1\x82\r56140056130012416612124111', @@ -404,6 +435,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\x0e5915005914001354701311542900', b'\xf1\x873Q0959655CN\xf1\x890720\xf1\x82\x0e5915005914001305701311052900', b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\x1315120011111200631145171716121691132111', + b'\xf1\x875QF959655S \xf1\x890639\xf1\x82\x13131100131300111111000120----2211114A48', ], (Ecu.eps, 0x712, None): [ b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566B00611A1', @@ -412,6 +444,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0060803', b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803', b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820526B0060905', + b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521B00606A1', b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\x0516B00501A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521B00603A1', @@ -428,9 +461,10 @@ FW_VERSIONS = { b'\xf1\x873Q0907572C \xf1\x890196', b'\xf1\x875Q0907572P \xf1\x890682', b'\xf1\x875Q0907572R \xf1\x890771', + b'\xf1\x875Q0907572S \xf1\x890780', ], }, - CAR.PASSAT_NMS: { + CAR.VOLKSWAGEN_PASSAT_NMS: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8706K906016C \xf1\x899609', b'\xf1\x8706K906016E \xf1\x899830', @@ -452,7 +486,7 @@ FW_VERSIONS = { b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0152', ], }, - CAR.POLO_MK6: { + CAR.VOLKSWAGEN_POLO_MK6: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704C906025H \xf1\x895177', b'\xf1\x8705C906032J \xf1\x891702', @@ -476,7 +510,7 @@ FW_VERSIONS = { b'\xf1\x872Q0907572R \xf1\x890372', ], }, - CAR.SHARAN_MK2: { + CAR.VOLKSWAGEN_SHARAN_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906016HE\xf1\x894635', ], @@ -487,7 +521,7 @@ FW_VERSIONS = { b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0153', ], }, - CAR.TAOS_MK1: { + CAR.VOLKSWAGEN_TAOS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906025CK\xf1\x892228', b'\xf1\x8704E906027NJ\xf1\x891445', @@ -517,7 +551,7 @@ FW_VERSIONS = { b'\xf1\x872Q0907572T \xf1\x890383', ], }, - CAR.TCROSS_MK1: { + CAR.VOLKSWAGEN_TCROSS_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704C906025AK\xf1\x897053', ], @@ -534,7 +568,7 @@ FW_VERSIONS = { b'\xf1\x872Q0907572T \xf1\x890383', ], }, - CAR.TIGUAN_MK2: { + CAR.VOLKSWAGEN_TIGUAN_MK2: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8703N906026D \xf1\x893680', b'\xf1\x8704E906024AP\xf1\x891461', @@ -557,6 +591,7 @@ FW_VERSIONS = { b'\xf1\x8783A907115Q \xf1\x890001', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158DS\xf1\x893699', b'\xf1\x8709G927158DT\xf1\x893698', b'\xf1\x8709G927158FM\xf1\x893757', b'\xf1\x8709G927158GC\xf1\x893821', @@ -564,6 +599,7 @@ FW_VERSIONS = { b'\xf1\x8709G927158GM\xf1\x893936', b'\xf1\x8709G927158GN\xf1\x893938', b'\xf1\x8709G927158HB\xf1\x894069', + b'\xf1\x8709G927158HC\xf1\x894070', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DD300046K \xf1\x892302', b'\xf1\x870DL300011N \xf1\x892001', @@ -620,29 +656,37 @@ FW_VERSIONS = { b'\xf1\x872Q0907572T \xf1\x890383', ], }, - CAR.TOURAN_MK2: { + CAR.VOLKSWAGEN_TOURAN_MK2: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704E906025BE\xf1\x890720', + b'\xf1\x8704E906027HQ\xf1\x893746', b'\xf1\x8704L906026HM\xf1\x893017', b'\xf1\x8705E906018CQ\xf1\x890808', ], (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x870CW300020A \xf1\x891936', b'\xf1\x870CW300041E \xf1\x891005', + b'\xf1\x870CW300041Q \xf1\x891606', b'\xf1\x870CW300051M \xf1\x891926', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\x1336350021353335314132014730479333313100', b'\xf1\x875Q0959655AS\xf1\x890318\xf1\x82\x13363500213533353141324C4732479333313100', b'\xf1\x875Q0959655CH\xf1\x890421\xf1\x82\x1336350021353336314740025250529333613100', + b'\xf1\x875QD959655AJ\xf1\x890421\xf1\x82\x1336350021313300314240023330339333663100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820531B0062105', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567A8090400', + b'\xf1\x875QD909144F \xf1\x891082\xf1\x82\x0521A00642A1', ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x873Q0907572C \xf1\x890195', + b'\xf1\x875Q0907572R \xf1\x890771', ], }, - CAR.TRANSPORTER_T61: { + CAR.VOLKSWAGEN_TRANSPORTER_T61: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906056AG\xf1\x899970', b'\xf1\x8704L906056AL\xf1\x899970', @@ -674,7 +718,7 @@ FW_VERSIONS = { b'\xf1\x872Q0907572R \xf1\x890372', ], }, - CAR.TROC_MK1: { + CAR.VOLKSWAGEN_TROC_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8705E906018AT\xf1\x899640', b'\xf1\x8705E906018CK\xf1\x890863', @@ -691,13 +735,16 @@ FW_VERSIONS = { b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\x1311110012333300314240681152119333463100', b'\xf1\x875Q0959655CF\xf1\x890421\xf1\x82\x1311110012333300314240021150119333613100', b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1152119333613100', + b'\xf1\x875Q0959655CG\xf1\x890421\xf1\x82\x13111100123333003142404M1154119333613100', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521060403A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521060405A1', b'\xf1\x875WA907144M \xf1\x891051\xf1\x82\x001T06081T7N', + b'\xf1\x875WA907144Q \xf1\x891063\xf1\x82\x001O06081OOM', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', b'\xf1\x872Q0907572M \xf1\x890233', b'\xf1\x872Q0907572T \xf1\x890383', ], @@ -842,28 +889,13 @@ FW_VERSIONS = { CAR.SEAT_ATECA_MK1: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027KA\xf1\x893749', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x870D9300014S \xf1\x895202', - ], - (Ecu.srs, 0x715, None): [ - b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1212001211001305121211052900', - ], - (Ecu.eps, 0x712, None): [ - b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571N60511A1', - ], - (Ecu.fwdRadar, 0x757, None): [ - b'\xf1\x872Q0907572M \xf1\x890233', - ], - }, - CAR.SEAT_LEON_MK3: { - (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704L906021EL\xf1\x897542', b'\xf1\x8704L906026BP\xf1\x891198', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906056CR\xf1\x892181', b'\xf1\x8704L906056CR\xf1\x892797', b'\xf1\x8705E906018AS\xf1\x899596', + b'\xf1\x8781A906259B \xf1\x890003', b'\xf1\x878V0906264H \xf1\x890005', b'\xf1\x878V0907115E \xf1\x890002', ], @@ -871,25 +903,33 @@ FW_VERSIONS = { b'\xf1\x870CW300041D \xf1\x891004', b'\xf1\x870CW300041G \xf1\x891003', b'\xf1\x870CW300050J \xf1\x891908', + b'\xf1\x870D9300014S \xf1\x895202', b'\xf1\x870D9300042M \xf1\x895016', + b'\xf1\x870GC300014P \xf1\x892801', b'\xf1\x870GC300043A \xf1\x892304', ], (Ecu.srs, 0x715, None): [ b'\xf1\x873Q0959655AC\xf1\x890189\xf1\x82\r11110011110011021511110200', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r11110011110011021511110200', b'\xf1\x873Q0959655AS\xf1\x890200\xf1\x82\r12110012120012021612110200', + b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1212001211001305121211052900', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1312001313001305171311052900', b'\xf1\x873Q0959655BH\xf1\x890712\xf1\x82\x0e1312001313001305171311052900', b'\xf1\x873Q0959655CM\xf1\x890720\xf1\x82\x0e1312001313001305171311052900', + b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100110200--1113121149', ], (Ecu.eps, 0x712, None): [ + b'\xf1\x873Q0909144L \xf1\x895081\xf1\x82\x0571N60511A1', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521N01842A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521N01342A1', b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\x0511N01805A0', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521N01309A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521N05808A1', + b'\xf1\x875WA907145M \xf1\x891051\xf1\x82\x0013N619137N', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x872Q0907572AA\xf1\x890396', + b'\xf1\x872Q0907572M \xf1\x890233', b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\x0101', b'\xf1\x875Q0907572H \xf1\x890620', b'\xf1\x875Q0907572K \xf1\x890402\xf1\x82\x0101', @@ -916,15 +956,22 @@ FW_VERSIONS = { }, CAR.SKODA_KAMIQ_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704C906025AK\xf1\x897053', b'\xf1\x8705C906032M \xf1\x891333', + b'\xf1\x8705C906032M \xf1\x892365', b'\xf1\x8705E906013CK\xf1\x892540', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870CW300020 \xf1\x891906', + b'\xf1\x870CW300020 \xf1\x891907', b'\xf1\x870CW300020T \xf1\x892204', + b'\xf1\x870CW300050 \xf1\x891709', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1211110411110411--04040404131111112H14', + b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\x12111104111104112104040404111111112H14', b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\x122221042111042121040404042E2711152H14', + b'\xf1\x872Q0959655AS\xf1\x890411\xf1\x82\x1311150411110411210404040417151215391413', b'\xf1\x872Q0959655BJ\xf1\x890412\xf1\x82\x132223042111042121040404042B251215391423', ], (Ecu.eps, 0x712, None): [ @@ -933,11 +980,13 @@ FW_VERSIONS = { ], (Ecu.fwdRadar, 0x757, None): [ b'\xf1\x872Q0907572AA\xf1\x890396', + b'\xf1\x872Q0907572R \xf1\x890372', b'\xf1\x872Q0907572T \xf1\x890383', ], }, CAR.SKODA_KAROQ_MK1: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8705E906013CL\xf1\x892541', b'\xf1\x8705E906013H \xf1\x892407', b'\xf1\x8705E906018P \xf1\x895472', b'\xf1\x8705E906018P \xf1\x896020', @@ -957,6 +1006,7 @@ FW_VERSIONS = { (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0910143B \xf1\x892201\xf1\x82\x0563T6090500', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100500', + b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100600', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T6100700', ], (Ecu.fwdRadar, 0x757, None): [ @@ -977,7 +1027,9 @@ FW_VERSIONS = { b'\xf1\x8704L906026HT\xf1\x893617', b'\xf1\x8705E906018DJ\xf1\x890915', b'\xf1\x8705E906018DJ\xf1\x891903', + b'\xf1\x8705L906022GM\xf1\x893411', b'\xf1\x875NA906259E \xf1\x890003', + b'\xf1\x875NA907115D \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890003', b'\xf1\x875NA907115E \xf1\x890005', b'\xf1\x8783A907115E \xf1\x890001', @@ -986,14 +1038,17 @@ FW_VERSIONS = { b'\xf1\x870D9300014S \xf1\x895201', b'\xf1\x870D9300043 \xf1\x895202', b'\xf1\x870DL300011N \xf1\x892014', + b'\xf1\x870DL300012G \xf1\x892006', b'\xf1\x870DL300012M \xf1\x892107', b'\xf1\x870DL300012N \xf1\x892110', b'\xf1\x870DL300013G \xf1\x892119', b'\xf1\x870GC300014N \xf1\x892801', + b'\xf1\x870GC300018S \xf1\x892803', b'\xf1\x870GC300019H \xf1\x892806', b'\xf1\x870GC300046Q \xf1\x892802', ], (Ecu.srs, 0x715, None): [ + b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r11110011110011031111310311', b'\xf1\x873Q0959655AP\xf1\x890306\xf1\x82\r11110011110011421111314211', b'\xf1\x873Q0959655BH\xf1\x890703\xf1\x82\x0e1213001211001205212111052100', b'\xf1\x873Q0959655BJ\xf1\x890703\xf1\x82\x0e1213001211001205212111052100', @@ -1002,6 +1057,7 @@ FW_VERSIONS = { b'\xf1\x873Q0959655CQ\xf1\x890720\xf1\x82\x0e1213111211001205212112052111', b'\xf1\x873Q0959655DJ\xf1\x890731\xf1\x82\x0e1513001511001205232113052J00', b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100010200--1121240749', + b'\xf1\x875QF959655AT\xf1\x890755\xf1\x82\x1311110011110011111100010200--1121246149', ], (Ecu.eps, 0x712, None): [ b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6050405', @@ -1009,6 +1065,7 @@ FW_VERSIONS = { b'\xf1\x875Q0909143P \xf1\x892051\xf1\x820527T6070405', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G500', b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\x0567T600G600', + b'\xf1\x875TA907145F \xf1\x891063\xf1\x82\x0025T6BA25OM', b'\xf1\x875TA907145F \xf1\x891063\xf1\x82\x002LT61A2LOM', ], (Ecu.fwdRadar, 0x757, None): [ @@ -1027,8 +1084,10 @@ FW_VERSIONS = { b'\xf1\x8704E906027HD\xf1\x893742', b'\xf1\x8704E906027MH\xf1\x894786', b'\xf1\x8704L906021DT\xf1\x898127', + b'\xf1\x8704L906021ER\xf1\x898361', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026BS\xf1\x891541', + b'\xf1\x8704L906026BT\xf1\x897612', b'\xf1\x875G0906259C \xf1\x890002', ], (Ecu.transmission, 0x7e1, None): [ @@ -1036,6 +1095,8 @@ FW_VERSIONS = { b'\xf1\x870CW300041N \xf1\x891605', b'\xf1\x870CW300043B \xf1\x891601', b'\xf1\x870CW300043P \xf1\x891605', + b'\xf1\x870D9300012H \xf1\x894518', + b'\xf1\x870D9300014T \xf1\x895221', b'\xf1\x870D9300041C \xf1\x894936', b'\xf1\x870D9300041H \xf1\x895220', b'\xf1\x870D9300041J \xf1\x894902', @@ -1056,11 +1117,13 @@ FW_VERSIONS = { b'\xf1\x873Q0909144J \xf1\x895063\xf1\x82\x0566A01513A1', b'\xf1\x875Q0909144AA\xf1\x891081\xf1\x82\x0521T00403A1', b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00403A1', + b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\x0521T00603A1', b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\x0516A00604A1', b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\x0521T00601A1', b'\xf1\x875QD909144E \xf1\x891081\xf1\x82\x0521T00503A1', ], (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x875Q0907567P \xf1\x890100\xf1\x82\x0101', b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\x0101', b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\x0101', b'\xf1\x875Q0907572H \xf1\x890620', @@ -1070,29 +1133,6 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, - CAR.SKODA_SCALA_MK1: { - (Ecu.engine, 0x7e0, None): [ - b'\xf1\x8704C906025AK\xf1\x897053', - b'\xf1\x8705C906032M \xf1\x892365', - ], - (Ecu.transmission, 0x7e1, None): [ - b'\xf1\x870CW300020 \xf1\x891907', - b'\xf1\x870CW300050 \xf1\x891709', - ], - (Ecu.srs, 0x715, None): [ - b'\xf1\x872Q0959655AJ\xf1\x890250\xf1\x82\x1211110411110411--04040404131111112H14', - b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\x12111104111104112104040404111111112H14', - b'\xf1\x872Q0959655AS\xf1\x890411\xf1\x82\x1311150411110411210404040417151215391413', - ], - (Ecu.eps, 0x712, None): [ - b'\xf1\x872Q1909144AB\xf1\x896050', - b'\xf1\x872Q1909144M \xf1\x896041', - ], - (Ecu.fwdRadar, 0x757, None): [ - b'\xf1\x872Q0907572AA\xf1\x890396', - b'\xf1\x872Q0907572R \xf1\x890372', - ], - }, CAR.SKODA_SUPERB_MK3: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704E906027BS\xf1\x892887', diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 710e779d0a..91cd300e92 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,9 +1,8 @@ from cereal import car from panda import Panda -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags +from openpilot.selfdrive.car.volkswagen.values import CAR, CANBUS, CarControllerParams, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -20,14 +19,12 @@ class CarInterface(CarInterfaceBase): self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam - self.eps_timer_soft_disable_alert = False - @staticmethod - def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): + def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, docs): ret.carName = "volkswagen" ret.radarUnavailable = True - if candidate in PQ_CARS: + if ret.flags & VolkswagenFlags.PQ: # Set global PQ35/PQ46/NMS parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)] ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1 @@ -72,14 +69,17 @@ class CarInterface(CarInterfaceBase): # Global lateral tuning defaults, can be overridden per-vehicle - ret.steerActuatorDelay = 0.1 ret.steerLimitTimer = 0.4 - ret.steerRatio = 15.6 # Let the params learner figure this out - ret.lateralTuning.pid.kpBP = [0.] - ret.lateralTuning.pid.kiBP = [0.] - ret.lateralTuning.pid.kf = 0.00006 - ret.lateralTuning.pid.kpV = [0.6] - ret.lateralTuning.pid.kiV = [0.2] + if ret.flags & VolkswagenFlags.PQ: + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + else: + ret.steerActuatorDelay = 0.1 + ret.lateralTuning.pid.kpBP = [0.] + ret.lateralTuning.pid.kiBP = [0.] + ret.lateralTuning.pid.kf = 0.00006 + ret.lateralTuning.pid.kpV = [0.6] + ret.lateralTuning.pid.kiV = [0.2] # Global longitudinal tuning defaults, can be overridden per-vehicle @@ -98,130 +98,8 @@ class CarInterface(CarInterfaceBase): ret.vEgoStopping = 0.5 ret.longitudinalTuning.kpV = [0.1] ret.longitudinalTuning.kiV = [0.0] - - # Per-chassis tuning values, override tuning defaults here if desired - - if candidate == CAR.ARTEON_MK1: - ret.mass = 1733 - ret.wheelbase = 2.84 - - elif candidate == CAR.ATLAS_MK1: - ret.mass = 2011 - ret.wheelbase = 2.98 - - elif candidate == CAR.CRAFTER_MK2: - 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 - ret.wheelbase = 2.62 - - elif candidate == CAR.JETTA_MK7: - ret.mass = 1328 - ret.wheelbase = 2.71 - - elif candidate == CAR.PASSAT_MK8: - ret.mass = 1551 - ret.wheelbase = 2.79 - - elif candidate == CAR.PASSAT_NMS: - 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 - ret.steerActuatorDelay = 0.2 - CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) - - elif candidate == CAR.POLO_MK6: - ret.mass = 1230 - ret.wheelbase = 2.55 - - elif candidate == CAR.SHARAN_MK2: - 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 - ret.wheelbase = 2.69 - - elif candidate == CAR.TCROSS_MK1: - ret.mass = 1150 - ret.wheelbase = 2.60 - - elif candidate == CAR.TIGUAN_MK2: - ret.mass = 1715 - ret.wheelbase = 2.74 - - elif candidate == CAR.TOURAN_MK2: - ret.mass = 1516 - ret.wheelbase = 2.79 - - elif candidate == CAR.TRANSPORTER_T61: - 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 - ret.wheelbase = 2.63 - - elif candidate == CAR.AUDI_A3_MK3: - ret.mass = 1335 - ret.wheelbase = 2.61 - - elif candidate == CAR.AUDI_Q2_MK1: - ret.mass = 1205 - ret.wheelbase = 2.61 - - elif candidate == CAR.AUDI_Q3_MK2: - ret.mass = 1623 - ret.wheelbase = 2.68 - - elif candidate == CAR.SEAT_ATECA_MK1: - ret.mass = 1900 - ret.wheelbase = 2.64 - - elif candidate == CAR.SEAT_LEON_MK3: - ret.mass = 1227 - ret.wheelbase = 2.64 - - elif candidate == CAR.SKODA_FABIA_MK4: - ret.mass = 1266 - ret.wheelbase = 2.56 - - elif candidate == CAR.SKODA_KAMIQ_MK1: - ret.mass = 1265 - ret.wheelbase = 2.66 - - elif candidate == CAR.SKODA_KAROQ_MK1: - ret.mass = 1278 - ret.wheelbase = 2.66 - - elif candidate == CAR.SKODA_KODIAQ_MK1: - ret.mass = 1569 - ret.wheelbase = 2.79 - - elif candidate == CAR.SKODA_OCTAVIA_MK3: - ret.mass = 1388 - ret.wheelbase = 2.68 - - elif candidate == CAR.SKODA_SCALA_MK1: - ret.mass = 1192 - ret.wheelbase = 2.65 - - elif candidate == CAR.SKODA_SUPERB_MK3: - ret.mass = 1505 - ret.wheelbase = 2.84 - - else: - raise ValueError(f"unsupported car {candidate}") - ret.autoResumeSng = ret.minEnableSpeed == -1 - ret.centerToFront = ret.wheelbase * 0.45 + return ret # returns a car.CarState @@ -233,7 +111,7 @@ class CarInterface(CarInterfaceBase): enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) # Low speed steer alert hysteresis logic - if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.): + if (self.CP.minSteerSpeed - 1e-3) > CarControllerParams.DEFAULT_MIN_STEER_SPEED and ret.vEgo < (self.CP.minSteerSpeed + 1.): self.low_speed_alert = True elif ret.vEgo > (self.CP.minSteerSpeed + 2.): self.low_speed_alert = False @@ -246,13 +124,10 @@ class CarInterface(CarInterfaceBase): if c.enabled and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.speedTooLow) - if self.eps_timer_soft_disable_alert: + if self.CC.eps_timer_soft_disable_alert: events.add(EventName.steerTimeLimit) ret.events = events.to_msg() return ret - def apply(self, c, now_nanos): - new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos) - return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 787c7de530..763908b6b2 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -28,7 +28,7 @@ def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque): return packer.make_can_msg("LH_EPS_03", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control): values = {} if len(ldw_stock_values): values = {s: ldw_stock_values[s] for s in [ @@ -40,8 +40,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres ]} values.update({ - "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Texte": hud_alert, @@ -125,11 +125,11 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, - "ACC_Gesetzte_Zeitluecke": 3, + "ACC_Gesetzte_Zeitluecke": distance + 2, "ACC_Display_Prio": 3, "ACC_Abstandsindex": lead_distance, } diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index f42c3cf781..f8d161b970 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -9,7 +9,7 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): return packer.make_can_msg("HCA_1", bus, values) -def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): +def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control): values = {} if len(ldw_stock_values): values = {s: ldw_stock_values[s] for s in [ @@ -21,8 +21,8 @@ def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pres ]} values.update({ - "LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0, - "LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lampe_gelb": 1 if lat_active and steering_pressed else 0, + "LDW_Lampe_gruen": 1 if lat_active and not steering_pressed else 0, "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, "LDW_Textbits": hud_alert, @@ -91,10 +91,10 @@ def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_cont return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance): values = { "ACA_StaACC": acc_hud_status, - "ACA_Zeitluecke": 2, + "ACA_Zeitluecke": distance + 2, "ACA_V_Wunsch": set_speed, "ACA_gemZeitl": lead_distance, "ACA_PrioDisp": 3, diff --git a/selfdrive/car/volkswagen/tests/test_volkswagen.py b/selfdrive/car/volkswagen/tests/test_volkswagen.py new file mode 100644 index 0000000000..0002578105 --- /dev/null +++ b/selfdrive/car/volkswagen/tests/test_volkswagen.py @@ -0,0 +1,60 @@ +import random +import re + +from cereal import car +from openpilot.selfdrive.car.volkswagen.values import CAR, FW_QUERY_CONFIG, WMI +from openpilot.selfdrive.car.volkswagen.fingerprints import FW_VERSIONS + +Ecu = car.CarParams.Ecu + +CHASSIS_CODE_PATTERN = re.compile('[A-Z0-9]{2}') +# TODO: determine the unknown groups +SPARE_PART_FW_PATTERN = re.compile(b'\xf1\x87(?P[0-9][0-9A-Z]{2})(?P[0-9][0-9A-Z][0-9])(?P[0-9A-Z]{2}[0-9])([A-Z0-9]| )') + + +class TestVolkswagenPlatformConfigs: + def test_spare_part_fw_pattern(self, subtests): + # Relied on for determining if a FW is likely VW + for platform, ecus in FW_VERSIONS.items(): + with subtests.test(platform=platform.value): + for fws in ecus.values(): + for fw in fws: + assert SPARE_PART_FW_PATTERN.match(fw) is not None, f"Bad FW: {fw}" + + def test_chassis_codes(self, subtests): + for platform in CAR: + with subtests.test(platform=platform.value): + assert len(platform.config.wmis) > 0, "WMIs not set" + assert len(platform.config.chassis_codes) > 0, "Chassis codes not set" + assert all(CHASSIS_CODE_PATTERN.match(cc) for cc in \ + platform.config.chassis_codes), "Bad chassis codes" + + # No two platforms should share chassis codes + for comp in CAR: + if platform == comp: + continue + assert set() == platform.config.chassis_codes & comp.config.chassis_codes, \ + f"Shared chassis codes: {comp}" + + def test_custom_fuzzy_fingerprinting(self, subtests): + all_radar_fw = list({fw for ecus in FW_VERSIONS.values() for fw in ecus[Ecu.fwdRadar, 0x757, None]}) + + for platform in CAR: + with subtests.test(platform=platform.name): + for wmi in WMI: + for chassis_code in platform.config.chassis_codes | {"00"}: + vin = ["0"] * 17 + vin[0:3] = wmi + vin[6:8] = chassis_code + vin = "".join(vin) + + # Check a few FW cases - expected, unexpected + for radar_fw in random.sample(all_radar_fw, 5) + [b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x877H9907572AA\xf1\x890396']: + should_match = ((wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes) and + radar_fw in all_radar_fw) + + live_fws = {(0x757, None): [radar_fw]} + matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fws, vin, FW_VERSIONS) + + expected_matches = {platform} if should_match else set() + assert expected_matches == matches, "Bad match" diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 35cb3607ec..8b58769b3f 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,15 +1,15 @@ from collections import defaultdict, namedtuple from dataclasses import dataclass, field from enum import Enum, IntFlag, StrEnum -from typing import Dict, List, Union from cereal import car from panda.python import uds from opendbc.can.can_define import CANDefine -from openpilot.selfdrive.car import dbc_dict -from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column, \ - Device -from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car import dbc_dict, CarSpecs, DbcDict, PlatformConfig, Platforms +from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ + Device +from openpilot.selfdrive.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16 Ecu = car.CarParams.Ecu NetworkLocation = car.CarParams.NetworkLocation @@ -34,13 +34,15 @@ class CarControllerParams: STEER_TIME_ALERT = STEER_TIME_MAX - 10 # If mitigation fails, time to soft disengage before EPS timer expires STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period + DEFAULT_MIN_STEER_SPEED = 0.4 # m/s, newer EPS racks fault below this speed, don't show a low speed alert + ACCEL_MAX = 2.0 # 2.0 m/s max acceleration ACCEL_MIN = -3.5 # 3.5 m/s max deceleration def __init__(self, CP): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - if CP.carFingerprint in PQ_CARS: + if CP.flags & VolkswagenFlags.PQ: self.LDW_STEP = 5 # LDW_1 message frequency 20Hz self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm @@ -109,51 +111,57 @@ class CANBUS: cam = 2 +class WMI(StrEnum): + VOLKSWAGEN_USA_SUV = "1V2" + VOLKSWAGEN_USA_CAR = "1VW" + VOLKSWAGEN_MEXICO_SUV = "3VV" + VOLKSWAGEN_MEXICO_CAR = "3VW" + VOLKSWAGEN_ARGENTINA = "8AW" + VOLKSWAGEN_BRASIL = "9BW" + SAIC_VOLKSWAGEN = "LSV" + SKODA = "TMB" + SEAT = "VSS" + AUDI_EUROPE_MPV = "WA1" + AUDI_GERMANY_CAR = "WAU" + MAN = "WMA" + AUDI_SPORT = "WUA" + VOLKSWAGEN_COMMERCIAL = "WV1" + VOLKSWAGEN_COMMERCIAL_BUS_VAN = "WV2" + VOLKSWAGEN_EUROPE_SUV = "WVG" + VOLKSWAGEN_EUROPE_CAR = "WVW" + VOLKSWAGEN_GROUP_RUS = "XW8" + + class VolkswagenFlags(IntFlag): + # Detected flags STOCK_HCA_PRESENT = 1 + # Static flags + PQ = 2 -# Check the 7th and 8th characters of the VIN before adding a new CAR. If the -# chassis code is already listed below, don't add a new CAR, just add to the -# FW_VERSIONS for that existing CAR. -# Exception: SEAT Leon and SEAT Ateca share a chassis code - -class CAR(StrEnum): - ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN" # Chassis AN, Mk1 VW Arteon and variants - ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN" # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport - CRAFTER_MK2 = "VOLKSWAGEN CRAFTER 2ND GEN" # Chassis SY/SZ, Mk2 VW Crafter, VW Grand California, MAN TGE - GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants - JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta - PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants - PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift - POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo - SHARAN_MK2 = "VOLKSWAGEN SHARAN 2ND GEN" # Chassis 7N, Mk2 Volkswagen Sharan and SEAT Alhambra - TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu - TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants - TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN" # Chassis AD/BW, Mk2 VW Tiguan and variants - TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN" # Chassis 1T, Mk2 VW Touran and variants - TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1" # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California - TROC_MK1 = "VOLKSWAGEN T-ROC 1ST GEN" # Chassis A1, Mk1 VW T-Roc and variants - AUDI_A3_MK3 = "AUDI A3 3RD GEN" # Chassis 8V/FF, Mk3 Audi A3 and variants - AUDI_Q2_MK1 = "AUDI Q2 1ST GEN" # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only) - AUDI_Q3_MK2 = "AUDI Q3 2ND GEN" # Chassis 8U/F3/FS, Mk2 Audi Q3 and variants - SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN" # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca - SEAT_LEON_MK3 = "SEAT LEON 3RD GEN" # Chassis 5F, Mk3 SEAT Leon and variants - SKODA_FABIA_MK4 = "SKODA FABIA 4TH GEN" # Chassis PJ, Mk4 Skoda Fabia - SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN" # Chassis NW, Mk1 Skoda Kamiq - SKODA_KAROQ_MK1 = "SKODA KAROQ 1ST GEN" # Chassis NU, Mk1 Skoda Karoq - SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN" # Chassis NS, Mk1 Skoda Kodiaq - SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN" # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq - SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants - SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants - - -PQ_CARS = {CAR.PASSAT_NMS, CAR.SHARAN_MK2} - - -DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None)) -for car_type in PQ_CARS: - DBC[car_type] = dbc_dict("vw_golf_mk4", None) + +@dataclass +class VolkswagenMQBPlatformConfig(PlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('vw_mqb_2010', None)) + # Volkswagen uses the VIN WMI and chassis code to match in the absence of the comma power + # on camera-integrated cars, as we lose too many ECUs to reliably identify the vehicle + chassis_codes: set[str] = field(default_factory=set) + wmis: set[WMI] = field(default_factory=set) + + +@dataclass +class VolkswagenPQPlatformConfig(VolkswagenMQBPlatformConfig): + dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('vw_golf_mk4', None)) + + def init(self): + self.flags |= VolkswagenFlags.PQ + + +@dataclass(frozen=True, kw_only=True) +class VolkswagenCarSpecs(CarSpecs): + centerToFrontRatio: float = 0.45 + steerRatio: float = 15.6 + minSteerSpeed: float = CarControllerParams.DEFAULT_MIN_STEER_SPEED class Footnote(Enum): @@ -178,7 +186,7 @@ class Footnote(Enum): @dataclass -class VWCarInfo(CarInfo): +class VWCarDocs(CarDocs): package: str = "Adaptive Cruise Control (ACC) & Lane Assist" car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.j533])) @@ -187,93 +195,283 @@ class VWCarInfo(CarInfo): if "SKODA" in CP.carFingerprint: self.footnotes.append(Footnote.SKODA_HEATED_WINDSHIELD) - if CP.carFingerprint in (CAR.CRAFTER_MK2, CAR.TRANSPORTER_T61): + if CP.carFingerprint in (CAR.VOLKSWAGEN_CRAFTER_MK2, CAR.VOLKSWAGEN_TRANSPORTER_T61): self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.j533]) + if abs(CP.minSteerSpeed - CarControllerParams.DEFAULT_MIN_STEER_SPEED) < 1e-3: + self.min_steer_speed = 0 -CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { - CAR.ARTEON_MK1: [ - VWCarInfo("Volkswagen Arteon 2018-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon R 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen Arteon eHybrid 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), - VWCarInfo("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), - ], - CAR.ATLAS_MK1: [ - VWCarInfo("Volkswagen Atlas 2018-23"), - VWCarInfo("Volkswagen Atlas Cross Sport 2020-22"), - VWCarInfo("Volkswagen Teramont 2018-22"), - VWCarInfo("Volkswagen Teramont Cross Sport 2021-22"), - VWCarInfo("Volkswagen Teramont X 2021-22"), - ], - CAR.CRAFTER_MK2: [ - VWCarInfo("Volkswagen Crafter 2017-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("Volkswagen e-Crafter 2018-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("Volkswagen Grand California 2019-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("MAN TGE 2017-23", video_link="https://youtu.be/4100gLeabmo"), - VWCarInfo("MAN eTGE 2020-23", video_link="https://youtu.be/4100gLeabmo"), - ], - CAR.GOLF_MK7: [ - VWCarInfo("Volkswagen e-Golf 2014-20"), - VWCarInfo("Volkswagen Golf 2015-20", auto_resume=False), - VWCarInfo("Volkswagen Golf Alltrack 2015-19", auto_resume=False), - VWCarInfo("Volkswagen Golf GTD 2015-20"), - VWCarInfo("Volkswagen Golf GTE 2015-20"), - VWCarInfo("Volkswagen Golf GTI 2015-21", auto_resume=False), - VWCarInfo("Volkswagen Golf R 2015-19"), - VWCarInfo("Volkswagen Golf SportsVan 2015-20"), - ], - CAR.JETTA_MK7: [ - VWCarInfo("Volkswagen Jetta 2018-24"), - VWCarInfo("Volkswagen Jetta GLI 2021-24"), - ], - CAR.PASSAT_MK8: [ - VWCarInfo("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]), - VWCarInfo("Volkswagen Passat Alltrack 2015-22"), - VWCarInfo("Volkswagen Passat GTE 2015-22"), - ], - CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22"), - CAR.POLO_MK6: [ - VWCarInfo("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]), - VWCarInfo("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]), - ], - CAR.SHARAN_MK2: [ - VWCarInfo("Volkswagen Sharan 2018-22"), - VWCarInfo("SEAT Alhambra 2018-20"), - ], - CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022-23"), - CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0]), - CAR.TIGUAN_MK2: [ - VWCarInfo("Volkswagen Tiguan 2018-24"), - VWCarInfo("Volkswagen Tiguan eHybrid 2021-23"), - ], - CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2016-23"), - CAR.TRANSPORTER_T61: [ - VWCarInfo("Volkswagen Caravelle 2020"), - VWCarInfo("Volkswagen California 2021-23"), - ], - CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2018-22", footnotes=[Footnote.VW_MQB_A0]), - CAR.AUDI_A3_MK3: [ - VWCarInfo("Audi A3 2014-19"), - VWCarInfo("Audi A3 Sportback e-tron 2017-18"), - VWCarInfo("Audi RS3 2018"), - VWCarInfo("Audi S3 2015-17"), - ], - CAR.AUDI_Q2_MK1: VWCarInfo("Audi Q2 2018"), - CAR.AUDI_Q3_MK2: VWCarInfo("Audi Q3 2019-23"), - CAR.SEAT_ATECA_MK1: VWCarInfo("SEAT Ateca 2018"), - CAR.SEAT_LEON_MK3: VWCarInfo("SEAT Leon 2014-20"), - CAR.SKODA_FABIA_MK4: VWCarInfo("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0]), - CAR.SKODA_KAMIQ_MK1: VWCarInfo("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]), - CAR.SKODA_KAROQ_MK1: VWCarInfo("Škoda Karoq 2019-23"), - CAR.SKODA_KODIAQ_MK1: VWCarInfo("Škoda Kodiaq 2017-23"), - CAR.SKODA_SCALA_MK1: VWCarInfo("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]), - CAR.SKODA_SUPERB_MK3: VWCarInfo("Škoda Superb 2015-22"), - CAR.SKODA_OCTAVIA_MK3: [ - VWCarInfo("Škoda Octavia 2015-19"), - VWCarInfo("Škoda Octavia RS 2016"), - ], -} +# Check the 7th and 8th characters of the VIN before adding a new CAR. If the +# chassis code is already listed below, don't add a new CAR, just add to the +# FW_VERSIONS for that existing CAR. + +class CAR(Platforms): + config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig + + VOLKSWAGEN_ARTEON_MK1 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Arteon 2018-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen Arteon R 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen Arteon eHybrid 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen Arteon Shooting Brake 2020-23", video_link="https://youtu.be/FAomFKPFlDA"), + VWCarDocs("Volkswagen CC 2018-22", video_link="https://youtu.be/FAomFKPFlDA"), + ], + VolkswagenCarSpecs(mass=1733, wheelbase=2.84), + chassis_codes={"AN", "3H"}, + wmis={WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_ATLAS_MK1 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Atlas 2018-23"), + VWCarDocs("Volkswagen Atlas Cross Sport 2020-22"), + VWCarDocs("Volkswagen Teramont 2018-22"), + VWCarDocs("Volkswagen Teramont Cross Sport 2021-22"), + VWCarDocs("Volkswagen Teramont X 2021-22"), + ], + VolkswagenCarSpecs(mass=2011, wheelbase=2.98), + chassis_codes={"CA"}, + wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_SUV}, + ) + VOLKSWAGEN_CADDY_MK3 = VolkswagenPQPlatformConfig( + [ + VWCarDocs("Volkswagen Caddy 2019"), + VWCarDocs("Volkswagen Caddy Maxi 2019"), + ], + VolkswagenCarSpecs(mass=1613, wheelbase=2.6, minSteerSpeed=21 * CV.KPH_TO_MS), + chassis_codes={"2K"}, + wmis={WMI.VOLKSWAGEN_COMMERCIAL_BUS_VAN}, + ) + VOLKSWAGEN_CRAFTER_MK2 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Crafter 2017-24", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("Volkswagen e-Crafter 2018-24", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("Volkswagen Grand California 2019-24", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("MAN TGE 2017-24", video_link="https://youtu.be/4100gLeabmo"), + VWCarDocs("MAN eTGE 2020-24", video_link="https://youtu.be/4100gLeabmo"), + ], + VolkswagenCarSpecs(mass=2100, wheelbase=3.64, minSteerSpeed=50 * CV.KPH_TO_MS), + chassis_codes={"SY", "SZ", "UY", "UZ"}, + wmis={WMI.VOLKSWAGEN_COMMERCIAL, WMI.MAN}, + ) + VOLKSWAGEN_GOLF_MK7 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen e-Golf 2014-20"), + VWCarDocs("Volkswagen Golf 2015-20", auto_resume=False), + VWCarDocs("Volkswagen Golf Alltrack 2015-19", auto_resume=False), + VWCarDocs("Volkswagen Golf GTD 2015-20"), + VWCarDocs("Volkswagen Golf GTE 2015-20"), + VWCarDocs("Volkswagen Golf GTI 2015-21", auto_resume=False), + VWCarDocs("Volkswagen Golf R 2015-19"), + VWCarDocs("Volkswagen Golf SportsVan 2015-20"), + ], + VolkswagenCarSpecs(mass=1397, wheelbase=2.62), + chassis_codes={"5G", "AU", "BA", "BE"}, + wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_JETTA_MK7 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Jetta 2018-24"), + VWCarDocs("Volkswagen Jetta GLI 2021-24"), + ], + VolkswagenCarSpecs(mass=1328, wheelbase=2.71), + chassis_codes={"BU"}, + wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_PASSAT_MK8 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]), + VWCarDocs("Volkswagen Passat Alltrack 2015-22"), + VWCarDocs("Volkswagen Passat GTE 2015-22"), + ], + VolkswagenCarSpecs(mass=1551, wheelbase=2.79), + chassis_codes={"3C", "3G"}, + wmis={WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_PASSAT_NMS = VolkswagenPQPlatformConfig( + [VWCarDocs("Volkswagen Passat NMS 2017-22")], + VolkswagenCarSpecs(mass=1503, wheelbase=2.80, minSteerSpeed=50 * CV.KPH_TO_MS, minEnableSpeed=20 * CV.KPH_TO_MS), + chassis_codes={"A3"}, + wmis={WMI.VOLKSWAGEN_USA_CAR}, + ) + VOLKSWAGEN_POLO_MK6 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]), + VWCarDocs("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]), + ], + VolkswagenCarSpecs(mass=1230, wheelbase=2.55), + chassis_codes={"AW"}, + wmis={WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_SHARAN_MK2 = VolkswagenPQPlatformConfig( + [ + VWCarDocs("Volkswagen Sharan 2018-22"), + VWCarDocs("SEAT Alhambra 2018-20"), + ], + VolkswagenCarSpecs(mass=1639, wheelbase=2.92, minSteerSpeed=50 * CV.KPH_TO_MS), + chassis_codes={"7N"}, + wmis={WMI.VOLKSWAGEN_EUROPE_CAR}, + ) + VOLKSWAGEN_TAOS_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Volkswagen Taos 2022-23")], + VolkswagenCarSpecs(mass=1498, wheelbase=2.69), + chassis_codes={"B2"}, + wmis={WMI.VOLKSWAGEN_MEXICO_SUV, WMI.VOLKSWAGEN_ARGENTINA}, + ) + VOLKSWAGEN_TCROSS_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1150, wheelbase=2.60), + chassis_codes={"C1"}, + wmis={WMI.VOLKSWAGEN_EUROPE_SUV}, + ) + VOLKSWAGEN_TIGUAN_MK2 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Tiguan 2018-24"), + VWCarDocs("Volkswagen Tiguan eHybrid 2021-23"), + ], + VolkswagenCarSpecs(mass=1715, wheelbase=2.74), + chassis_codes={"5N", "AD", "AX", "BW"}, + wmis={WMI.VOLKSWAGEN_EUROPE_SUV, WMI.VOLKSWAGEN_MEXICO_SUV}, + ) + VOLKSWAGEN_TOURAN_MK2 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Volkswagen Touran 2016-23")], + VolkswagenCarSpecs(mass=1516, wheelbase=2.79), + chassis_codes={"1T"}, + wmis={WMI.VOLKSWAGEN_EUROPE_SUV}, + ) + VOLKSWAGEN_TRANSPORTER_T61 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Volkswagen Caravelle 2020"), + VWCarDocs("Volkswagen California 2021-23"), + ], + VolkswagenCarSpecs(mass=1926, wheelbase=3.00, minSteerSpeed=14.0), + chassis_codes={"7H", "7L"}, + wmis={WMI.VOLKSWAGEN_COMMERCIAL_BUS_VAN}, + ) + VOLKSWAGEN_TROC_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Volkswagen T-Roc 2018-23")], + VolkswagenCarSpecs(mass=1413, wheelbase=2.63), + chassis_codes={"A1"}, + wmis={WMI.VOLKSWAGEN_EUROPE_SUV}, + ) + AUDI_A3_MK3 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Audi A3 2014-19"), + VWCarDocs("Audi A3 Sportback e-tron 2017-18"), + VWCarDocs("Audi RS3 2018"), + VWCarDocs("Audi S3 2015-17"), + ], + VolkswagenCarSpecs(mass=1335, wheelbase=2.61), + chassis_codes={"8V", "FF"}, + wmis={WMI.AUDI_GERMANY_CAR, WMI.AUDI_SPORT}, + ) + AUDI_Q2_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Audi Q2 2018")], + VolkswagenCarSpecs(mass=1205, wheelbase=2.61), + chassis_codes={"GA"}, + wmis={WMI.AUDI_GERMANY_CAR}, + ) + AUDI_Q3_MK2 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Audi Q3 2019-23")], + VolkswagenCarSpecs(mass=1623, wheelbase=2.68), + chassis_codes={"8U", "F3", "FS"}, + wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR}, + ) + SEAT_ATECA_MK1 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("CUPRA Ateca 2018-23"), + VWCarDocs("SEAT Ateca 2016-23"), + VWCarDocs("SEAT Leon 2014-20"), + ], + VolkswagenCarSpecs(mass=1300, wheelbase=2.64), + chassis_codes={"5F"}, + wmis={WMI.SEAT}, + ) + SKODA_FABIA_MK4 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0])], + VolkswagenCarSpecs(mass=1266, wheelbase=2.56), + chassis_codes={"PJ"}, + wmis={WMI.SKODA}, + ) + SKODA_KAMIQ_MK1 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]), + VWCarDocs("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]), + ], + VolkswagenCarSpecs(mass=1230, wheelbase=2.66), + chassis_codes={"NW"}, + wmis={WMI.SKODA}, + ) + SKODA_KAROQ_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Škoda Karoq 2019-23")], + VolkswagenCarSpecs(mass=1278, wheelbase=2.66), + chassis_codes={"NU"}, + wmis={WMI.SKODA}, + ) + SKODA_KODIAQ_MK1 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Škoda Kodiaq 2017-23")], + VolkswagenCarSpecs(mass=1569, wheelbase=2.79), + chassis_codes={"NS"}, + wmis={WMI.SKODA, WMI.VOLKSWAGEN_GROUP_RUS}, + ) + SKODA_OCTAVIA_MK3 = VolkswagenMQBPlatformConfig( + [ + VWCarDocs("Škoda Octavia 2015-19"), + VWCarDocs("Škoda Octavia RS 2016"), + VWCarDocs("Škoda Octavia Scout 2017-19"), + ], + VolkswagenCarSpecs(mass=1388, wheelbase=2.68), + chassis_codes={"NE"}, + wmis={WMI.SKODA}, + ) + SKODA_SUPERB_MK3 = VolkswagenMQBPlatformConfig( + [VWCarDocs("Škoda Superb 2015-22")], + VolkswagenCarSpecs(mass=1505, wheelbase=2.84), + chassis_codes={"3V", "NP"}, + wmis={WMI.SKODA}, + ) + + +def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]: + candidates = set() + + # Compile all FW versions for each ECU + all_ecu_versions: dict[EcuAddrSubAddr, set[str]] = defaultdict(set) + for ecus in offline_fw_versions.values(): + for ecu, versions in ecus.items(): + all_ecu_versions[ecu] |= set(versions) + + # Check the WMI and chassis code to determine the platform + wmi = vin[:3] + chassis_code = vin[6:8] + + for platform in CAR: + valid_ecus = set() + for ecu in offline_fw_versions[platform]: + addr = ecu[1:] + if ecu[0] not in CHECK_FUZZY_ECUS: + continue + + # Sanity check that live FW is in the superset of all FW, Volkswagen ECU part numbers are commonly shared + found_versions = live_fw_versions.get(addr, []) + expected_versions = all_ecu_versions[ecu] + if not any(found_version in expected_versions for found_version in found_versions): + break + + valid_ecus.add(ecu[0]) + + if valid_ecus != CHECK_FUZZY_ECUS: + continue + + if wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes: + candidates.add(platform) + + return {str(c) for c in candidates} + + +# These ECUs are required to match to gain a VIN match +# TODO: do we want to check camera when we add its FW? +CHECK_FUZZY_ECUS = {Ecu.fwdRadar} # All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars # with a manual trans won't return transmission firmware, but all other cars will. @@ -293,17 +491,26 @@ VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + VOLKSWAGEN_RX_OFFSET = 0x6a FW_QUERY_CONFIG = FwQueryConfig( - requests=[ + requests=[request for bus, obd_multiplexing in [(1, True), (1, False), (0, False)] for request in [ Request( [VOLKSWAGEN_VERSION_REQUEST_MULTI], [VOLKSWAGEN_VERSION_RESPONSE], - whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar], + whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar, Ecu.fwdCamera], rx_offset=VOLKSWAGEN_RX_OFFSET, + bus=bus, + obd_multiplexing=obd_multiplexing, ), Request( [VOLKSWAGEN_VERSION_REQUEST_MULTI], [VOLKSWAGEN_VERSION_RESPONSE], whitelist_ecus=[Ecu.engine, Ecu.transmission], + bus=bus, + obd_multiplexing=obd_multiplexing, ), - ], + ]], + non_essential_ecus={Ecu.eps: list(CAR)}, + extra_ecus=[(Ecu.fwdCamera, 0x74f, None)], + match_fw_to_car_fuzzy=match_fw_to_car_fuzzy, ) + +DBC = CAR.create_dbc_map() diff --git a/selfdrive/controls/.gitignore b/selfdrive/controls/.gitignore new file mode 100644 index 0000000000..22a371d8ff --- /dev/null +++ b/selfdrive/controls/.gitignore @@ -0,0 +1,2 @@ +calibration_param +traces diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bf2b14c1da..4f5bd19405 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -8,19 +8,17 @@ from typing import SupportsFloat import cereal.messaging as messaging from cereal import car, log -from cereal.visionipc import VisionIpcClient, VisionStreamType +from msgq.visionipc import VisionIpcClient, VisionStreamType -from panda import ALTERNATIVE_EXPERIENCE from openpilot.common.conversions import Conversions as CV +from openpilot.common.git import get_short_branch from openpilot.common.numpy_fast import clip from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp -from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can -from openpilot.selfdrive.car.interfaces import CarInterfaceBase +from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature from openpilot.selfdrive.controls.lib.events import Events, ET @@ -32,7 +30,6 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE -from openpilot.system.version import get_short_branch SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -61,101 +58,22 @@ ACTIVE_STATES = (State.enabled, State.softDisabling, State.overriding) ENABLED_STATES = (State.preEnabled, *ACTIVE_STATES) -class CarD: - CI: CarInterfaceBase - CS: car.CarState - +class Controls: def __init__(self, CI=None): - self.can_sock = messaging.sub_sock('can', timeout=20) - self.sm = messaging.SubMaster(['pandaStates']) - self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams']) - - self.can_rcv_timeout_counter = 0 # conseuctive timeout count - self.can_rcv_cum_timeout_counter = 0 # cumulative timeout count - self.params = Params() if CI is None: - # wait for one pandaState and one CAN packet - print("Waiting for CAN messages...") - get_one_can(self.can_sock) - - num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + cloudlog.info("controlsd is waiting for CarParams") + with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg: + # TODO: this shouldn't need to be a builder + self.CP = msg.as_builder() + cloudlog.info("controlsd got CarParams") + + # Uses car interface helper functions, altering state won't be considered by card for actuation + self.CI = get_car_interface(self.CP) else: self.CI, self.CP = CI, CI.CP - def initialize(self): - """Initialize CarInterface, once controls are ready""" - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) - - def state_update(self, CC: car.CarControl): - """carState update loop, driven by can""" - - # TODO: This should not depend on carControl - - # Update carState from CAN - can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - self.CS = self.CI.update(CC, can_strs) - - self.sm.update(0) - - can_rcv_valid = len(can_strs) > 0 - - # Check for CAN timeout - if not can_rcv_valid: - self.can_rcv_timeout_counter += 1 - self.can_rcv_cum_timeout_counter += 1 - else: - self.can_rcv_timeout_counter = 0 - - self.can_rcv_timeout = self.can_rcv_timeout_counter >= 5 - - if can_rcv_valid and REPLAY: - self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime - - return self.CS - - def state_publish(self, car_events): - """carState and carParams publish loop""" - - # TODO: carState should be independent of the event loop - - # carState - cs_send = messaging.new_message('carState') - cs_send.valid = self.CS.canValid - cs_send.carState = self.CS - cs_send.carState.events = car_events - self.pm.send('carState', cs_send) - - # carParams - logged every 50 seconds (> 1 per segment) - if (self.sm.frame % int(50. / DT_CTRL) == 0): - cp_send = messaging.new_message('carParams') - cp_send.valid = True - cp_send.carParams = self.CP - self.pm.send('carParams', cp_send) - - def controls_update(self, CC: car.CarControl): - """control update loop, driven by carControl""" - - # send car controls over can - now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) - actuators_output, can_sends = self.CI.apply(CC, now_nanos) - self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid)) - - return actuators_output - - -class Controls: - def __init__(self, CI=None): - self.card = CarD(CI) - - self.CP = self.card.CP - self.CI = self.card.CI - - config_realtime_process(4, Priority.CTRL_HIGH) - # Ensure the current branch is cached, otherwise the first iteration of controlsd lags self.branch = get_short_branch() @@ -167,12 +85,17 @@ class Controls: self.log_sock = messaging.sub_sock('androidLog') - self.params = Params() + # TODO: de-couple controlsd with card/conflate on carState without introducing controls mismatches + self.car_state_sock = messaging.sub_sock('carState', timeout=20) + ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: ignore += ['driverCameraState', 'managerState'] + if REPLAY: + # no vipc in replay will make them ignored anyways + ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets, ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ], @@ -180,47 +103,21 @@ class Controls: self.joystick_mode = self.params.get_bool("JoystickDebugMode") - # set alternative experiences from parameters - self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") - self.CP.alternativeExperience = 0 - if not self.disengage_on_accelerator: - self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS - # read params self.is_metric = self.params.get_bool("IsMetric") self.is_ldw_enabled = self.params.get_bool("IsLdwEnabled") - openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") # detect sound card presence and ensure successful init sounds_available = HARDWARE.get_sound_card_online() car_recognized = self.CP.carName != 'mock' - controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly - self.CP.passive = not car_recognized or not controller_available or self.CP.dashcamOnly - if self.CP.passive: - safety_config = car.CarParams.SafetyConfig.new_message() - safety_config.safetyModel = car.CarParams.SafetyModel.noOutput - self.CP.safetyConfigs = [safety_config] - - # Write previous route's CarParams - prev_cp = self.params.get("CarParamsPersistent") - if prev_cp is not None: - self.params.put("CarParamsPrevRoute", prev_cp) - - # Write CarParams for radard - cp_bytes = self.CP.to_bytes() - self.params.put("CarParams", cp_bytes) - self.params.put_nonblocking("CarParamsCache", cp_bytes) - self.params.put_nonblocking("CarParamsPersistent", cp_bytes) - # cleanup old params if not self.CP.experimentalLongitudinalAvailable: self.params.remove("ExperimentalLongitudinalEnabled") if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") - self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() self.AM = AlertManager() self.events = Events() @@ -251,16 +148,16 @@ class Controls: self.current_alert_types = [ET.PERMANENT] self.logged_comm_issue = None self.not_running_prev = None - self.last_actuators = car.CarControl.Actuators.new_message() self.steer_limited = False self.desired_curvature = 0.0 self.experimental_mode = False + self.personality = self.read_personality_param() self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False self.can_log_mono_time = 0 - self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) + self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) @@ -273,12 +170,12 @@ class Controls: elif self.CP.passive: self.events.add(EventName.dashcamMode, static=True) - # controlsd is driven by can recv, expected at 100Hz + # controlsd is driven by carState, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) def set_initial_state(self): if REPLAY: - controls_state = Params().get("ReplayControlsState") + controls_state = self.params.get("ReplayControlsState") if controls_state is not None: with log.ControlsState.from_bytes(controls_state) as controls_state: self.v_cruise_helper.v_cruise_kph = controls_state.vCruise @@ -315,18 +212,6 @@ class Controls: if not self.CP.pcmCruise and not self.v_cruise_helper.v_cruise_initialized and resume_pressed: self.events.add(EventName.resumeBlocked) - # Disable on rising edge of accelerator or brake. Also disable on brake when speed > 0 - if (CS.gasPressed and not self.CS_prev.gasPressed and self.disengage_on_accelerator) or \ - (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) or \ - (CS.regenBraking and (not self.CS_prev.regenBraking or not CS.standstill)): - self.events.add(EventName.pedalPressed) - - if CS.brakePressed and CS.standstill: - self.events.add(EventName.preEnableStandstill) - - if CS.gasPressed: - self.events.add(EventName.gasPressedOverride) - if not self.CP.notCar: self.events.add_from_msg(self.sm['driverMonitoringState'].events) @@ -394,7 +279,7 @@ class Controls: else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES - # safety mismatch allows some time for boardd to set the safety mode and publish it back from panda + # safety mismatch allows some time for pandad to set the safety mode and publish it back from panda if (safety_mismatch and self.sm.frame*DT_CTRL > 10.) or pandaState.safetyRxChecksInvalid or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) @@ -420,7 +305,7 @@ class Controls: self.events.add(EventName.cameraFrameRate) if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) - if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): + if len(self.sm['radarState'].radarErrors) or ((not self.rk.lagging or REPLAY) and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) if not self.sm.valid['pandaStates']: self.events.add(EventName.usbError) @@ -432,19 +317,18 @@ class Controls: # generic catch-all. ideally, a more specific event should be added above instead has_disable_events = self.events.contains(ET.NO_ENTRY) and (self.events.contains(ET.SOFT_DISABLE) or self.events.contains(ET.IMMEDIATE_DISABLE)) no_system_errors = (not has_disable_events) or (len(self.events) == num_events) - if (not self.sm.all_checks() or self.card.can_rcv_timeout) and no_system_errors: + if not self.sm.all_checks() and no_system_errors: if not self.sm.all_alive(): self.events.add(EventName.commIssue) elif not self.sm.all_freq_ok(): self.events.add(EventName.commIssueAvgFreq) - else: # invalid or can_rcv_timeout. + else: self.events.add(EventName.commIssue) logs = { 'invalid': [s for s, valid in self.sm.valid.items() if not valid], 'not_alive': [s for s, alive in self.sm.alive.items() if not alive], 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], - 'can_rcv_timeout': self.card.can_rcv_timeout, } if logs != self.logged_comm_issue: cloudlog.event("commIssue", error=True, **logs) @@ -477,7 +361,7 @@ class Controls: stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.25 model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled - if planner_fcw or model_fcw: + if (planner_fcw or model_fcw) and not (self.CP.notCar and self.joystick_mode): self.events.add(EventName.fcw) for m in messaging.drain_sock(self.log_sock, wait_for_one=False): @@ -494,7 +378,7 @@ class Controls: # TODO: fix simulator if not SIMULATION or REPLAY: # Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes - if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1000): + if not self.sm['liveLocationKalman'].gpsOK and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500): self.events.add(EventName.noGps) if self.sm['liveLocationKalman'].gpsOK: self.distance_traveled = 0 @@ -504,9 +388,10 @@ class Controls: self.events.add(EventName.modeldLagging) def data_sample(self): - """Receive data from sockets and update carState""" + """Receive data from sockets""" - CS = self.card.state_update(self.CC) + car_state = messaging.recv_one(self.car_state_sock) + CS = car_state.carState if car_state else self.CS_prev self.sm.update(0) @@ -520,12 +405,8 @@ class Controls: if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: self.sm.ignore_alive.append('wideRoadCameraState') - if not self.CP.passive: - self.card.initialize() - self.initialized = True self.set_initial_state() - self.params.put_bool_nonblocking("ControlsReady", True) cloudlog.event( "controlsd.initialized", @@ -729,7 +610,7 @@ class Controls: undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 turning = abs(lac_log.desiredLateralAccel) > 1.0 good_speed = CS.vEgo > 5 - max_torque = abs(self.last_actuators.steer) > 0.99 + max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 if undershooting and turning and good_speed and max_torque: lac_log.active and self.events.add(EventName.steerSaturated) elif lac_log.saturated: @@ -759,6 +640,12 @@ class Controls: cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}") setattr(actuators, p, 0.0) + # decrement personality on distance button press + if self.CP.openpilotLongitudinalControl: + if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents): + self.personality = (self.personality - 1) % 3 + self.params.put_nonblocking('LongitudinalPersonality', str(self.personality)) + return CC, lac_log def publish_logs(self, CS, start_time, CC, lac_log): @@ -787,6 +674,7 @@ class Controls: hudControl.speedVisible = self.enabled hudControl.lanesVisible = self.enabled hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead + hudControl.leadDistanceBars = self.personality + 1 hudControl.rightLaneVisible = True hudControl.leftLaneVisible = True @@ -826,13 +714,12 @@ class Controls: hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.last_actuators = self.card.controls_update(CC) - CC.actuatorsOutput = self.last_actuators + CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ + self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD else: - self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + self.steer_limited = abs(CC.actuators.steer - CO.actuatorsOutput.steer) > 1e-2 force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) @@ -874,8 +761,8 @@ class Controls: controlsState.cumLagMs = -self.rk.remaining * 1000. controlsState.startMonoTime = int(start_time * 1e9) controlsState.forceDecel = bool(force_decel) - controlsState.canErrorCounter = self.card.can_rcv_cum_timeout_counter controlsState.experimentalMode = self.experimental_mode + controlsState.personality = self.personality lat_tuning = self.CP.lateralTuning.which() if self.joystick_mode: @@ -889,15 +776,11 @@ class Controls: self.pm.send('controlsState', dat) - car_events = self.events.to_msg() - - self.card.state_publish(car_events) - # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('onroadEvents', len(self.events)) ce_send.valid = True - ce_send.onroadEvents = car_events + ce_send.onroadEvents = self.events.to_msg() self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() @@ -907,9 +790,6 @@ class Controls: cc_send.carControl = CC self.pm.send('carControl', cc_send) - # copy CarControl to pass to CarInterface on the next iteration - self.CC = CC - def step(self): start_time = time.monotonic() @@ -932,10 +812,17 @@ class Controls: self.CS_prev = CS + def read_personality_param(self): + try: + return int(self.params.get('LongitudinalPersonality')) + except (ValueError, TypeError): + return log.LongitudinalPersonality.standard + def params_thread(self, evt): while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl + self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) @@ -954,6 +841,7 @@ class Controls: def main(): + config_realtime_process(4, Priority.CTRL_HIGH) controls = Controls() controls.controlsd_thread() diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 6abcf4cbba..f67e269fa9 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -3,7 +3,6 @@ import os import json from collections import defaultdict from dataclasses import dataclass -from typing import List, Dict, Optional from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params @@ -14,7 +13,7 @@ with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) a OFFROAD_ALERTS = json.load(f) -def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = None) -> None: +def set_offroad_alert(alert: str, show_alert: bool, extra_text: str = None) -> None: if show_alert: a = copy.copy(OFFROAD_ALERTS[alert]) a['extra'] = extra_text or '' @@ -25,7 +24,7 @@ def set_offroad_alert(alert: str, show_alert: bool, extra_text: Optional[str] = @dataclass class AlertEntry: - alert: Optional[Alert] = None + alert: Alert | None = None start_frame: int = -1 end_frame: int = -1 @@ -34,9 +33,9 @@ class AlertEntry: class AlertManager: def __init__(self): - self.alerts: Dict[str, AlertEntry] = defaultdict(AlertEntry) + self.alerts: dict[str, AlertEntry] = defaultdict(AlertEntry) - def add_many(self, frame: int, alerts: List[Alert]) -> None: + def add_many(self, frame: int, alerts: list[Alert]) -> None: for alert in alerts: entry = self.alerts[alert.alert_type] entry.alert = alert @@ -45,7 +44,7 @@ class AlertManager: min_end_frame = entry.start_frame + alert.duration entry.end_frame = max(frame + 1, min_end_frame) - def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]: + def process_alerts(self, frame: int, clear_event_types: set) -> Alert | None: current_alert = AlertEntry() for v in self.alerts.values(): if not v.alert: diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 35446ca22b..6fb6c569b0 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -1,7 +1,7 @@ { "Offroad_TemperatureTooHigh": { "text": "Device temperature too high. System cooling down before starting. Current internal component temperature: %1", - "severity": 1 + "severity": 0 }, "Offroad_ConnectivityNeededPrompt": { "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in %1", @@ -17,10 +17,6 @@ "severity": 1, "_comment": "Set extra field to the failed reason." }, - "Offroad_InvalidTime": { - "text": "Invalid date and time settings, system won't start. Connect to internet to set time.", - "severity": 1 - }, "Offroad_IsTakingSnapshot": { "text": "Taking camera snapshots. System won't start until finished.", "severity": 0 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index d68c95bcf5..b01818d704 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1,15 +1,16 @@ #!/usr/bin/env python3 +import bisect import math import os from enum import IntEnum -from typing import Dict, Union, Callable, List, Optional +from collections.abc import Callable from cereal import log, car import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV +from openpilot.common.git import get_short_branch from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER -from openpilot.system.version import get_short_branch AlertSize = log.ControlsState.AlertSize AlertStatus = log.ControlsState.AlertStatus @@ -48,12 +49,12 @@ EVENT_NAME = {v: k for k, v in EventName.schema.enumerants.items()} class Events: def __init__(self): - self.events: List[int] = [] - self.static_events: List[int] = [] - self.events_prev = dict.fromkeys(EVENTS.keys(), 0) + self.events: list[int] = [] + self.static_events: list[int] = [] + self.event_counters = dict.fromkeys(EVENTS.keys(), 0) @property - def names(self) -> List[int]: + def names(self) -> list[int]: return self.events def __len__(self) -> int: @@ -61,17 +62,17 @@ class Events: def add(self, event_name: int, static: bool=False) -> None: if static: - self.static_events.append(event_name) - self.events.append(event_name) + bisect.insort(self.static_events, event_name) + bisect.insort(self.events, event_name) def clear(self) -> None: - self.events_prev = {k: (v + 1 if k in self.events else 0) for k, v in self.events_prev.items()} + self.event_counters = {k: (v + 1 if k in self.events else 0) for k, v in self.event_counters.items()} self.events = self.static_events.copy() def contains(self, event_type: str) -> bool: return any(event_type in EVENTS.get(e, {}) for e in self.events) - def create_alerts(self, event_types: List[str], callback_args=None): + def create_alerts(self, event_types: list[str], callback_args=None): if callback_args is None: callback_args = [] @@ -84,7 +85,7 @@ class Events: if not isinstance(alert, Alert): alert = alert(*callback_args) - if DT_CTRL * (self.events_prev[e] + 1) >= alert.creation_delay: + if DT_CTRL * (self.event_counters[e] + 1) >= alert.creation_delay: alert.alert_type = f"{EVENT_NAME[e]}/{et}" alert.event_type = et ret.append(alert) @@ -92,7 +93,7 @@ class Events: def add_from_msg(self, events): for e in events: - self.events.append(e.name.raw) + bisect.insort(self.events, e.name.raw) def to_msg(self): ret = [] @@ -132,7 +133,7 @@ class Alert: self.creation_delay = creation_delay self.alert_type = "" - self.event_type: Optional[str] = None + self.event_type: str | None = None def __str__(self) -> str: return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}" @@ -251,13 +252,6 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) -def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: - return Alert( - "Poor GPS reception", - "Hardware malfunctioning if sky is visible", - AlertStatus.normal, AlertSize.mid, - Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=300.) - # *** debug alerts *** def out_of_space_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: @@ -333,10 +327,11 @@ def joystick_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, -EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { +EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { # ********** events with no alerts ********** EventName.stockFcw: {}, + EventName.actuatorsApiUnavailable: {}, # ********** events only containing alerts displayed in all states ********** @@ -433,7 +428,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.preDriverDistracted: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "Pay Attention", "", AlertStatus.normal, AlertSize.small, @@ -441,7 +436,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.promptDriverDistracted: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "Pay Attention", "Driver Distracted", AlertStatus.userPrompt, AlertSize.mid, @@ -449,7 +444,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.driverDistracted: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Distracted", AlertStatus.critical, AlertSize.full, @@ -457,7 +452,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.preDriverUnresponsive: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "Touch Steering Wheel: No Face Detected", "", AlertStatus.normal, AlertSize.small, @@ -465,7 +460,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.promptDriverUnresponsive: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "Touch Steering Wheel", "Driver Unresponsive", AlertStatus.userPrompt, AlertSize.mid, @@ -473,7 +468,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.driverUnresponsive: { - ET.WARNING: Alert( + ET.PERMANENT: Alert( "DISENGAGE IMMEDIATELY", "Driver Unresponsive", AlertStatus.critical, AlertSize.full, @@ -493,7 +488,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { "Press Resume to Exit Standstill", "", AlertStatus.userPrompt, AlertSize.small, - Priority.MID, VisualAlert.none, AudibleAlert.none, .2), + Priority.LOW, VisualAlert.none, AudibleAlert.none, .2), }, EventName.belowSteerSpeed: { @@ -559,9 +554,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, # Unused - EventName.gpsMalfunction: { - ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Likely Hardware Issue"), - }, EventName.locationdTemporaryError: { ET.NO_ENTRY: NoEntryAlert("locationd Temporary Error"), @@ -696,7 +688,11 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { }, EventName.noGps: { - ET.PERMANENT: no_gps_alert, + ET.PERMANENT: Alert( + "Poor GPS reception", + "Ensure device has a clear view of the sky", + AlertStatus.normal, AlertSize.mid, + Priority.LOWER, VisualAlert.none, AudibleAlert.none, .2, creation_delay=600.) }, EventName.soundsUnavailable: { @@ -965,7 +961,7 @@ if __name__ == '__main__': from collections import defaultdict event_names = {v: k for k, v in EventName.schema.enumerants.items()} - alerts_by_type: Dict[str, Dict[Priority, List[str]]] = defaultdict(lambda: defaultdict(list)) + alerts_by_type: dict[str, dict[Priority, list[str]]] = defaultdict(lambda: defaultdict(list)) CP = car.CarParams.new_message() CS = car.CarState.new_message() @@ -977,7 +973,7 @@ if __name__ == '__main__': alert = alert(CP, CS, sm, False, 1) alerts_by_type[et][alert.priority].append(event_names[i]) - all_alerts: Dict[str, List[tuple[Priority, List[str]]]] = {} + all_alerts: dict[str, list[tuple[Priority, list[str]]]] = {} for et, priority_alerts in alerts_by_type.items(): all_alerts[et] = sorted(priority_alerts.items(), key=lambda x: x[0], reverse=True) diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript index 49666abe69..73242cb8f9 100644 --- a/selfdrive/controls/lib/lateral_mpc_lib/SConscript +++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch') +Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'opendbc_python') gen = "c_generated_code" @@ -60,6 +60,7 @@ lenv.Clean(generated_files, Dir(gen)) generated_lat = lenv.Command(generated_files, source_list, f"cd {Dir('.').abspath} && python3 lat_mpc.py") +lenv.Depends(generated_lat, [msgq_python, common_python, opendbc_python]) lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 83ec3b3a13..ad60861088 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -128,7 +128,7 @@ def gen_lat_ocp(): return ocp -class LateralMpc(): +class LateralMpc: def __init__(self, x0=None): if x0 is None: x0 = np.zeros(X_DIM) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ee65c4a69e..d08ee05035 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,13 +5,13 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_dead from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] + LongCtrlState = car.CarControl.Actuators.LongControlState def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_target_1sec, brake_pressed, cruise_standstill): - # Ignore cruise standstill if car has a gas interceptor - cruise_standstill = cruise_standstill and not CP.enableGasInterceptor accelerating = v_target_1sec > v_target planned_stop = (v_target < CP.vEgoStopping and v_target_1sec < CP.vEgoStopping and @@ -70,19 +70,19 @@ class LongControl: # Interp control trajectory speeds = long_plan.speeds if len(speeds) == CONTROL_N: - v_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) - a_target_now = interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels) + v_target_now = interp(t_since_plan, CONTROL_N_T_IDX, speeds) + a_target_now = interp(t_since_plan, CONTROL_N_T_IDX, long_plan.accels) - v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_lower = interp(self.CP.longitudinalActuatorDelayLowerBound + t_since_plan, CONTROL_N_T_IDX, speeds) a_target_lower = 2 * (v_target_lower - v_target_now) / self.CP.longitudinalActuatorDelayLowerBound - a_target_now - v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_upper = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan, CONTROL_N_T_IDX, speeds) a_target_upper = 2 * (v_target_upper - v_target_now) / self.CP.longitudinalActuatorDelayUpperBound - a_target_now v_target = min(v_target_lower, v_target_upper) a_target = min(a_target_lower, a_target_upper) - v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, ModelConstants.T_IDXS[:CONTROL_N], speeds) + v_target_1sec = interp(self.CP.longitudinalActuatorDelayUpperBound + t_since_plan + 1.0, CONTROL_N_T_IDX, speeds) else: v_target = 0.0 v_target_now = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript index 79afa1d918..592c1c2c2d 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript @@ -1,4 +1,4 @@ -Import('env', 'envCython', 'arch', 'messaging_python', 'common_python') +Import('env', 'envCython', 'arch', 'msgq_python', 'common_python', 'opendbc_python') gen = "c_generated_code" @@ -66,7 +66,7 @@ lenv.Clean(generated_files, Dir(gen)) generated_long = lenv.Command(generated_files, source_list, f"cd {Dir('.').abspath} && python3 long_mpc.py") -lenv.Depends(generated_long, [messaging_python, common_python]) +lenv.Depends(generated_long, [msgq_python, common_python, opendbc_python]) lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES") lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES") diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 39cfda4e8a..bad37add63 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -4,6 +4,7 @@ import time import numpy as np from cereal import log from openpilot.common.numpy_fast import clip +from openpilot.common.realtime import DT_MDL from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function @@ -220,8 +221,9 @@ def gen_long_ocp(): class LongitudinalMpc: - def __init__(self, mode='acc'): + def __init__(self, mode='acc', dt=DT_MDL): self.mode = mode + self.dt = dt self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] @@ -440,7 +442,7 @@ class LongitudinalMpc: self.a_solution = self.x_sol[:,2] self.j_solution = self.u_sol[:,0] - self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) + self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution) t = time.monotonic() if self.solution_status != 0: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 018768f248..ff5f901b52 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,8 +2,6 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params -from cereal import log import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -49,7 +47,7 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP - self.mpc = LongitudinalMpc() + self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt @@ -61,22 +59,12 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - self.params = Params() - self.param_read_counter = 0 - self.read_param() - self.personality = log.LongitudinalPersonality.standard - - def read_param(self): - try: - self.personality = int(self.params.get('LongitudinalPersonality')) - except (ValueError, TypeError): - self.personality = log.LongitudinalPersonality.standard @staticmethod def parse_model(model_msg, model_error): - if (len(model_msg.position.x) == 33 and - len(model_msg.velocity.x) == 33 and - len(model_msg.acceleration.x) == 33): + if (len(model_msg.position.x) == ModelConstants.IDX_N and + len(model_msg.velocity.x) == ModelConstants.IDX_N and + len(model_msg.acceleration.x) == ModelConstants.IDX_N): x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x) @@ -89,9 +77,6 @@ class LongitudinalPlanner: return x, v, a, j def update(self, sm): - if self.param_read_counter % 50 == 0: - self.read_param() - self.param_read_counter += 1 self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -130,11 +115,11 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=self.personality) + self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) @@ -170,6 +155,5 @@ class LongitudinalPlanner: longitudinalPlan.fcw = self.fcw longitudinalPlan.solverExecutionTime = self.mpc.solve_time - longitudinalPlan.personality = self.personality pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index f4ec7e5996..29c4d8bd46 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -4,7 +4,7 @@ from numbers import Number from openpilot.common.numpy_fast import clip, interp -class PIDController(): +class PIDController: def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): self._k_p = k_p self._k_i = k_i diff --git a/selfdrive/controls/lib/tests/__init__.py b/selfdrive/controls/lib/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py new file mode 100644 index 0000000000..8b9c18a9d4 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -0,0 +1,39 @@ +import random + +from openpilot.selfdrive.controls.lib.events import Alert, EVENTS +from openpilot.selfdrive.controls.lib.alertmanager import AlertManager + + +class TestAlertManager: + + def test_duration(self): + """ + Enforce that an alert lasts for max(alert duration, duration the alert is added) + """ + for duration in range(1, 100): + alert = None + while not isinstance(alert, Alert): + event = random.choice([e for e in EVENTS.values() if len(e)]) + alert = random.choice(list(event.values())) + + alert.duration = duration + + # check two cases: + # - alert is added to AM for <= the alert's duration + # - alert is added to AM for > alert's duration + for greater in (True, False): + if greater: + add_duration = duration + random.randint(1, 10) + else: + add_duration = random.randint(1, duration) + show_duration = max(duration, add_duration) + + AM = AlertManager() + for frame in range(duration+10): + if frame < add_duration: + AM.add_many(frame, [alert, ]) + current_alert = AM.process_alerts(frame, {}) + + shown = current_alert is not None + should_show = frame <= show_duration + assert shown == should_show, f"{frame=} {add_duration=} {duration=}" diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py new file mode 100644 index 0000000000..81411edec1 --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -0,0 +1,36 @@ +from parameterized import parameterized + +from cereal import car, log +from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.nissan.values import CAR as NISSAN +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel +from openpilot.common.mock.generators import generate_liveLocationKalman + + +class TestLatControl: + + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle)]) + def test_saturation(self, car_name, controller): + CarInterface, CarController, CarState = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CI = CarInterface(CP, CarController, CarState) + VM = VehicleModel(CP) + + controller = controller(CP, CI) + + CS = car.CarState.new_message() + CS.vEgo = 30 + CS.steeringPressed = False + + params = log.LiveParametersData.new_message() + + llk = generate_liveLocationKalman() + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, llk) + + assert lac_log.saturated diff --git a/selfdrive/controls/lib/tests/test_vehicle_model.py b/selfdrive/controls/lib/tests/test_vehicle_model.py new file mode 100644 index 0000000000..4d0e41805d --- /dev/null +++ b/selfdrive/controls/lib/tests/test_vehicle_model.py @@ -0,0 +1,64 @@ +import pytest +import math + +import numpy as np +from control import StateSpace + +from openpilot.selfdrive.car.honda.interface import CarInterface +from openpilot.selfdrive.car.honda.values import CAR +from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices + + +class TestVehicleModel: + def setup_method(self): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + self.VM = VehicleModel(CP) + + def test_round_trip_yaw_rate(self): + # TODO: fix VM to work at zero speed + for u in np.linspace(1, 30, num=10): + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + yr = self.VM.yaw_rate(sa, u, roll) + new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll) + + assert sa == pytest.approx(new_sa) + + def test_dyn_ss_sol_against_yaw_rate(self): + """Verify that the yaw_rate helper function matches the results + from the state space model.""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + + # Compute yaw rate based on state space model + _, yr1 = dyn_ss_sol(sa, u, roll, self.VM) + + # Compute yaw rate using direct computations + yr2 = self.VM.yaw_rate(sa, u, roll) + assert float(yr1[0]) == pytest.approx(yr2) + + def test_syn_ss_sol_simulate(self): + """Verifies that dyn_ss_sol matches a simulation""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + A, B = create_dyn_state_matrices(u, self.VM) + + # Convert to discrete time system + ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2))) + ss = ss.sample(0.01) + + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + inp = np.array([[sa], [roll]]) + + # Simulate for 1 second + x1 = np.zeros((2, 1)) + for _ in range(100): + x1 = ss.A @ x1 + ss.B @ inp + + # Compute steady state solution directly + x2 = dyn_ss_sol(sa, u, roll, self.VM) + + np.testing.assert_almost_equal(x1, x2, decimal=3) diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py index 0750384918..b6f50b4ba8 100755 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -12,7 +12,6 @@ x_dot = A*x + B*u A depends on longitudinal speed, u [m/s], and vehicle parameters CP """ -from typing import Tuple import numpy as np from numpy.linalg import solve @@ -169,7 +168,7 @@ def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray: return K * sa -def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, np.ndarray]: +def create_dyn_state_matrices(u: float, VM: VehicleModel) -> tuple[np.ndarray, np.ndarray]: """Returns the A and B matrix for the dynamics system Args: diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index b0b9350dec..7420f666f7 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -2,7 +2,7 @@ import importlib import math from collections import deque -from typing import Optional, Dict, Any +from typing import Any import capnp from cereal import messaging, log, car @@ -125,7 +125,7 @@ def laplacian_pdf(x: float, mu: float, b: float): return math.exp(-abs(x-mu)/b) -def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: Dict[int, Track]): +def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): @@ -133,7 +133,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks prob_y = laplacian_pdf(c.yRel, -lead.y[0], lead.yStd[0]) prob_v = laplacian_pdf(c.vRel + v_ego, lead.v[0], lead.vStd[0]) - # This is isn't exactly right, but good heuristic + # This isn't exactly right, but it's a good heuristic return prob_d * prob_y * prob_v track = max(tracks.values(), key=prob) @@ -166,8 +166,8 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa } -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]: +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: track = match_vision_to_track(v_ego, lead_msg, tracks) @@ -196,19 +196,19 @@ class RadarD: def __init__(self, radar_ts: float, delay: int = 0): self.current_time = 0.0 - self.tracks: Dict[int, Track] = {} + self.tracks: dict[int, Track] = {} self.kalman_params = KalmanParams(radar_ts) self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=delay+1) self.last_v_ego_frame = -1 - self.radar_state: Optional[capnp._DynamicStructBuilder] = None + self.radar_state: capnp._DynamicStructBuilder | None = None self.radar_state_valid = False self.ready = False - def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): + def update(self, sm: messaging.SubMaster, rr): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) diff --git a/selfdrive/controls/tests/__init__.py b/selfdrive/controls/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/controls/tests/test_alerts.py b/selfdrive/controls/tests/test_alerts.py new file mode 100644 index 0000000000..38dc045949 --- /dev/null +++ b/selfdrive/controls/tests/test_alerts.py @@ -0,0 +1,130 @@ +import copy +import json +import os +import random +from PIL import Image, ImageDraw, ImageFont + +from cereal import log, car +from cereal.messaging import SubMaster +from openpilot.common.basedir import BASEDIR +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.events import Alert, EVENTS, ET +from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS + +AlertSize = log.ControlsState.AlertSize + +OFFROAD_ALERTS_PATH = os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json") + +# TODO: add callback alerts +ALERTS = [] +for event_types in EVENTS.values(): + for alert in event_types.values(): + ALERTS.append(alert) + + +class TestAlerts: + + @classmethod + def setup_class(cls): + with open(OFFROAD_ALERTS_PATH) as f: + cls.offroad_alerts = json.loads(f.read()) + + # Create fake objects for callback + cls.CS = car.CarState.new_message() + cls.CP = car.CarParams.new_message() + cfg = [c for c in CONFIGS if c.proc_name == 'controlsd'][0] + cls.sm = SubMaster(cfg.pubs) + + def test_events_defined(self): + # Ensure all events in capnp schema are defined in events.py + events = car.CarEvent.EventName.schema.enumerants + + for name, e in events.items(): + if not name.endswith("DEPRECATED"): + fail_msg = "%s @%d not in EVENTS" % (name, e) + assert e in EVENTS.keys(), fail_msg + + # ensure alert text doesn't exceed allowed width + def test_alert_text_length(self): + font_path = os.path.join(BASEDIR, "selfdrive/assets/fonts") + regular_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") + bold_font_path = os.path.join(font_path, "Inter-Bold.ttf") + semibold_font_path = os.path.join(font_path, "Inter-SemiBold.ttf") + + max_text_width = 2160 - 300 # full screen width is usable, minus sidebar + draw = ImageDraw.Draw(Image.new('RGB', (0, 0))) + + fonts = { + AlertSize.small: [ImageFont.truetype(semibold_font_path, 74)], + AlertSize.mid: [ImageFont.truetype(bold_font_path, 88), + ImageFont.truetype(regular_font_path, 66)], + } + + for alert in ALERTS: + if not isinstance(alert, Alert): + alert = alert(self.CP, self.CS, self.sm, metric=False, soft_disable_time=100) + + # for full size alerts, both text fields wrap the text, + # so it's unlikely that they would go past the max width + if alert.alert_size in (AlertSize.none, AlertSize.full): + continue + + for i, txt in enumerate([alert.alert_text_1, alert.alert_text_2]): + if i >= len(fonts[alert.alert_size]): + break + + font = fonts[alert.alert_size][i] + left, _, right, _ = draw.textbbox((0, 0), txt, font) + width = right - left + msg = f"type: {alert.alert_type} msg: {txt}" + assert width <= max_text_width, msg + + def test_alert_sanity_check(self): + for event_types in EVENTS.values(): + for event_type, a in event_types.items(): + # TODO: add callback alerts + if not isinstance(a, Alert): + continue + + if a.alert_size == AlertSize.none: + assert len(a.alert_text_1) == 0 + assert len(a.alert_text_2) == 0 + elif a.alert_size == AlertSize.small: + assert len(a.alert_text_1) > 0 + assert len(a.alert_text_2) == 0 + elif a.alert_size == AlertSize.mid: + assert len(a.alert_text_1) > 0 + assert len(a.alert_text_2) > 0 + else: + assert len(a.alert_text_1) > 0 + + assert a.duration >= 0. + + if event_type not in (ET.WARNING, ET.PERMANENT, ET.PRE_ENABLE): + assert a.creation_delay == 0. + + def test_offroad_alerts(self): + params = Params() + for a in self.offroad_alerts: + # set the alert + alert = copy.copy(self.offroad_alerts[a]) + set_offroad_alert(a, True) + alert['extra'] = '' + assert json.dumps(alert) == params.get(a, encoding='utf8') + + # then delete it + set_offroad_alert(a, False) + assert params.get(a) is None + + def test_offroad_alerts_extra_text(self): + params = Params() + for i in range(50): + # set the alert + a = random.choice(list(self.offroad_alerts)) + alert = self.offroad_alerts[a] + set_offroad_alert(a, True, extra_text="a"*i) + + written_alert = json.loads(params.get(a, encoding='utf8')) + assert "a"*i == written_alert['extra'] + assert alert["text"] == written_alert['text'] diff --git a/selfdrive/controls/tests/test_cruise_speed.py b/selfdrive/controls/tests/test_cruise_speed.py new file mode 100644 index 0000000000..8f451a49bc --- /dev/null +++ b/selfdrive/controls/tests/test_cruise_speed.py @@ -0,0 +1,151 @@ +import pytest +import itertools +import numpy as np + +from parameterized import parameterized_class +from cereal import log +from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, V_CRUISE_MIN, V_CRUISE_MAX, V_CRUISE_INITIAL, IMPERIAL_INCREMENT +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + +ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type + + +def run_cruise_simulation(cruise, e2e, personality, t_end=20.): + man = Maneuver( + '', + duration=t_end, + initial_speed=max(cruise - 1., 0.0), + lead_relevancy=True, + initial_distance_lead=100, + cruise_values=[cruise], + prob_lead_values=[0.0], + breakpoints=[0.], + e2e=e2e, + personality=personality, + ) + valid, output = man.evaluate() + assert valid + return output[-1, 3] + + +@parameterized_class(("e2e", "personality", "speed"), itertools.product( + [True, False], # e2e + log.LongitudinalPersonality.schema.enumerants, # personality + [5,35])) # speed +class TestCruiseSpeed: + def test_cruise_speed(self): + print(f'Testing {self.speed} m/s') + cruise_speed = float(self.speed) + + simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) + assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' + + +# TODO: test pcmCruise +@parameterized_class(('pcm_cruise',), [(False,)]) +class TestVCruiseHelper: + def setup_method(self): + self.CP = car.CarParams(pcmCruise=self.pcm_cruise) + self.v_cruise_helper = VCruiseHelper(self.CP) + self.reset_cruise_speed_state() + + def reset_cruise_speed_state(self): + # Two resets previous cruise speed + for _ in range(2): + self.v_cruise_helper.update_v_cruise(car.CarState(cruiseState={"available": False}), enabled=False, is_metric=False) + + def enable(self, v_ego, experimental_mode): + # Simulates user pressing set with a current speed + self.v_cruise_helper.initialize_v_cruise(car.CarState(vEgo=v_ego), experimental_mode) + + def test_adjust_speed(self): + """ + Asserts speed changes on falling edges of buttons. + """ + + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) + + for btn in (ButtonType.accelCruise, ButtonType.decelCruise): + for pressed in (True, False): + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=btn, pressed=pressed)] + + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) + + def test_rising_edge_enable(self): + """ + Some car interfaces may enable on rising edge of a button, + ensure we don't adjust speed if enabled changes mid-press. + """ + + # NOTE: enabled is always one frame behind the result from button press in controlsd + for enabled, pressed in ((False, False), + (False, True), + (True, False)): + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=pressed)] + self.v_cruise_helper.update_v_cruise(CS, enabled=enabled, is_metric=False) + if pressed: + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) + + # Expected diff on enabling. Speed should not change on falling edge of pressed + assert not pressed == self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last + + def test_resume_in_standstill(self): + """ + Asserts we don't increment set speed if user presses resume/accel to exit cruise standstill. + """ + + self.enable(0, False) + + for standstill in (True, False): + for pressed in (True, False): + CS = car.CarState(cruiseState={"available": True, "standstill": standstill}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + + # speed should only update if not at standstill and button falling edge + should_equal = standstill or pressed + assert should_equal == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last) + + def test_set_gas_pressed(self): + """ + Asserts pressing set while enabled with gas pressed sets + the speed to the maximum of vEgo and current cruise speed. + """ + + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) + + # first decrement speed, then perform gas pressed logic + expected_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - IMPERIAL_INCREMENT + expected_v_cruise_kph = max(expected_v_cruise_kph, v_ego * CV.MS_TO_KPH) # clip to min of vEgo + expected_v_cruise_kph = float(np.clip(round(expected_v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)) + + CS = car.CarState(vEgo=float(v_ego), gasPressed=True, cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=ButtonType.decelCruise, pressed=False)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=False) + + # TODO: fix skipping first run due to enabled on rising edge exception + if v_ego == 0.0: + continue + assert expected_v_cruise_kph == self.v_cruise_helper.v_cruise_kph + + def test_initialize_v_cruise(self): + """ + Asserts allowed cruise speeds on enabling with SET. + """ + + for experimental_mode in (True, False): + for v_ego in np.linspace(0, 100, 101): + self.reset_cruise_speed_state() + assert not self.v_cruise_helper.v_cruise_initialized + + self.enable(float(v_ego), experimental_mode) + assert V_CRUISE_INITIAL <= self.v_cruise_helper.v_cruise_kph <= V_CRUISE_MAX + assert self.v_cruise_helper.v_cruise_initialized diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py new file mode 100644 index 0000000000..0fd543dd60 --- /dev/null +++ b/selfdrive/controls/tests/test_following_distance.py @@ -0,0 +1,40 @@ +import pytest +import itertools +from parameterized import parameterized_class + +from cereal import log + +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance, get_T_FOLLOW +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + + +def run_following_distance_simulation(v_lead, t_end=100.0, e2e=False, personality=0): + man = Maneuver( + '', + duration=t_end, + initial_speed=float(v_lead), + lead_relevancy=True, + initial_distance_lead=100, + speed_lead_values=[v_lead], + breakpoints=[0.], + e2e=e2e, + personality=personality, + ) + valid, output = man.evaluate() + assert valid + return output[-1,2] - output[-1,1] + + +@parameterized_class(("e2e", "personality", "speed"), itertools.product( + [True, False], # e2e + [log.LongitudinalPersonality.relaxed, # personality + log.LongitudinalPersonality.standard, + log.LongitudinalPersonality.aggressive], + [0,10,35])) # speed +class TestFollowingDistance: + def test_following_distance(self): + v_lead = float(self.speed) + simulation_steady_state = run_following_distance_simulation(v_lead, e2e=self.e2e, personality=self.personality) + correct_steady_state = desired_follow_distance(v_lead, v_lead, get_T_FOLLOW(self.personality)) + err_ratio = 0.2 if self.e2e else 0.1 + assert simulation_steady_state == pytest.approx(correct_steady_state, abs=err_ratio * correct_steady_state + .5) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py new file mode 100644 index 0000000000..3aa0fd1bce --- /dev/null +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -0,0 +1,85 @@ +import pytest +import numpy as np +from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc +from openpilot.selfdrive.controls.lib.drive_helpers import CAR_ROTATION_RADIUS +from openpilot.selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N + + +def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0., + lane_width=3.6, poly_shift=0.): + + if lat_mpc is None: + lat_mpc = LateralMpc() + lat_mpc.set_weights(1., .1, 0.0, .05, 800) + + y_pts = poly_shift * np.ones(LAT_MPC_N + 1) + heading_pts = np.zeros(LAT_MPC_N + 1) + curv_rate_pts = np.zeros(LAT_MPC_N + 1) + + x0 = np.array([x_init, y_init, psi_init, curvature_init]) + p = np.column_stack([v_ref * np.ones(LAT_MPC_N + 1), + CAR_ROTATION_RADIUS * np.ones(LAT_MPC_N + 1)]) + + # converge in no more than 10 iterations + for _ in range(10): + lat_mpc.run(x0, p, + y_pts, heading_pts, curv_rate_pts) + return lat_mpc.x_sol + + +class TestLateralMpc: + + def _assert_null(self, sol, curvature=1e-6): + for i in range(len(sol)): + assert sol[0,i,1] == pytest.approx(0, abs=curvature) + assert sol[0,i,2] == pytest.approx(0, abs=curvature) + assert sol[0,i,3] == pytest.approx(0, abs=curvature) + + def _assert_simmetry(self, sol, curvature=1e-6): + for i in range(len(sol)): + assert sol[0,i,1] == pytest.approx(-sol[1,i,1], abs=curvature) + assert sol[0,i,2] == pytest.approx(-sol[1,i,2], abs=curvature) + assert sol[0,i,3] == pytest.approx(-sol[1,i,3], abs=curvature) + assert sol[0,i,0] == pytest.approx(sol[1,i,0], abs=curvature) + + def test_straight(self): + sol = run_mpc() + self._assert_null(np.array([sol])) + + def test_y_symmetry(self): + sol = [] + for y_init in [-0.5, 0.5]: + sol.append(run_mpc(y_init=y_init)) + self._assert_simmetry(np.array(sol)) + + def test_poly_symmetry(self): + sol = [] + for poly_shift in [-1., 1.]: + sol.append(run_mpc(poly_shift=poly_shift)) + self._assert_simmetry(np.array(sol)) + + def test_curvature_symmetry(self): + sol = [] + for curvature_init in [-0.1, 0.1]: + sol.append(run_mpc(curvature_init=curvature_init)) + self._assert_simmetry(np.array(sol)) + + def test_psi_symmetry(self): + sol = [] + for psi_init in [-0.1, 0.1]: + sol.append(run_mpc(psi_init=psi_init)) + self._assert_simmetry(np.array(sol)) + + def test_no_overshoot(self): + y_init = 1. + sol = run_mpc(y_init=y_init) + for y in list(sol[:,1]): + assert y_init >= abs(y) + + def test_switch_convergence(self): + lat_mpc = LateralMpc() + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0) + right_psi_deg = np.degrees(sol[:,2]) + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0) + left_psi_deg = np.degrees(sol[:,2]) + np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3) diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py new file mode 100644 index 0000000000..f1f4449afd --- /dev/null +++ b/selfdrive/controls/tests/test_leads.py @@ -0,0 +1,29 @@ +import cereal.messaging as messaging + +from openpilot.selfdrive.test.process_replay import replay_process_with_name +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA + + +class TestLeads: + def test_radar_fault(self): + # if there's no radar-related can traffic, radard should either not respond or respond with an error + # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check + def single_iter_pkg(): + # single iter package, with meaningless cans and empty carState/modelV2 + msgs = [] + for _ in range(5): + can = messaging.new_message("can", 1) + cs = messaging.new_message("carState") + msgs.append(can.as_reader()) + msgs.append(cs.as_reader()) + model = messaging.new_message("modelV2") + msgs.append(model.as_reader()) + + return msgs + + msgs = [m for _ in range(3) for m in single_iter_pkg()] + out = replay_process_with_name("radard", msgs, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2) + states = [m for m in out if m.which() == "radarState"] + failures = [not state.valid and len(state.radarState.radarErrors) for state in states] + + assert len(states) == 0 or all(failures) diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py new file mode 100644 index 0000000000..14b0788a3d --- /dev/null +++ b/selfdrive/controls/tests/test_startup.py @@ -0,0 +1,121 @@ +import os +from parameterized import parameterized + +from cereal import log, car +import cereal.messaging as messaging +from openpilot.common.params import Params +from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp +from openpilot.selfdrive.car.fingerprints import _FINGERPRINTS +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.mazda.values import CAR as MAZDA +from openpilot.selfdrive.controls.lib.events import EVENT_NAME +from openpilot.system.manager.process_config import managed_processes + +EventName = car.CarEvent.EventName +Ecu = car.CarParams.Ecu + +COROLLA_FW_VERSIONS = [ + (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.abs, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'), + (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'), + (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'), + (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'), +] +COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')] +COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1] + +CX5_FW_VERSIONS = [ + (Ecu.engine, 0x7e0, None, b'PYNF-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.abs, 0x760, None, b'K123-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.eps, 0x730, None, b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdRadar, 0x764, None, b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.fwdCamera, 0x706, None, b'B61L-67XK2-T\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), + (Ecu.transmission, 0x7e1, None, b'PYNC-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'), +] + + +@parameterized.expand([ + # TODO: test EventName.startup for release branches + + # officially supported car + (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), + (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS, "toyota"), + + # dashcamOnly car + (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), + (EventName.startupNoControl, MAZDA.MAZDA_CX5, CX5_FW_VERSIONS, "mazda"), + + # unrecognized car with no fw + (EventName.startupNoFw, None, None, ""), + (EventName.startupNoFw, None, None, ""), + + # unrecognized car + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1], "toyota"), + + # fuzzy match + (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), + (EventName.startupMaster, TOYOTA.TOYOTA_COROLLA, COROLLA_FW_VERSIONS_FUZZY, "toyota"), +]) +def test_startup_alert(expected_event, car_model, fw_versions, brand): + controls_sock = messaging.sub_sock("controlsState") + pm = messaging.PubMaster(['can', 'pandaStates']) + + params = Params() + params.put_bool("OpenpilotEnabledToggle", True) + + # Build capnn version of FW array + if fw_versions is not None: + car_fw = [] + cp = car.CarParams.new_message() + for ecu, addr, subaddress, version in fw_versions: + f = car.CarParams.CarFw.new_message() + f.ecu = ecu + f.address = addr + f.fwVersion = version + f.brand = brand + + if subaddress is not None: + f.subAddress = subaddress + + car_fw.append(f) + cp.carVin = "1" * 17 + cp.carFw = car_fw + params.put("CarParamsCache", cp.to_bytes()) + else: + os.environ['SKIP_FW_QUERY'] = '1' + + managed_processes['controlsd'].start() + managed_processes['card'].start() + + assert pm.wait_for_readers_to_update('can', 5) + pm.send('can', can_list_to_can_capnp([[0, 0, b"", 0]])) + + assert pm.wait_for_readers_to_update('pandaStates', 5) + msg = messaging.new_message('pandaStates', 1) + msg.pandaStates[0].pandaType = log.PandaState.PandaType.uno + pm.send('pandaStates', msg) + + # fingerprint + if (car_model is None) or (fw_versions is not None): + finger = {addr: 1 for addr in range(1, 100)} + else: + finger = _FINGERPRINTS[car_model][0] + + msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()] + for _ in range(1000): + # card waits for pandad to echo back that it has changed the multiplexing mode + if not params.get_bool("ObdMultiplexingChanged"): + params.put_bool("ObdMultiplexingChanged", True) + + pm.send('can', can_list_to_can_capnp(msgs)) + assert pm.wait_for_readers_to_update('can', 5, dt=0.001), f"step: {_}" + + ctrls = messaging.drain_sock(controls_sock) + if len(ctrls): + event_name = ctrls[0].controlsState.alertType.split("/")[0] + assert EVENT_NAME[expected_event] == event_name, f"expected {EVENT_NAME[expected_event]} for '{car_model}', got {event_name}" + break + else: + raise Exception(f"failed to fingerprint {car_model}") diff --git a/selfdrive/controls/tests/test_state_machine.py b/selfdrive/controls/tests/test_state_machine.py new file mode 100644 index 0000000000..b6ec512dc4 --- /dev/null +++ b/selfdrive/controls/tests/test_state_machine.py @@ -0,0 +1,102 @@ +from cereal import car, log +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.car_helpers import interfaces +from openpilot.selfdrive.controls.controlsd import Controls, SOFT_DISABLE_TIME +from openpilot.selfdrive.controls.lib.events import Events, ET, Alert, Priority, AlertSize, AlertStatus, VisualAlert, \ + AudibleAlert, EVENTS +from openpilot.selfdrive.car.mock.values import CAR as MOCK + +State = log.ControlsState.OpenpilotState + +# The event types that maintain the current state +MAINTAIN_STATES = {State.enabled: (None,), State.disabled: (None,), State.softDisabling: (ET.SOFT_DISABLE,), + State.preEnabled: (ET.PRE_ENABLE,), State.overriding: (ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL)} +ALL_STATES = tuple(State.schema.enumerants.values()) +# The event types checked in DISABLED section of state machine +ENABLE_EVENT_TYPES = (ET.ENABLE, ET.PRE_ENABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL) + + +def make_event(event_types): + event = {} + for ev in event_types: + event[ev] = Alert("", "", AlertStatus.normal, AlertSize.small, Priority.LOW, + VisualAlert.none, AudibleAlert.none, 1.) + EVENTS[0] = event + return 0 + + +class TestStateMachine: + + def setup_method(self): + CarInterface, CarController, CarState = interfaces[MOCK.MOCK] + CP = CarInterface.get_non_essential_params(MOCK.MOCK) + CI = CarInterface(CP, CarController, CarState) + + self.controlsd = Controls(CI=CI) + self.controlsd.events = Events() + self.controlsd.soft_disable_timer = int(SOFT_DISABLE_TIME / DT_CTRL) + self.CS = car.CarState() + + def test_immediate_disable(self): + for state in ALL_STATES: + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.IMMEDIATE_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + assert State.disabled == self.controlsd.state + self.controlsd.events.clear() + + def test_user_disable(self): + for state in ALL_STATES: + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.USER_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + assert State.disabled == self.controlsd.state + self.controlsd.events.clear() + + def test_soft_disable(self): + for state in ALL_STATES: + if state == State.preEnabled: # preEnabled considers NO_ENTRY instead + continue + for et in MAINTAIN_STATES[state]: + self.controlsd.events.add(make_event([et, ET.SOFT_DISABLE])) + self.controlsd.state = state + self.controlsd.state_transition(self.CS) + assert self.controlsd.state == State.disabled if state == State.disabled else State.softDisabling + self.controlsd.events.clear() + + def test_soft_disable_timer(self): + self.controlsd.state = State.enabled + self.controlsd.events.add(make_event([ET.SOFT_DISABLE])) + self.controlsd.state_transition(self.CS) + for _ in range(int(SOFT_DISABLE_TIME / DT_CTRL)): + assert self.controlsd.state == State.softDisabling + self.controlsd.state_transition(self.CS) + + assert self.controlsd.state == State.disabled + + def test_no_entry(self): + # Make sure noEntry keeps us disabled + for et in ENABLE_EVENT_TYPES: + self.controlsd.events.add(make_event([ET.NO_ENTRY, et])) + self.controlsd.state_transition(self.CS) + assert self.controlsd.state == State.disabled + self.controlsd.events.clear() + + def test_no_entry_pre_enable(self): + # preEnabled with noEntry event + self.controlsd.state = State.preEnabled + self.controlsd.events.add(make_event([ET.NO_ENTRY, ET.PRE_ENABLE])) + self.controlsd.state_transition(self.CS) + assert self.controlsd.state == State.preEnabled + + def test_maintain_states(self): + # Given current state's event type, we should maintain state + for state in ALL_STATES: + for et in MAINTAIN_STATES[state]: + self.controlsd.state = state + self.controlsd.events.add(make_event([et])) + self.controlsd.state_transition(self.CS) + assert self.controlsd.state == state + self.controlsd.events.clear() diff --git a/selfdrive/debug/README.md b/selfdrive/debug/README.md new file mode 100644 index 0000000000..83b8a994db --- /dev/null +++ b/selfdrive/debug/README.md @@ -0,0 +1,59 @@ +# debug scripts + +## [can_printer.py](can_printer.py) + +``` +usage: can_printer.py [-h] [--bus BUS] [--max_msg MAX_MSG] [--addr ADDR] + +simple CAN data viewer + +optional arguments: + -h, --help show this help message and exit + --bus BUS CAN bus to print out (default: 0) + --max_msg MAX_MSG max addr (default: None) + --addr ADDR +``` + +## [dump.py](dump.py) + +``` +usage: dump.py [-h] [--pipe] [--raw] [--json] [--dump-json] [--no-print] [--addr ADDR] [--values VALUES] [socket [socket ...]] + +Dump communication sockets. See cereal/services.py for a complete list of available sockets. + +positional arguments: + socket socket names to dump. defaults to all services defined in cereal + +optional arguments: + -h, --help show this help message and exit + --pipe + --raw + --json + --dump-json + --no-print + --addr ADDR + --values VALUES values to monitor (instead of entire event) +``` + +## [vw_mqb_config.py](vw_mqb_config.py) + +``` +usage: vw_mqb_config.py [-h] [--debug] {enable,show,disable} + +Shows Volkswagen EPS software and coding info, and enables or disables Heading Control +Assist (Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want +to use openpilot integrated at the CAN gateway (J533). + +positional arguments: + {enable,show,disable} + show or modify current EPS HCA config + +optional arguments: + -h, --help show this help message and exit + --debug enable ISO-TP/UDS stack debugging output + +This tool is meant to run directly on a vehicle-installed comma three, with +the openpilot/tmux processes stopped. It should also work on a separate PC with a USB- +attached comma panda. Vehicle ignition must be on. Recommend engine not be running when +making changes. Must turn ignition off and on again for any changes to take effect. +``` diff --git a/selfdrive/debug/__init__.py b/selfdrive/debug/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh new file mode 100755 index 0000000000..919a82fefc --- /dev/null +++ b/selfdrive/debug/adb.sh @@ -0,0 +1,11 @@ +#!/usr/bin/bash +set -e + +PORT=5555 + +setprop service.adb.tcp.port $PORT +sudo systemctl start adbd + +IP=$(echo $SSH_CONNECTION | awk '{ print $3}') +echo "then, connect on your computer:" +echo "adb connect $IP:$PORT" diff --git a/selfdrive/debug/can_print_changes.py b/selfdrive/debug/can_print_changes.py new file mode 100755 index 0000000000..97d60b2b05 --- /dev/null +++ b/selfdrive/debug/can_print_changes.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 +import argparse +import binascii +import time +from collections import defaultdict + +import cereal.messaging as messaging +from openpilot.selfdrive.debug.can_table import can_table +from openpilot.tools.lib.logreader import LogIterable, LogReader + +RED = '\033[91m' +CLEAR = '\033[0m' + +def update(msgs, bus, dat, low_to_high, high_to_low, quiet=False): + for x in msgs: + if x.which() != 'can': + continue + + for y in x.can: + if y.src == bus: + dat[y.address] = y.dat + + i = int.from_bytes(y.dat, byteorder='big') + l_h = low_to_high[y.address] + h_l = high_to_low[y.address] + + change = None + if (i | l_h) != l_h: + low_to_high[y.address] = i | l_h + change = "+" + + if (~i | h_l) != h_l: + high_to_low[y.address] = ~i | h_l + change = "-" + + if change and not quiet: + print(f"{time.monotonic():.2f}\t{hex(y.address)} ({y.address})\t{change}{binascii.hexlify(y.dat)}") + + +def can_printer(bus=0, init_msgs=None, new_msgs=None, table=False): + logcan = messaging.sub_sock('can', timeout=10) + + dat = defaultdict(int) + low_to_high = defaultdict(int) + high_to_low = defaultdict(int) + + if init_msgs is not None: + update(init_msgs, bus, dat, low_to_high, high_to_low, quiet=True) + + low_to_high_init = low_to_high.copy() + high_to_low_init = high_to_low.copy() + + if new_msgs is not None: + update(new_msgs, bus, dat, low_to_high, high_to_low) + else: + # Live mode + print(f"Waiting for messages on bus {bus}") + try: + while 1: + can_recv = messaging.drain_sock(logcan) + update(can_recv, bus, dat, low_to_high, high_to_low) + time.sleep(0.02) + except KeyboardInterrupt: + pass + + print("\n\n") + tables = "" + for addr in sorted(dat.keys()): + init = low_to_high_init[addr] & high_to_low_init[addr] + now = low_to_high[addr] & high_to_low[addr] + d = now & ~init + if d == 0: + continue + b = d.to_bytes(len(dat[addr]), byteorder='big') + + byts = ''.join([(c if c == '0' else f'{RED}{c}{CLEAR}') for c in str(binascii.hexlify(b))[2:-1]]) + header = f"{hex(addr).ljust(6)}({str(addr).ljust(4)})" + print(header, byts) + tables += f"{header}\n" + tables += can_table(b) + "\n\n" + + if table: + print(tables) + +if __name__ == "__main__": + desc = """Collects messages and prints when a new bit transition is observed. + This is very useful to find signals based on user triggered actions, such as blinkers and seatbelt. + Leave the script running until no new transitions are seen, then perform the action.""" + parser = argparse.ArgumentParser(description=desc, + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0) + parser.add_argument("--table", action="store_true", help="Print a cabana-like table") + parser.add_argument("init", type=str, nargs='?', help="Route or segment to initialize with. Use empty quotes to compare against all zeros.") + parser.add_argument("comp", type=str, nargs='?', help="Route or segment to compare against init") + + args = parser.parse_args() + + init_lr: LogIterable | None = None + new_lr: LogIterable | None = None + + if args.init: + if args.init == '': + init_lr = [] + else: + init_lr = LogReader(args.init) + if args.comp: + new_lr = LogReader(args.comp) + + can_printer(args.bus, init_msgs=init_lr, new_msgs=new_lr, table=args.table) diff --git a/selfdrive/debug/can_table.py b/selfdrive/debug/can_table.py new file mode 100755 index 0000000000..11d070e708 --- /dev/null +++ b/selfdrive/debug/can_table.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 +import argparse +import pandas as pd + +import cereal.messaging as messaging + + +def can_table(dat): + rows = [] + for b in dat: + r = list(bin(b).lstrip('0b').zfill(8)) + r += [hex(b)] + rows.append(r) + + df = pd.DataFrame(data=rows) + df.columns = [str(n) for n in range(7, -1, -1)] + [' '] + table = df.to_markdown(tablefmt='grid') + return table + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Cabana-like table of bits for your terminal", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("addr", type=str, nargs=1) + parser.add_argument("bus", type=int, default=0, nargs='?') + + args = parser.parse_args() + + addr = int(args.addr[0], 0) + can = messaging.sub_sock('can', conflate=False, timeout=None) + + print(f"waiting for {hex(addr)} ({addr}) on bus {args.bus}...") + + latest = None + while True: + for msg in messaging.drain_sock(can, wait_for_one=True): + for m in msg.can: + if m.address == addr and m.src == args.bus: + latest = m + + if latest is None: + continue + + table = can_table(latest.dat) + print(f"\n\n{hex(addr)} ({addr}) on bus {args.bus}\n{table}") diff --git a/selfdrive/debug/check_can_parser_performance.py b/selfdrive/debug/check_can_parser_performance.py new file mode 100755 index 0000000000..604a1df124 --- /dev/null +++ b/selfdrive/debug/check_can_parser_performance.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import numpy as np +import time +from tqdm import tqdm + +from cereal import car +from openpilot.selfdrive.car.tests.routes import CarTestRoute +from openpilot.selfdrive.car.tests.test_models import TestCarModelBase +from openpilot.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)): + msgs = [(m.as_builder().to_bytes(),) for m in tm.can_msgs] + start_t = time.process_time_ns() + for msg in msgs: + for cp in tm.CI.can_parsers: + if cp is not None: + cp.update_strings(msg) + 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') diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py index 7e7b05e950..1765aeb86b 100755 --- a/selfdrive/debug/check_freq.py +++ b/selfdrive/debug/check_freq.py @@ -3,7 +3,7 @@ import argparse import numpy as np import time from collections import defaultdict, deque -from typing import DefaultDict, Deque, MutableSequence +from collections.abc import MutableSequence import cereal.messaging as messaging @@ -19,8 +19,8 @@ if __name__ == "__main__": socket_names = args.socket sockets = {} - rcv_times: DefaultDict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) - valids: DefaultDict[str, Deque[bool]] = defaultdict(lambda: deque(maxlen=100)) + rcv_times: defaultdict[str, MutableSequence[float]] = defaultdict(lambda: deque(maxlen=100)) + valids: defaultdict[str, deque[bool]] = defaultdict(lambda: deque(maxlen=100)) t = time.monotonic() for name in socket_names: diff --git a/selfdrive/debug/check_lag.py b/selfdrive/debug/check_lag.py new file mode 100755 index 0000000000..341ae79c8b --- /dev/null +++ b/selfdrive/debug/check_lag.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST + +TO_CHECK = ['carState'] + + +if __name__ == "__main__": + sm = messaging.SubMaster(TO_CHECK) + + prev_t: dict[str, float] = {} + + while True: + sm.update() + + for s in TO_CHECK: + if sm.updated[s]: + t = sm.logMonoTime[s] / 1e9 + + if s in prev_t: + expected = 1.0 / (SERVICE_LIST[s].frequency) + dt = t - prev_t[s] + if dt > 10 * expected: + print(t, s, dt) + + prev_t[s] = t diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py new file mode 100755 index 0000000000..fc527cd81b --- /dev/null +++ b/selfdrive/debug/check_timings.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +import sys +import time +import numpy as np +import datetime +from collections.abc import MutableSequence +from collections import defaultdict + +import cereal.messaging as messaging + + +if __name__ == "__main__": + ts: defaultdict[str, MutableSequence[float]] = defaultdict(list) + socks = {s: messaging.sub_sock(s, conflate=False) for s in sys.argv[1:]} + try: + st = time.monotonic() + while True: + print() + for s, sock in socks.items(): + msgs = messaging.drain_sock(sock) + for m in msgs: + ts[s].append(m.logMonoTime / 1e6) + + if len(ts[s]) > 2: + d = np.diff(ts[s])[-100:] + print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}") + time.sleep(1) + except KeyboardInterrupt: + print("\n") + print("="*5, "timing summary", "="*5) + for s, sock in socks.items(): + msgs = messaging.drain_sock(sock) + if len(ts[s]) > 2: + d = np.diff(ts[s]) + print(f"{s:25} {np.mean(d):7.2f} {np.std(d):7.2f} {np.max(d):7.2f} {np.min(d):7.2f}") + print("="*5, datetime.timedelta(seconds=time.monotonic()-st), "="*5) diff --git a/selfdrive/debug/clear_dtc.py b/selfdrive/debug/clear_dtc.py new file mode 100755 index 0000000000..a931c423a9 --- /dev/null +++ b/selfdrive/debug/clear_dtc.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +import sys +import argparse +from subprocess import check_output, CalledProcessError +from panda import Panda +from panda.python.uds import UdsClient, MessageTimeoutError, SESSION_TYPE, DTC_GROUP_TYPE + +parser = argparse.ArgumentParser(description="clear DTC status") +parser.add_argument("addr", type=lambda x: int(x,0), nargs="?", default=0x7DF) # default is functional (broadcast) address +parser.add_argument("--bus", type=int, default=0) +parser.add_argument('--debug', action='store_true') +args = parser.parse_args() + +try: + check_output(["pidof", "pandad"]) + print("pandad is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) +except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (pandad not running) + raise e + +panda = Panda() +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) +print("extended diagnostic session ...") +try: + uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + raise +print("clear diagnostic info ...") +try: + uds_client.clear_diagnostic_information(DTC_GROUP_TYPE.ALL) +except MessageTimeoutError: + # functional address isn't properly handled so a timeout occurs + if args.addr != 0x7DF: + pass +print("") +print("you may need to power cycle your vehicle now") diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py new file mode 100755 index 0000000000..5942054757 --- /dev/null +++ b/selfdrive/debug/count_events.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +import sys +import math +import datetime +from collections import Counter +from pprint import pprint +from typing import cast + +from cereal.services import SERVICE_LIST +from openpilot.tools.lib.logreader import LogReader, ReadMode + +if __name__ == "__main__": + cnt_events: Counter = Counter() + + cams = [s for s in SERVICE_LIST if s.endswith('CameraState')] + cnt_cameras = dict.fromkeys(cams, 0) + + events: list[tuple[float, set[str]]] = [] + alerts: list[tuple[float, str]] = [] + start_time = math.inf + end_time = -math.inf + ignition_off = None + for msg in LogReader(sys.argv[1], ReadMode.QLOG): + t = (msg.logMonoTime - start_time) / 1e9 + end_time = max(end_time, msg.logMonoTime) + start_time = min(start_time, msg.logMonoTime) + + if msg.which() == 'onroadEvents': + for e in msg.onroadEvents: + cnt_events[e.name] += 1 + + ae = {str(e.name) for e in msg.onroadEvents if e.name not in ('pedalPressed', 'steerOverride', 'gasPressedOverride')} + if len(events) == 0 or ae != events[-1][1]: + events.append((t, ae)) + + elif msg.which() == 'controlsState': + at = msg.controlsState.alertType + if "/override" not in at or "lanechange" in at.lower(): + if len(alerts) == 0 or alerts[-1][1] != at: + alerts.append((t, at)) + elif msg.which() == 'pandaStates': + if ignition_off is None: + ign = any(ps.ignitionLine or ps.ignitionCan for ps in msg.pandaStates) + if not ign: + ignition_off = msg.logMonoTime + break + elif msg.which() in cams: + cnt_cameras[msg.which()] += 1 + + duration = (end_time - start_time) / 1e9 + + print("Events") + pprint(cnt_events) + + print("\n") + print("Events") + for t, evt in events: + print(f"{t:8.2f} {evt}") + + print("\n") + print("Cameras") + for k, v in cnt_cameras.items(): + s = SERVICE_LIST[k] + expected_frames = int(s.frequency * duration / cast(float, s.decimation)) + print(" ", k.ljust(20), f"{v}, {v/expected_frames:.1%} of expected") + + print("\n") + print("Alerts") + for t, a in alerts: + print(f"{t:8.2f} {a}") + + print("\n") + if ignition_off is not None: + ignition_off = round((ignition_off - start_time) / 1e9, 2) + print("Ignition off at", ignition_off) + print("Route duration", datetime.timedelta(seconds=duration)) diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py new file mode 100755 index 0000000000..234dfea3cc --- /dev/null +++ b/selfdrive/debug/cpu_usage_stat.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +# type: ignore +''' +System tools like top/htop can only show current cpu usage values, so I write this script to do statistics jobs. + Features: + Use psutil library to sample cpu usage(avergage for all cores) of openpilot processes, at a rate of 5 samples/sec. + Do cpu usage statistics periodically, 5 seconds as a cycle. + Calculate the average cpu usage within this cycle. + Calculate minumium/maximum/accumulated_average cpu usage as long term inspections. + Monitor multiple processes simuteneously. + Sample usage: + root@localhost:/data/openpilot$ python selfdrive/debug/cpu_usage_stat.py pandad,ubloxd + ('Add monitored proc:', './pandad') + ('Add monitored proc:', 'python locationd/ubloxd.py') + pandad: 1.96%, min: 1.96%, max: 1.96%, acc: 1.96% + ubloxd.py: 0.39%, min: 0.39%, max: 0.39%, acc: 0.39% +''' +import psutil +import time +import os +import sys +import numpy as np +import argparse +import re +from collections import defaultdict + +from openpilot.system.manager.process_config import managed_processes + +# Do statistics every 5 seconds +PRINT_INTERVAL = 5 +SLEEP_INTERVAL = 0.2 + +monitored_proc_names = [ + # android procs + 'SurfaceFlinger', 'sensors.qcom' +] + list(managed_processes.keys()) + +cpu_time_names = ['user', 'system', 'children_user', 'children_system'] + +timer = getattr(time, 'monotonic', time.time) + + +def get_arg_parser(): + parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("proc_names", nargs="?", default='', + help="Process names to be monitored, comma separated") + parser.add_argument("--list_all", action='store_true', + help="Show all running processes' cmdline") + parser.add_argument("--detailed_times", action='store_true', + help="show cpu time details (split by user, system, child user, child system)") + return parser + + +if __name__ == "__main__": + args = get_arg_parser().parse_args(sys.argv[1:]) + if args.list_all: + for p in psutil.process_iter(): + print('cmdline', p.cmdline(), 'name', p.name()) + sys.exit(0) + + if len(args.proc_names) > 0: + monitored_proc_names = args.proc_names.split(',') + monitored_procs = [] + stats = {} + for p in psutil.process_iter(): + if p == psutil.Process(): + continue + matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(fr'.*{pn}.*', l, re.M | re.I))) + if matched: + k = ' '.join(p.cmdline()) + print('Add monitored proc:', k) + stats[k] = {'cpu_samples': defaultdict(list), 'min': defaultdict(lambda: None), 'max': defaultdict(lambda: None), + 'avg': defaultdict(float), 'last_cpu_times': None, 'last_sys_time': None} + stats[k]['last_sys_time'] = timer() + stats[k]['last_cpu_times'] = p.cpu_times() + monitored_procs.append(p) + i = 0 + interval_int = int(PRINT_INTERVAL / SLEEP_INTERVAL) + while True: + for p in monitored_procs: + k = ' '.join(p.cmdline()) + cur_sys_time = timer() + cur_cpu_times = p.cpu_times() + cpu_times = np.subtract(cur_cpu_times, stats[k]['last_cpu_times']) / (cur_sys_time - stats[k]['last_sys_time']) + stats[k]['last_sys_time'] = cur_sys_time + stats[k]['last_cpu_times'] = cur_cpu_times + cpu_percent = 0 + for num, name in enumerate(cpu_time_names): + stats[k]['cpu_samples'][name].append(cpu_times[num]) + cpu_percent += cpu_times[num] + stats[k]['cpu_samples']['total'].append(cpu_percent) + time.sleep(SLEEP_INTERVAL) + i += 1 + if i % interval_int == 0: + l = [] + for k, stat in stats.items(): + if len(stat['cpu_samples']) <= 0: + continue + for name, samples in stat['cpu_samples'].items(): + samples = np.array(samples) + avg = samples.mean() + c = samples.size + min_cpu = np.amin(samples) + max_cpu = np.amax(samples) + if stat['min'][name] is None or min_cpu < stat['min'][name]: + stat['min'][name] = min_cpu + if stat['max'][name] is None or max_cpu > stat['max'][name]: + stat['max'][name] = max_cpu + stat['avg'][name] = (stat['avg'][name] * (i - c) + avg * c) / (i) + stat['cpu_samples'][name] = [] + + msg = f"avg: {stat['avg']['total']:.2%}, min: {stat['min']['total']:.2%}, max: {stat['max']['total']:.2%} {os.path.basename(k)}" + if args.detailed_times: + for stat_type in ['avg', 'min', 'max']: + msg += f"\n {stat_type}: {[(name + ':' + str(round(stat[stat_type][name] * 100, 2))) for name in cpu_time_names]}" + l.append((os.path.basename(k), stat['avg']['total'], msg)) + l.sort(key=lambda x: -x[1]) + for x in l: + print(x[2]) + print('avg sum: {:.2%} over {} samples {} seconds\n'.format( + sum(stat['avg']['total'] for k, stat in stats.items()), i, i * SLEEP_INTERVAL + )) diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py new file mode 100755 index 0000000000..93b0430c1e --- /dev/null +++ b/selfdrive/debug/cycle_alerts.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +import time +import random + +from cereal import car, log +import cereal.messaging as messaging +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.honda.interface import CarInterface +from openpilot.selfdrive.controls.lib.events import ET, Events +from openpilot.selfdrive.controls.lib.alertmanager import AlertManager +from openpilot.system.manager.process_config import managed_processes + +EventName = car.CarEvent.EventName + +def randperc() -> float: + return 100. * random.random() + +def cycle_alerts(duration=200, is_metric=False): + # all alerts + #alerts = list(EVENTS.keys()) + + # this plays each type of audible alert + alerts = [ + (EventName.buttonEnable, ET.ENABLE), + (EventName.buttonCancel, ET.USER_DISABLE), + (EventName.wrongGear, ET.NO_ENTRY), + + (EventName.locationdTemporaryError, ET.SOFT_DISABLE), + (EventName.paramsdTemporaryError, ET.SOFT_DISABLE), + (EventName.accFaulted, ET.IMMEDIATE_DISABLE), + + # DM sequence + (EventName.preDriverDistracted, ET.WARNING), + (EventName.promptDriverDistracted, ET.WARNING), + (EventName.driverDistracted, ET.WARNING), + ] + + # debug alerts + alerts = [ + #(EventName.highCpuUsage, ET.NO_ENTRY), + #(EventName.lowMemory, ET.PERMANENT), + #(EventName.overheat, ET.PERMANENT), + #(EventName.outOfSpace, ET.PERMANENT), + #(EventName.modeldLagging, ET.PERMANENT), + #(EventName.processNotRunning, ET.NO_ENTRY), + #(EventName.commIssue, ET.NO_ENTRY), + #(EventName.calibrationInvalid, ET.PERMANENT), + (EventName.cameraMalfunction, ET.PERMANENT), + (EventName.cameraFrameRate, ET.PERMANENT), + ] + + cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState'] + + CS = car.CarState.new_message() + CP = CarInterface.get_non_essential_params("HONDA_CIVIC") + sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration', + 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', + 'managerState'] + cameras) + + pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState']) + + events = Events() + AM = AlertManager() + + frame = 0 + while True: + for alert, et in alerts: + events.clear() + events.add(alert) + + sm['deviceState'].freeSpacePercent = randperc() + sm['deviceState'].memoryUsagePercent = int(randperc()) + sm['deviceState'].cpuTempC = [randperc() for _ in range(3)] + sm['deviceState'].gpuTempC = [randperc() for _ in range(3)] + sm['deviceState'].cpuUsagePercent = [int(randperc()) for _ in range(8)] + sm['modelV2'].frameDropPerc = randperc() + + if random.random() > 0.25: + sm['modelV2'].velocity.x = [random.random(), ] + if random.random() > 0.25: + CS.vEgo = random.random() + + procs = [p.get_process_state_msg() for p in managed_processes.values()] + random.shuffle(procs) + for i in range(random.randint(0, 10)): + procs[i].shouldBeRunning = True + sm['managerState'].processes = procs + + sm['liveCalibration'].rpyCalib = [-1 * random.random() for _ in range(random.randint(0, 3))] + + for s in sm.data.keys(): + prob = 0.3 if s in cameras else 0.08 + sm.alive[s] = random.random() > prob + sm.valid[s] = random.random() > prob + sm.freq_ok[s] = random.random() > prob + + a = events.create_alerts([et, ], [CP, CS, sm, is_metric, 0]) + AM.add_many(frame, a) + alert = AM.process_alerts(frame, []) + print(alert) + for _ in range(duration): + dat = messaging.new_message() + dat.init('controlsState') + dat.controlsState.enabled = False + + if alert: + dat.controlsState.alertText1 = alert.alert_text_1 + dat.controlsState.alertText2 = alert.alert_text_2 + dat.controlsState.alertSize = alert.alert_size + dat.controlsState.alertStatus = alert.alert_status + dat.controlsState.alertBlinkingRate = alert.alert_rate + dat.controlsState.alertType = alert.alert_type + dat.controlsState.alertSound = alert.audible_alert + pm.send('controlsState', dat) + + dat = messaging.new_message() + dat.init('deviceState') + dat.deviceState.started = True + pm.send('deviceState', dat) + + dat = messaging.new_message('pandaStates', 1) + dat.pandaStates[0].ignitionLine = True + dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno + pm.send('pandaStates', dat) + + frame += 1 + time.sleep(DT_CTRL) + +if __name__ == '__main__': + cycle_alerts() diff --git a/selfdrive/debug/debug_fw_fingerprinting_offline.py b/selfdrive/debug/debug_fw_fingerprinting_offline.py new file mode 100755 index 0000000000..3df2924738 --- /dev/null +++ b/selfdrive/debug/debug_fw_fingerprinting_offline.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +import argparse + +from openpilot.tools.lib.live_logreader import live_logreader +from openpilot.tools.lib.logreader import LogReader, ReadMode +from panda.python import uds + + +def main(route: str | None, addrs: list[int]): + """ + TODO: + - highlight TX vs RX clearly + - disambiguate sendcan and can (useful to know if something sent on sendcan made it to the bus on can->128) + - print as fixed width table, easier to read + """ + + if route is None: + lr = live_logreader() + else: + lr = LogReader(route, default_mode=ReadMode.RLOG, sort_by_time=True) + + start_mono_time = None + prev_mono_time = 0 + + # include rx addresses + addrs = addrs + [uds.get_rx_addr_for_tx_addr(addr) for addr in addrs] + + for msg in lr: + if msg.which() == 'can': + if start_mono_time is None: + start_mono_time = msg.logMonoTime + + if msg.which() in ("can", 'sendcan'): + for can in getattr(msg, msg.which()): + if can.address in addrs or not len(addrs): + if msg.logMonoTime != prev_mono_time: + print() + prev_mono_time = msg.logMonoTime + print(f"{msg.which():>7}: rxaddr={can.address}, bus={can.src}, {round((msg.logMonoTime - start_mono_time) * 1e-6, 2)} ms, " + + f"0x{can.dat.hex()}, {can.dat}, {len(can.dat)=}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='View back and forth ISO-TP communication between various ECUs given an address') + parser.add_argument('route', nargs='?', help='Route name, live if not specified') + parser.add_argument('--addrs', nargs='*', default=[], help='List of tx address to view (0x7e0 for engine)') + args = parser.parse_args() + + addrs = [int(addr, base=16) if addr.startswith('0x') else int(addr) for addr in args.addrs] + main(args.route, addrs) diff --git a/selfdrive/debug/dump_car_docs.py b/selfdrive/debug/dump_car_docs.py new file mode 100755 index 0000000000..f09c602cff --- /dev/null +++ b/selfdrive/debug/dump_car_docs.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python3 +import argparse +import pickle + +from openpilot.selfdrive.car.docs import get_all_car_docs + + +def dump_car_docs(path): + with open(path, 'wb') as f: + pickle.dump(get_all_car_docs(), f) + print(f'Dumping car info to {path}') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--path", required=True) + args = parser.parse_args() + dump_car_docs(args.path) diff --git a/selfdrive/debug/fingerprint_from_route.py b/selfdrive/debug/fingerprint_from_route.py new file mode 100755 index 0000000000..68308bb627 --- /dev/null +++ b/selfdrive/debug/fingerprint_from_route.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +import sys +from openpilot.tools.lib.logreader import LogReader, ReadMode + + +def get_fingerprint(lr): + # TODO: make this a nice tool for car ports. should also work with qlogs for FW + + fw = None + msgs = {} + for msg in lr: + if msg.which() == 'carParams': + fw = msg.carParams.carFw + elif msg.which() == 'can': + for c in msg.can: + # read also msgs sent by EON on CAN bus 0x80 and filter out the + # addr with more than 11 bits + if c.src % 0x80 == 0 and c.address < 0x800 and c.address not in (0x7df, 0x7e0, 0x7e8): + msgs[c.address] = len(c.dat) + + # show CAN fingerprint + fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items())) + print(f"\nfound {len(msgs)} messages. CAN fingerprint:\n") + print(fingerprint) + + # TODO: also print the fw fingerprint merged with the existing ones + # show FW fingerprint + print("\nFW fingerprint:\n") + for f in fw: + print(f" (Ecu.{f.ecu}, {hex(f.address)}, {None if f.subAddress == 0 else f.subAddress}): [") + print(f" {f.fwVersion},") + print(" ],") + print() + + +if __name__ == "__main__": + if len(sys.argv) < 2: + print("Usage: ./fingerprint_from_route.py ") + sys.exit(1) + + lr = LogReader(sys.argv[1], ReadMode.QLOG) + get_fingerprint(lr) diff --git a/selfdrive/debug/format_fingerprints.py b/selfdrive/debug/format_fingerprints.py index bd5822729a..2a5e4e6080 100755 --- a/selfdrive/debug/format_fingerprints.py +++ b/selfdrive/debug/format_fingerprints.py @@ -67,7 +67,7 @@ def format_brand_fw_versions(brand, extra_fw_versions: None | dict[str, dict[tup extra_fw_versions = extra_fw_versions or {} fingerprints_file = os.path.join(BASEDIR, f"selfdrive/car/{brand}/fingerprints.py") - with open(fingerprints_file, "r") as f: + with open(fingerprints_file) as f: comments = [line for line in f.readlines() if line.startswith("#") and "noqa" not in line] with open(fingerprints_file, "w") as f: diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index f7f7a1604f..14f6c0e33c 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -4,7 +4,7 @@ # Instructions: # - connect to a Panda -# - run selfdrive/boardd/boardd +# - run selfdrive/pandad/pandad # - launching this script # Note: it's very important that the car is in stock mode, in order to collect a complete fingerprint # - since some messages are published at low frequency, keep this script running for at least 30s, diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py index 7735391f4f..5e7080ef64 100755 --- a/selfdrive/debug/hyundai_enable_radar_points.py +++ b/selfdrive/debug/hyundai_enable_radar_points.py @@ -36,6 +36,9 @@ SUPPORTED_FW_VERSIONS = { default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), # 2021 SONATA HYBRID + b"DNhe SCC FHCUP 1.00 1.00 99110-L5000\x19\x04&\x13' ": ConfigValues( + default_config=b"\x00\x00\x00\x01\x00\x00", + tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), b"DNhe SCC FHCUP 1.00 1.02 99110-L5000 \x01#\x15# ": ConfigValues( default_config=b"\x00\x00\x00\x01\x00\x00", tracks_enabled=b"\x00\x00\x00\x01\x00\x01"), @@ -76,11 +79,11 @@ if __name__ == "__main__": args = parser.parse_args() try: - check_output(["pidof", "boardd"]) - print("boardd is running, please kill openpilot before running this script! (aborted)") + check_output(["pidof", "pandad"]) + print("pandad is running, please kill openpilot before running this script! (aborted)") sys.exit(1) except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (boardd not running) + if e.returncode != 1: # 1 == no process found (pandad not running) raise e confirm = input("power on the vehicle keeping the engine off (press start button twice) then type OK to continue: ").upper().strip() diff --git a/selfdrive/debug/internal/__init__.py b/selfdrive/debug/internal/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/debug/internal/fuzz_fw_fingerprint.py b/selfdrive/debug/internal/fuzz_fw_fingerprint.py new file mode 100755 index 0000000000..aedb3ada1d --- /dev/null +++ b/selfdrive/debug/internal/fuzz_fw_fingerprint.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# type: ignore +import random +from collections import defaultdict + +from tqdm import tqdm + +from openpilot.selfdrive.car.fw_versions import match_fw_to_car_fuzzy +from openpilot.selfdrive.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS +from openpilot.selfdrive.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS +from openpilot.selfdrive.car.hyundai.values import FW_VERSIONS as HYUNDAI_FW_VERSIONS +from openpilot.selfdrive.car.volkswagen.values import FW_VERSIONS as VW_FW_VERSIONS + + +FWS = {} +FWS.update(TOYOTA_FW_VERSIONS) +FWS.update(HONDA_FW_VERSIONS) +FWS.update(HYUNDAI_FW_VERSIONS) +FWS.update(VW_FW_VERSIONS) + +if __name__ == "__main__": + total = 0 + match = 0 + wrong_match = 0 + confusions = defaultdict(set) + + for _ in tqdm(range(1000)): + for candidate, fws in FWS.items(): + fw_dict = {} + 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) + + total += 1 + if len(matches) == 1: + if list(matches)[0] == candidate: + match += 1 + else: + confusions[candidate] |= matches + wrong_match += 1 + + print() + for candidate, wrong_matches in sorted(confusions.items()): + print(candidate, wrong_matches) + + print() + print(f"Total fuzz cases: {total}") + print(f"Correct matches: {match}") + print(f"Wrong matches: {wrong_match}") + + diff --git a/selfdrive/debug/internal/measure_modeld_packet_drop.py b/selfdrive/debug/internal/measure_modeld_packet_drop.py new file mode 100755 index 0000000000..9814942ce2 --- /dev/null +++ b/selfdrive/debug/internal/measure_modeld_packet_drop.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +import cereal.messaging as messaging + +if __name__ == "__main__": + modeld_sock = messaging.sub_sock("modelV2") + + last_frame_id = None + start_t: int | None = None + frame_cnt = 0 + dropped = 0 + + while True: + m = messaging.recv_one(modeld_sock) + if m is None: + continue + + frame_id = m.modelV2.frameId + t = m.logMonoTime / 1e9 + frame_cnt += 1 + + if start_t is None: + start_t = t + last_frame_id = frame_id + continue + + d_frame = frame_id - last_frame_id + dropped += d_frame - 1 + + expected_num_frames = int((t - start_t) * 20) + frame_drop = 100 * (1 - (expected_num_frames / frame_cnt)) + print(f"Num dropped {dropped}, Drop compared to 20Hz: {frame_drop:.2f}%") + + last_frame_id = frame_id diff --git a/selfdrive/debug/internal/measure_torque_time_to_max.py b/selfdrive/debug/internal/measure_torque_time_to_max.py new file mode 100755 index 0000000000..ef3152b50c --- /dev/null +++ b/selfdrive/debug/internal/measure_torque_time_to_max.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python3 +# type: ignore + +import os +import argparse +import struct +from collections import deque +from statistics import mean + +from cereal import log +import cereal.messaging as messaging + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Sniff a communication socket') + parser.add_argument('--addr', default='127.0.0.1') + args = parser.parse_args() + + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + poller = messaging.Poller() + messaging.sub_sock('can', poller, addr=args.addr) + + active = 0 + start_t = 0 + start_v = 0 + max_v = 0 + max_t = 0 + window = deque(maxlen=10) + avg = 0 + while 1: + polld = poller.poll(1000) + for sock in polld: + msg = sock.receive() + with log.Event.from_bytes(msg) as log_evt: + evt = log_evt + + for item in evt.can: + if item.address == 0xe4 and item.src == 128: + torque_req = struct.unpack('!h', item.dat[0:2])[0] + # print(torque_req) + active = abs(torque_req) > 0 + if abs(torque_req) < 100: + if max_v > 5: + print(f'{start_v} -> {max_v} = {round(max_v - start_v, 2)} over {round(max_t - start_t, 2)}s') + start_t = evt.logMonoTime / 1e9 + start_v = avg + max_t = 0 + max_v = 0 + if item.address == 0x1ab and item.src == 0: + motor_torque = ((item.dat[0] & 0x3) << 8) + item.dat[1] + window.append(motor_torque) + avg = mean(window) + #print(f'{evt.logMonoTime}: {avg}') + if active and avg > max_v + 0.5: + max_v = avg + max_t = evt.logMonoTime / 1e9 diff --git a/selfdrive/debug/internal/qlog_size.py b/selfdrive/debug/internal/qlog_size.py new file mode 100755 index 0000000000..b51cb3af2f --- /dev/null +++ b/selfdrive/debug/internal/qlog_size.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import argparse +import bz2 +from collections import defaultdict + +import matplotlib.pyplot as plt + +from cereal.services import SERVICE_LIST +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.route import Route + +MIN_SIZE = 0.5 # Percent size of total to show as separate entry + + +def make_pie(msgs, typ): + compressed_length_by_type = {k: len(bz2.compress(b"".join(v))) for k, v in msgs.items()} + + total = sum(compressed_length_by_type.values()) + + sizes = sorted(compressed_length_by_type.items(), key=lambda kv: kv[1]) + + print(f"{typ} - Total {total / 1024:.2f} kB") + for (name, sz) in sizes: + print(f"{name} - {sz / 1024:.2f} kB") + print() + + 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, strict=True) + + plt.figure() + plt.title(f"{typ}") + plt.pie(sizes, labels=labels, autopct='%1.1f%%') + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Check qlog size based on a rlog') + parser.add_argument('route', help='route to use') + parser.add_argument('segment', type=int, help='segment number to use') + args = parser.parse_args() + + r = Route(args.route) + rlog = r.log_paths()[args.segment] + msgs = list(LogReader(rlog)) + + msgs_by_type = defaultdict(list) + for m in msgs: + msgs_by_type[m.which()].append(m.as_builder().to_bytes()) + + qlog_by_type = defaultdict(list) + for name, service in SERVICE_LIST.items(): + if service.decimation is None: + continue + + for i, msg in enumerate(msgs_by_type[name]): + if i % service.decimation == 0: + qlog_by_type[name].append(msg) + + make_pie(msgs_by_type, 'rlog') + make_pie(qlog_by_type, 'qlog') + plt.show() diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py new file mode 100755 index 0000000000..8549b92665 --- /dev/null +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +import argparse +import capnp +from collections import defaultdict + +from cereal.messaging import SubMaster +from openpilot.common.numpy_fast import mean + +def cputime_total(ct): + return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq + + +def cputime_busy(ct): + return ct.user + ct.nice + ct.system + ct.irq + ct.softirq + + +def proc_cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def proc_name(proc): + name = proc.name + if len(proc.cmdline): + name = proc.cmdline[0] + if len(proc.exe): + name = proc.exe + " - " + name + + return name + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument('--mem', action='store_true') + parser.add_argument('--cpu', action='store_true') + args = parser.parse_args() + + sm = SubMaster(['deviceState', 'procLog']) + + last_temp = 0.0 + last_mem = 0.0 + total_times = [0.]*8 + busy_times = [0.]*8 + + prev_proclog: capnp._DynamicStructReader | None = None + prev_proclog_t: int | None = None + + while True: + sm.update() + + if sm.updated['deviceState']: + t = sm['deviceState'] + last_temp = mean(t.cpuTempC) + last_mem = t.memoryUsagePercent + + if sm.updated['procLog']: + m = sm['procLog'] + + cores = [0.]*8 + total_times_new = [0.]*8 + busy_times_new = [0.]*8 + + for c in m.cpuTimes: + n = c.cpuNum + total_times_new[n] = cputime_total(c) + busy_times_new[n] = cputime_busy(c) + + for n in range(8): + t_busy = busy_times_new[n] - busy_times[n] + t_total = total_times_new[n] - total_times[n] + cores[n] = t_busy / t_total + + total_times = total_times_new[:] + busy_times = busy_times_new[:] + + print(f"CPU {100.0 * mean(cores):.2f}% - RAM: {last_mem:.2f}% - Temp {last_temp:.2f}C") + + if args.cpu and prev_proclog is not None and prev_proclog_t is not None: + procs: dict[str, float] = defaultdict(float) + dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9 + for proc in m.procs: + try: + name = proc_name(proc) + prev_proc = [p for p in prev_proclog.procs if proc.pid == p.pid][0] + cpu_time = proc_cputime_total(proc) - proc_cputime_total(prev_proc) + cpu_usage = cpu_time / dt * 100. + procs[name] += cpu_usage + except IndexError: + pass + + print("Top CPU usage:") + for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} %") + print() + + if args.mem: + mems = {} + for proc in m.procs: + name = proc_name(proc) + mems[name] = float(proc.memRss) / 1e6 + print("Top memory usage:") + for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} MB") + print() + + prev_proclog = m + prev_proclog_t = sm.logMonoTime['procLog'] diff --git a/selfdrive/debug/print_docs_diff.py b/selfdrive/debug/print_docs_diff.py new file mode 100755 index 0000000000..7ef89a6eca --- /dev/null +++ b/selfdrive/debug/print_docs_diff.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python3 +import argparse +from collections import defaultdict +import difflib +import pickle + +from openpilot.selfdrive.car.docs import get_all_car_docs +from openpilot.selfdrive.car.docs_definitions import Column + +FOOTNOTE_TAG = "{}" +STAR_ICON = '' +VIDEO_ICON = '' + \ + '' +COLUMNS = "|" + "|".join([column.value for column in Column]) + "|" +COLUMN_HEADER = "|---|---|---|{}|".format("|".join([":---:"] * (len(Column) - 3))) +ARROW_SYMBOL = "➡️" + + +def load_base_car_docs(path): + with open(path, "rb") as f: + return pickle.load(f) + + +def match_cars(base_cars, new_cars): + changes = [] + additions = [] + for new in new_cars: + # Addition if no close matches or close match already used + # Change if close match and not already used + matches = difflib.get_close_matches(new.name, [b.name for b in base_cars], cutoff=0.) + if not len(matches) or matches[0] in [c[1].name for c in changes]: + additions.append(new) + else: + changes.append((new, next(car for car in base_cars if car.name == matches[0]))) + + # Removal if base car not in changes + removals = [b for b in base_cars if b.name not in [c[1].name for c in changes]] + return changes, additions, removals + + +def build_column_diff(base_car, new_car): + row_builder = [] + for column in Column: + base_column = base_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + new_column = new_car.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) + + if base_column != new_column: + row_builder.append(f"{base_column} {ARROW_SYMBOL} {new_column}") + else: + row_builder.append(new_column) + + return format_row(row_builder) + + +def format_row(builder): + return "|" + "|".join(builder) + "|" + + +def print_car_docs_diff(path): + base_car_docs = defaultdict(list) + new_car_docs = defaultdict(list) + + for car in load_base_car_docs(path): + base_car_docs[car.car_fingerprint].append(car) + for car in get_all_car_docs(): + new_car_docs[car.car_fingerprint].append(car) + + # Add new platforms to base cars so we can detect additions and removals in one pass + base_car_docs.update({car: [] for car in new_car_docs if car not in base_car_docs}) + + changes = defaultdict(list) + for base_car_model, base_cars in base_car_docs.items(): + # Match car info changes, and get additions and removals + new_cars = new_car_docs[base_car_model] + car_changes, car_additions, car_removals = match_cars(base_cars, new_cars) + + # Removals + for car_docs in car_removals: + changes["removals"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + + # Additions + for car_docs in car_additions: + changes["additions"].append(format_row([car_docs.get_column(column, STAR_ICON, VIDEO_ICON, FOOTNOTE_TAG) for column in Column])) + + for new_car, base_car in car_changes: + # Column changes + row_diff = build_column_diff(base_car, new_car) + if ARROW_SYMBOL in row_diff: + changes["column"].append(row_diff) + + # Detail sentence changes + if base_car.detail_sentence != new_car.detail_sentence: + changes["detail"].append(f"- Sentence for {base_car.name} changed!\n" + + " ```diff\n" + + f" - {base_car.detail_sentence}\n" + + f" + {new_car.detail_sentence}\n" + + " ```") + + # Print diff + if any(len(c) for c in changes.values()): + markdown_builder = ["### ⚠️ This PR makes changes to [CARS.md](../blob/master/docs/CARS.md) ⚠️"] + + for title, category in (("## 🔀 Column Changes", "column"), ("## ❌ Removed", "removals"), + ("## ➕ Added", "additions"), ("## 📖 Detail Sentence Changes", "detail")): + if len(changes[category]): + markdown_builder.append(title) + if category not in ("detail",): + markdown_builder.append(COLUMNS) + markdown_builder.append(COLUMN_HEADER) + markdown_builder.extend(changes[category]) + + print("\n".join(markdown_builder)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--path", required=True) + args = parser.parse_args() + print_car_docs_diff(args.path) diff --git a/selfdrive/debug/print_flags.py b/selfdrive/debug/print_flags.py new file mode 100755 index 0000000000..c69765550e --- /dev/null +++ b/selfdrive/debug/print_flags.py @@ -0,0 +1,14 @@ +#!/usr/bin/env python3 +from openpilot.selfdrive.car.values import BRANDS + +for brand in BRANDS: + all_flags = set() + for platform in brand: + if platform.config.flags != 0: + all_flags |= set(platform.config.flags) + + if len(all_flags): + print(brand.__module__.split('.')[-2].upper() + ':') + for flag in sorted(all_flags): + print(f' {flag.name:<24}:', {platform.name for platform in brand.with_flags(flag)}) + print() diff --git a/selfdrive/debug/read_dtc_status.py b/selfdrive/debug/read_dtc_status.py new file mode 100755 index 0000000000..2d27dd2bb2 --- /dev/null +++ b/selfdrive/debug/read_dtc_status.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +import sys +import argparse +from subprocess import check_output, CalledProcessError +from panda import Panda +from panda.python.uds import UdsClient, SESSION_TYPE, DTC_REPORT_TYPE, DTC_STATUS_MASK_TYPE +from panda.python.uds import get_dtc_num_as_str, get_dtc_status_names + +parser = argparse.ArgumentParser(description="read DTC status") +parser.add_argument("addr", type=lambda x: int(x,0)) +parser.add_argument("--bus", type=int, default=0) +parser.add_argument('--debug', action='store_true') +args = parser.parse_args() + +try: + check_output(["pidof", "pandad"]) + print("pandad is running, please kill openpilot before running this script! (aborted)") + sys.exit(1) +except CalledProcessError as e: + if e.returncode != 1: # 1 == no process found (pandad not running) + raise e + +panda = Panda() +panda.set_safety_mode(Panda.SAFETY_ELM327) +uds_client = UdsClient(panda, args.addr, bus=args.bus, debug=args.debug) +print("extended diagnostic session ...") +uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) +print("read diagnostic codes ...") +data = uds_client.read_dtc_information(DTC_REPORT_TYPE.DTC_BY_STATUS_MASK, DTC_STATUS_MASK_TYPE.ALL) +print("status availability:", " ".join(get_dtc_status_names(data[0]))) +print("DTC status:") +for i in range(1, len(data), 4): + dtc_num = get_dtc_num_as_str(data[i:i+3]) + dtc_status = " ".join(get_dtc_status_names(data[i+3])) + print(dtc_num, dtc_status) diff --git a/selfdrive/debug/run_process_on_route.py b/selfdrive/debug/run_process_on_route.py new file mode 100755 index 0000000000..90db14bc9a --- /dev/null +++ b/selfdrive/debug/run_process_on_route.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 + +import argparse + +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, replay_process +from openpilot.tools.lib.helpers import save_log +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Run process on route and create new logs", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + parser.add_argument("--fingerprint", help="The fingerprint to use") + parser.add_argument("route", help="The route name to use") + parser.add_argument("process", help="The process to run") + args = parser.parse_args() + + cfg = [c for c in CONFIGS if c.proc_name == args.process][0] + + lr = LogReader(args.route) + inputs = list(lr) + + outputs = replay_process(cfg, inputs, fingerprint=args.fingerprint) + + # Remove message generated by the process under test and merge in the new messages + produces = {o.which() for o in outputs} + inputs = [i for i in inputs if i.which() not in produces] + outputs = sorted(inputs + outputs, key=lambda x: x.logMonoTime) + + fn = f"{args.route.replace('/', '_')}_{args.process}.bz2" + print(f"Saved log to {fn}") + save_log(fn, outputs) diff --git a/selfdrive/debug/set_car_params.py b/selfdrive/debug/set_car_params.py new file mode 100755 index 0000000000..6060dfbc36 --- /dev/null +++ b/selfdrive/debug/set_car_params.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +import sys + +from cereal import car +from openpilot.common.params import Params +from openpilot.tools.lib.route import Route +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + CP = None + if len(sys.argv) > 1: + r = Route(sys.argv[1]) + cps = [m for m in LogReader(r.qlog_paths()[0]) if m.which() == 'carParams'] + CP = cps[0].carParams.as_builder() + else: + CP = car.CarParams.new_message() + CP.openpilotLongitudinalControl = True + CP.experimentalLongitudinalAvailable = False + + cp_bytes = CP.to_bytes() + for p in ("CarParams", "CarParamsCache", "CarParamsPersistent"): + Params().put(p, cp_bytes) diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py new file mode 100755 index 0000000000..19144ead7e --- /dev/null +++ b/selfdrive/debug/show_matching_cars.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars +import cereal.messaging as messaging + + +# rav4 2019 and corolla tss2 +fingerprint = {896: 8, 898: 8, 900: 6, 976: 1, 1541: 8, 902: 6, 905: 8, 810: 2, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1552: 8, 1553: 8, 1556: 8, 1571: 8, 921: 8, 1056: 8, 544: 4, 1570: 8, 1059: 1, 36: 8, 37: 8, 550: 8, 935: 8, 552: 4, 170: 8, 812: 8, 944: 8, 945: 8, 562: 6, 180: 8, 1077: 8, 951: 8, 1592: 8, 1076: 8, 186: 4, 955: 8, 956: 8, 1001: 8, 705: 8, 452: 8, 1788: 8, 464: 8, 824: 8, 466: 8, 467: 8, 761: 8, 728: 8, 1572: 8, 1114: 8, 933: 8, 800: 8, 608: 8, 865: 8, 610: 8, 1595: 8, 934: 8, 998: 5, 1745: 8, 1000: 8, 764: 8, 1002: 8, 999: 7, 1789: 8, 1649: 8, 1779: 8, 1568: 8, 1017: 8, 1786: 8, 1787: 8, 1020: 8, 426: 6, 1279: 8} # noqa: E501 + +candidate_cars = all_legacy_fingerprint_cars() + + +for addr, l in fingerprint.items(): + dat = messaging.new_message('can', 1) + + msg = dat.can[0] + msg.address = addr + msg.dat = " " * l + + candidate_cars = eliminate_incompatible_cars(msg, candidate_cars) + print(candidate_cars) diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py new file mode 100755 index 0000000000..e338110a7d --- /dev/null +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +# type: ignore + +from collections import defaultdict +import argparse +import os +import traceback +from tqdm import tqdm +from openpilot.tools.lib.logreader import LogReader, ReadMode +from openpilot.tools.lib.route import SegmentRange +from openpilot.selfdrive.car.car_helpers import interface_names +from openpilot.selfdrive.car.fingerprints import MIGRATION +from openpilot.selfdrive.car.fw_versions import VERSIONS, match_fw_to_car + + +NO_API = "NO_API" in os.environ +SUPPORTED_BRANDS = VERSIONS.keys() +SUPPORTED_CARS = [brand for brand in SUPPORTED_BRANDS for brand in interface_names[brand]] +UNKNOWN_BRAND = "unknown" + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Run FW fingerprint on Qlog of route or list of routes') + parser.add_argument('route', help='Route or file with list of routes') + parser.add_argument('--car', help='Force comparison fingerprint to known car') + args = parser.parse_args() + + if os.path.exists(args.route): + routes = list(open(args.route)) + else: + routes = [args.route] + + mismatches = defaultdict(list) + + not_fingerprinted = 0 + solved_by_fuzzy = 0 + + good_exact = 0 + wrong_fuzzy = 0 + good_fuzzy = 0 + + dongles = [] + for route in tqdm(routes): + sr = SegmentRange(route) + dongle_id = sr.dongle_id + + if dongle_id in dongles: + continue + + if sr.slice == '' and sr.selector is None: + route += '/0' + + lr = LogReader(route, default_mode=ReadMode.QLOG) + + try: + dongles.append(dongle_id) + + CP = None + for msg in lr: + if msg.which() == "pandaStates": + if msg.pandaStates[0].pandaType in ('unknown', 'whitePanda', 'greyPanda', 'pedal'): + print("wrong panda type") + break + + elif msg.which() == "carParams": + CP = msg.carParams + car_fw = [fw for fw in CP.carFw if not fw.logging] + if len(car_fw) == 0: + print("no fw") + break + + live_fingerprint = CP.carFingerprint + live_fingerprint = MIGRATION.get(live_fingerprint, live_fingerprint) + + if args.car is not None: + live_fingerprint = args.car + + if live_fingerprint not in SUPPORTED_CARS: + print("not in supported cars") + break + + _, exact_matches = match_fw_to_car(car_fw, CP.carVin, allow_exact=True, allow_fuzzy=False) + _, fuzzy_matches = match_fw_to_car(car_fw, CP.carVin, allow_exact=False, allow_fuzzy=True) + + if (len(exact_matches) == 1) and (list(exact_matches)[0] == live_fingerprint): + good_exact += 1 + print(f"Correct! Live: {live_fingerprint} - Fuzzy: {fuzzy_matches}") + + # Check if fuzzy match was correct + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] != live_fingerprint: + wrong_fuzzy += 1 + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + else: + good_fuzzy += 1 + break + + print("Old style:", live_fingerprint, "Vin", CP.carVin) + print("New style (exact):", exact_matches) + print("New style (fuzzy):", fuzzy_matches) + + padding = max([len(fw.brand or UNKNOWN_BRAND) for fw in car_fw]) + for version in sorted(car_fw, key=lambda fw: fw.brand): + subaddr = None if version.subAddress == 0 else hex(version.subAddress) + print(f" Brand: {version.brand or UNKNOWN_BRAND:{padding}}, bus: {version.bus} - " + + f"(Ecu.{version.ecu}, {hex(version.address)}, {subaddr}): [{version.fwVersion}],") + + print("Mismatches") + found = False + for brand in SUPPORTED_BRANDS: + car_fws = VERSIONS[brand] + if live_fingerprint in car_fws: + found = True + expected = car_fws[live_fingerprint] + for (_, expected_addr, expected_sub_addr), v in expected.items(): + for version in car_fw: + if version.brand != brand and len(version.brand): + continue + sub_addr = None if version.subAddress == 0 else version.subAddress + addr = version.address + + if (addr, sub_addr) == (expected_addr, expected_sub_addr): + if version.fwVersion not in v: + print(f"({hex(addr)}, {'None' if sub_addr is None else hex(sub_addr)}) - {version.fwVersion}") + + # Add to global list of mismatches + mismatch = (addr, sub_addr, version.fwVersion) + if mismatch not in mismatches[live_fingerprint]: + mismatches[live_fingerprint].append(mismatch) + + # No FW versions for this car yet, add them all to mismatch list + if not found: + for version in car_fw: + sub_addr = None if version.subAddress == 0 else version.subAddress + addr = version.address + mismatch = (addr, sub_addr, version.fwVersion) + if mismatch not in mismatches[live_fingerprint]: + mismatches[live_fingerprint].append(mismatch) + + print() + not_fingerprinted += 1 + + if len(fuzzy_matches) == 1: + if list(fuzzy_matches)[0] == live_fingerprint: + solved_by_fuzzy += 1 + else: + wrong_fuzzy += 1 + print("Fuzzy match wrong! Fuzzy:", fuzzy_matches, "Live:", live_fingerprint) + + break + + if CP is None: + print("no CarParams in logs") + except Exception: + traceback.print_exc() + except KeyboardInterrupt: + break + + print() + # Print FW versions that need to be added separated out by car and address + for car, m in sorted(mismatches.items()): + print(car) + addrs = defaultdict(list) + for (addr, sub_addr, version) in m: + addrs[(addr, sub_addr)].append(version) + + for (addr, sub_addr), versions in addrs.items(): + print(f" ({hex(addr)}, {'None' if sub_addr is None else hex(sub_addr)}): [") + for v in versions: + print(f" {v},") + print(" ]") + print() + + print() + print(f"Number of dongle ids checked: {len(dongles)}") + print(f"Fingerprinted: {good_exact}") + print(f"Not fingerprinted: {not_fingerprinted}") + print(f" of which had a fuzzy match: {solved_by_fuzzy}") + + print() + print(f"Correct fuzzy matches: {good_fuzzy}") + print(f"Wrong fuzzy matches: {wrong_fuzzy}") + print() + diff --git a/selfdrive/debug/toyota_eps_factor.py b/selfdrive/debug/toyota_eps_factor.py new file mode 100755 index 0000000000..dc83c8f7a4 --- /dev/null +++ b/selfdrive/debug/toyota_eps_factor.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import sys +import numpy as np +import matplotlib.pyplot as plt +from sklearn import linear_model +from openpilot.selfdrive.car.toyota.values import STEER_THRESHOLD + +from openpilot.tools.lib.logreader import LogReader + +MIN_SAMPLES = 30 * 100 + + +def to_signed(n, bits): + if n >= (1 << max((bits - 1), 0)): + n = n - (1 << max(bits, 0)) + return n + + +def get_eps_factor(lr, plot=False): + engaged = False + steering_pressed = False + torque_cmd, eps_torque = None, None + cmds, eps = [], [] + + for msg in lr: + if msg.which() != 'can': + continue + + for m in msg.can: + if m.address == 0x2e4 and m.src == 128: + engaged = bool(m.dat[0] & 1) + torque_cmd = to_signed((m.dat[1] << 8) | m.dat[2], 16) + elif m.address == 0x260 and m.src == 0: + eps_torque = to_signed((m.dat[5] << 8) | m.dat[6], 16) + steering_pressed = abs(to_signed((m.dat[1] << 8) | m.dat[2], 16)) > STEER_THRESHOLD + + if engaged and torque_cmd is not None and eps_torque is not None and not steering_pressed: + cmds.append(torque_cmd) + eps.append(eps_torque) + else: + if len(cmds) > MIN_SAMPLES: + break + cmds, eps = [], [] + + if len(cmds) < MIN_SAMPLES: + raise Exception("too few samples found in route") + + lm = linear_model.LinearRegression(fit_intercept=False) + lm.fit(np.array(cmds).reshape(-1, 1), eps) + scale_factor = 1. / lm.coef_[0] + + if plot: + plt.plot(np.array(eps) * scale_factor) + plt.plot(cmds) + plt.show() + return scale_factor + + +if __name__ == "__main__": + lr = LogReader(sys.argv[1]) + n = get_eps_factor(lr, plot="--plot" in sys.argv) + print("EPS torque factor: ", n) diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index f4440a912c..8e75769a85 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -3,13 +3,14 @@ import time from cereal import car, log, messaging from openpilot.common.params import Params -from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.hardware import HARDWARE if __name__ == "__main__": - CP = car.CarParams(notCar=True) + CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10) Params().put("CarParams", CP.to_bytes()) - procs = ['camerad', 'ui', 'modeld', 'calibrationd'] + procs = ['camerad', 'ui', 'modeld', 'calibrationd', 'plannerd', 'dmonitoringmodeld', 'dmonitoringd'] for p in procs: managed_processes[p].start() @@ -17,6 +18,7 @@ if __name__ == "__main__": msgs = {s: messaging.new_message(s) for s in ['controlsState', 'deviceState', 'carParams']} msgs['deviceState'].deviceState.started = True + msgs['deviceState'].deviceState.deviceType = HARDWARE.get_device_type() msgs['carParams'].carParams.openpilotLongitudinalControl = True msgs['pandaStates'] = messaging.new_message('pandaStates', 1) diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py index 75409e3f87..6df1e57ce3 100755 --- a/selfdrive/debug/vw_mqb_config.py +++ b/selfdrive/debug/vw_mqb_config.py @@ -6,6 +6,7 @@ from enum import IntEnum from panda import Panda from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ DATA_IDENTIFIER_TYPE, ACCESS_TYPE +from datetime import date # TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks class VOLKSWAGEN_DATA_IDENTIFIER_TYPE(IntEnum): @@ -136,8 +137,10 @@ if __name__ == "__main__": # last two bytes, but not the VZ/importer or tester serial number # Can't seem to read it back, but we can read the calibration tester, # so fib a little and say that same tester did the programming - # TODO: encode the actual current date - prog_date = b'\x22\x02\x08' + current_date = date.today() + formatted_date = current_date.strftime('%y-%m-%d') + year, month, day = (int(part) for part in formatted_date.split('-')) + prog_date = bytes([year, month, day]) uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.PROGRAMMING_DATE, prog_date) tester_num = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.CALIBRATION_REPAIR_SHOP_CODE_OR_CALIBRATION_EQUIPMENT_SERIAL_NUMBER) uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.REPAIR_SHOP_CODE_OR_TESTER_SERIAL_NUMBER, tester_num) diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 07555a6087..27cd4d5b40 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,6 +1,6 @@ -Import('env', 'arch', 'common', 'cereal', 'messaging', 'rednose', 'transformations') +Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations') -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread', 'dl'] +loc_libs = [messaging, common, 'pthread', 'dl'] # build ekf models rednose_gen_dir = 'models/generated' diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index da1c4ec37b..6e154bf07c 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -10,7 +10,7 @@ import gc import os import capnp import numpy as np -from typing import List, NoReturn, Optional +from typing import NoReturn from cereal import log import cereal.messaging as messaging @@ -89,7 +89,7 @@ class Calibrator: valid_blocks: int = 0, wide_from_device_euler_init: np.ndarray = WIDE_FROM_DEVICE_EULER_INIT, height_init: np.ndarray = HEIGHT_INIT, - smooth_from: Optional[np.ndarray] = None) -> None: + smooth_from: np.ndarray = None) -> None: if not np.isfinite(rpy_init).all(): self.rpy = RPY_INIT.copy() else: @@ -125,7 +125,7 @@ class Calibrator: self.old_rpy = smooth_from self.old_rpy_weight = 1.0 - def get_valid_idxs(self) -> List[int]: + def get_valid_idxs(self) -> list[int]: # exclude current block_idx from validity window before_current = list(range(self.block_idx)) after_current = list(range(min(self.valid_blocks, self.block_idx + 1), self.valid_blocks)) @@ -175,12 +175,12 @@ class Calibrator: else: return self.rpy - def handle_cam_odom(self, trans: List[float], - rot: List[float], - wide_from_device_euler: List[float], - trans_std: List[float], - road_transform_trans: List[float], - road_transform_trans_std: List[float]) -> Optional[np.ndarray]: + def handle_cam_odom(self, trans: list[float], + rot: list[float], + wide_from_device_euler: list[float], + trans_std: list[float], + road_transform_trans: list[float], + road_transform_trans_std: list[float]) -> np.ndarray | None: self.old_rpy_weight = max(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index c292e9886c..786bdbbfec 100644 --- a/selfdrive/locationd/helpers.py +++ b/selfdrive/locationd/helpers.py @@ -1,5 +1,5 @@ import numpy as np -from typing import List, Optional, Tuple, Any +from typing import Any from cereal import log @@ -12,7 +12,7 @@ class NPQueue: def __len__(self) -> int: return len(self.arr) - def append(self, pt: List[float]) -> None: + def append(self, pt: list[float]) -> None: if len(self.arr) < self.maxlen: self.arr = np.append(self.arr, [pt], axis=0) else: @@ -21,7 +21,7 @@ class NPQueue: class PointBuckets: - def __init__(self, x_bounds: List[Tuple[float, float]], min_points: List[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: + def __init__(self, x_bounds: list[tuple[float, float]], min_points: list[float], min_points_total: int, points_per_bucket: int, rowsize: int) -> None: self.x_bounds = x_bounds self.buckets = {bounds: NPQueue(maxlen=points_per_bucket, rowsize=rowsize) for bounds in x_bounds} self.buckets_min_points = dict(zip(x_bounds, min_points, strict=True)) @@ -41,13 +41,13 @@ class PointBuckets: def add_point(self, x: float, y: float, bucket_val: float) -> None: raise NotImplementedError - def get_points(self, num_points: Optional[int] = None) -> Any: + def get_points(self, num_points: int = None) -> Any: points = np.vstack([x.arr for x in self.buckets.values()]) if num_points is None: return points return points[np.random.choice(np.arange(len(points)), min(len(points), num_points), replace=False)] - def load_points(self, points: List[List[float]]) -> None: + def load_points(self, points: list[list[float]]) -> None: for point in points: self.add_point(*point) diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 1050d68b9b..2ac392a778 100644 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -308,14 +308,12 @@ void Localizer::input_fake_gps_observations(double current_time) { } void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) { - // ignore the message if the fix is invalid - bool gps_invalid_flag = (log.getFlags() % 2 == 0); - bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); + bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY); bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)); bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)); bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK); - if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { + if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) { //this->gps_valid = false; this->determine_gps_mode(current_time); return; @@ -331,7 +329,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector(); VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos; - float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); + float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2)); MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal(); MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal(); diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 9230cb48f0..87a93cdd0c 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math import sys -from typing import Any, Dict +from typing import Any import numpy as np @@ -28,7 +28,7 @@ def _slice(n): return s -class States(): +class States: # Vehicle model params STIFFNESS = _slice(1) # [-] STEER_RATIO = _slice(1) # [-] @@ -70,7 +70,7 @@ class CarKalman(KalmanFilter): ]) P_initial = Q.copy() - obs_noise: Dict[int, Any] = { + obs_noise: dict[int, Any] = { ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py index c4933a6129..0cc3eca61d 100755 --- a/selfdrive/locationd/models/live_kf.py +++ b/selfdrive/locationd/models/live_kf.py @@ -20,7 +20,7 @@ def numpy2eigenstring(arr): return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()" -class States(): +class States: ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s @@ -39,7 +39,7 @@ class States(): ACC_BIAS_ERR = slice(18, 21) -class LiveKalman(): +class LiveKalman: name = 'live' initial_x = np.array([3.88e6, -3.37e6, 3.76e6, diff --git a/selfdrive/locationd/test/.gitignore b/selfdrive/locationd/test/.gitignore new file mode 100644 index 0000000000..89f9ac04aa --- /dev/null +++ b/selfdrive/locationd/test/.gitignore @@ -0,0 +1 @@ +out/ diff --git a/selfdrive/locationd/test/__init__.py b/selfdrive/locationd/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/locationd/test/test_calibrationd.py b/selfdrive/locationd/test/test_calibrationd.py new file mode 100644 index 0000000000..df61b6a7c7 --- /dev/null +++ b/selfdrive/locationd/test/test_calibrationd.py @@ -0,0 +1,112 @@ +import random + +import numpy as np + +import cereal.messaging as messaging +from cereal import log +from openpilot.common.params import Params +from openpilot.selfdrive.locationd.calibrationd import Calibrator, INPUTS_NEEDED, INPUTS_WANTED, BLOCK_SIZE, MIN_SPEED_FILTER, \ + MAX_YAW_RATE_FILTER, SMOOTH_CYCLES, HEIGHT_INIT, MAX_ALLOWED_PITCH_SPREAD, MAX_ALLOWED_YAW_SPREAD + + +def process_messages(c, cam_odo_calib, cycles, + cam_odo_speed=MIN_SPEED_FILTER + 1, + carstate_speed=MIN_SPEED_FILTER + 1, + cam_odo_yr=0.0, + cam_odo_speed_std=1e-3, + cam_odo_height_std=1e-3): + old_rpy_weight_prev = 0.0 + for _ in range(cycles): + assert (old_rpy_weight_prev - c.old_rpy_weight < 1/SMOOTH_CYCLES + 1e-3) + old_rpy_weight_prev = c.old_rpy_weight + c.handle_v_ego(carstate_speed) + c.handle_cam_odom([cam_odo_speed, + np.sin(cam_odo_calib[2]) * cam_odo_speed, + -np.sin(cam_odo_calib[1]) * cam_odo_speed], + [0.0, 0.0, cam_odo_yr], + [0.0, 0.0, 0.0], + [cam_odo_speed_std, cam_odo_speed_std, cam_odo_speed_std], + [0.0, 0.0, HEIGHT_INIT.item()], + [cam_odo_height_std, cam_odo_height_std, cam_odo_height_std]) + +class TestCalibrationd: + + def test_read_saved_params(self): + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = random.randint(1, 10) + msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)] + msg.liveCalibration.height = [random.random() for _ in range(1)] + Params().put("CalibrationParams", msg.to_bytes()) + c = Calibrator(param_put=True) + + np.testing.assert_allclose(msg.liveCalibration.rpyCalib, c.rpy) + np.testing.assert_allclose(msg.liveCalibration.height, c.height) + assert msg.liveCalibration.validBlocks == c.valid_blocks + + + def test_calibration_basics(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED) + assert c.valid_blocks == INPUTS_WANTED + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + c.reset() + + + def test_calibration_low_speed_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed=MIN_SPEED_FILTER - 1) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, carstate_speed=MIN_SPEED_FILTER - 1) + assert c.valid_blocks == 0 + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + + + def test_calibration_yaw_rate_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_yr=MAX_YAW_RATE_FILTER) + assert c.valid_blocks == 0 + np.testing.assert_allclose(c.rpy, np.zeros(3)) + np.testing.assert_allclose(c.height, HEIGHT_INIT) + + + def test_calibration_speed_std_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_speed_std=1e3) + assert c.valid_blocks == INPUTS_NEEDED + np.testing.assert_allclose(c.rpy, np.zeros(3)) + + + def test_calibration_speed_std_height_reject(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_WANTED, cam_odo_height_std=1e3) + assert c.valid_blocks == INPUTS_NEEDED + np.testing.assert_allclose(c.rpy, np.zeros(3)) + + + def test_calibration_auto_reset(self): + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + assert c.valid_blocks == INPUTS_NEEDED + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0], atol=1e-3) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*0.9, MAX_ALLOWED_YAW_SPREAD*0.9], BLOCK_SIZE + 10) + assert c.valid_blocks == INPUTS_NEEDED + 1 + assert c.cal_status == log.LiveCalibrationData.Status.calibrated + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + assert c.valid_blocks == INPUTS_NEEDED + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) + process_messages(c, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], BLOCK_SIZE + 10) + assert c.valid_blocks == 1 + assert c.cal_status == log.LiveCalibrationData.Status.recalibrating + np.testing.assert_allclose(c.rpy, [0.0, MAX_ALLOWED_PITCH_SPREAD*1.1, 0.0], atol=1e-2) + + c = Calibrator(param_put=False) + process_messages(c, [0.0, 0.0, 0.0], BLOCK_SIZE * INPUTS_NEEDED) + assert c.valid_blocks == INPUTS_NEEDED + np.testing.assert_allclose(c.rpy, [0.0, 0.0, 0.0]) + process_messages(c, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], BLOCK_SIZE + 10) + assert c.valid_blocks == 1 + assert c.cal_status == log.LiveCalibrationData.Status.recalibrating + np.testing.assert_allclose(c.rpy, [0.0, 0.0, MAX_ALLOWED_YAW_SPREAD*1.1], atol=1e-2) diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py new file mode 100644 index 0000000000..74ac7d2962 --- /dev/null +++ b/selfdrive/locationd/test/test_locationd.py @@ -0,0 +1,89 @@ +import pytest +import json +import random +import time +import capnp + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.params import Params +from openpilot.common.transformations.coordinates import ecef2geodetic + +from openpilot.system.manager.process_config import managed_processes + + +class TestLocationdProc: + LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', + 'accelerometer', 'gyroscope', 'magnetometer'] + + def setup_method(self): + self.pm = messaging.PubMaster(self.LLD_MSGS) + + self.params = Params() + self.params.put_bool("UbloxAvailable", True) + managed_processes['locationd'].prepare() + managed_processes['locationd'].start() + + def teardown_method(self): + managed_processes['locationd'].stop() + + def get_msg(self, name, t): + try: + msg = messaging.new_message(name) + except capnp.lib.capnp.KjException: + msg = messaging.new_message(name, 0) + + if name == "gpsLocationExternal": + msg.gpsLocationExternal.flags = 1 + msg.gpsLocationExternal.hasFix = True + msg.gpsLocationExternal.verticalAccuracy = 1.0 + msg.gpsLocationExternal.speedAccuracy = 1.0 + msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 + msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] + msg.gpsLocationExternal.latitude = float(self.lat) + msg.gpsLocationExternal.longitude = float(self.lon) + msg.gpsLocationExternal.unixTimestampMillis = t * 1e6 + msg.gpsLocationExternal.altitude = float(self.alt) + #if name == "gnssMeasurements": + # msg.gnssMeasurements.measTime = t + # msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z] + # msg.gnssMeasurements.positionECEF.std = [0,0,0] + # msg.gnssMeasurements.positionECEF.valid = True + # msg.gnssMeasurements.velocityECEF.value = [] + # msg.gnssMeasurements.velocityECEF.std = [0,0,0] + # msg.gnssMeasurements.velocityECEF.valid = True + elif name == 'cameraOdometry': + msg.cameraOdometry.rot = [0.0, 0.0, 0.0] + msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] + msg.cameraOdometry.trans = [0.0, 0.0, 0.0] + msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] + msg.logMonoTime = t + msg.valid = True + return msg + + def test_params_gps(self): + random.seed(123489234) + self.params.remove('LastGPSPosition') + + self.x = -2710700 + (random.random() * 1e5) + self.y = -4280600 + (random.random() * 1e5) + self.z = 3850300 + (random.random() * 1e5) + self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z]) + + # get fake messages at the correct frequency, listed in services.py + msgs = [] + for sec in range(65): + for name in self.LLD_MSGS: + for j in range(int(SERVICE_LIST[name].frequency)): + msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9))) + + for msg in sorted(msgs, key=lambda x: x.logMonoTime): + self.pm.send(msg.which(), msg) + if msg.which() == "cameraOdometry": + self.pm.wait_for_readers_to_update(msg.which(), 0.1, dt=0.005) + time.sleep(1) # wait for async params write + + lastGPS = json.loads(self.params.get('LastGPSPosition')) + assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001) + assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001) + assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.001) diff --git a/selfdrive/locationd/test/test_locationd_scenarios.py b/selfdrive/locationd/test/test_locationd_scenarios.py new file mode 100644 index 0000000000..ca52bffeea --- /dev/null +++ b/selfdrive/locationd/test/test_locationd_scenarios.py @@ -0,0 +1,223 @@ +import pytest +import numpy as np +from collections import defaultdict +from enum import Enum + +from openpilot.tools.lib.logreader import LogReader +from openpilot.selfdrive.test.process_replay.migration import migrate_all +from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name + +TEST_ROUTE = "ff2bd20623fcaeaa|2023-09-05--10-14-54/4" +GPS_MESSAGES = ['gpsLocationExternal', 'gpsLocation'] +SELECT_COMPARE_FIELDS = { + 'yaw_rate': ['angularVelocityCalibrated', 'value', 2], + 'roll': ['orientationNED', 'value', 0], + 'gps_flag': ['gpsOK'], + 'inputs_flag': ['inputsOK'], + 'sensors_flag': ['sensorsOK'], +} +JUNK_IDX = 100 + + +class Scenario(Enum): + BASE = 'base' + GPS_OFF = 'gps_off' + GPS_OFF_MIDWAY = 'gps_off_midway' + GPS_ON_MIDWAY = 'gps_on_midway' + GPS_TUNNEL = 'gps_tunnel' + GYRO_OFF = 'gyro_off' + GYRO_SPIKE_MIDWAY = 'gyro_spike_midway' + ACCEL_OFF = 'accel_off' + ACCEL_SPIKE_MIDWAY = 'accel_spike_midway' + + +def get_select_fields_data(logs): + def get_nested_keys(msg, keys): + val = None + for key in keys: + val = getattr(msg if val is None else val, key) if isinstance(key, str) else val[key] + return val + llk = [x.liveLocationKalman for x in logs if x.which() == 'liveLocationKalman'] + data = defaultdict(list) + for msg in llk: + for key, fields in SELECT_COMPARE_FIELDS.items(): + data[key].append(get_nested_keys(msg, fields)) + for key in data: + data[key] = np.array(data[key][JUNK_IDX:], dtype=float) + return data + + +def run_scenarios(scenario, logs): + if scenario == Scenario.BASE: + pass + + elif scenario == Scenario.GPS_OFF: + logs = sorted([x for x in logs if x.which() not in GPS_MESSAGES], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_OFF_MIDWAY: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[: len(gps) // 2], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_ON_MIDWAY: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[len(gps) // 2:], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GPS_TUNNEL: + non_gps = [x for x in logs if x.which() not in GPS_MESSAGES] + gps = [x for x in logs if x.which() in GPS_MESSAGES] + logs = sorted(non_gps + gps[:len(gps) // 4] + gps[-len(gps) // 4:], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GYRO_OFF: + logs = sorted([x for x in logs if x.which() != 'gyroscope'], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.GYRO_SPIKE_MIDWAY: + non_gyro = [x for x in logs if x.which() not in 'gyroscope'] + gyro = [x for x in logs if x.which() in 'gyroscope'] + temp = gyro[len(gyro) // 2].as_builder() + temp.gyroscope.gyroUncalibrated.v[0] += 3.0 + gyro[len(gyro) // 2] = temp.as_reader() + logs = sorted(non_gyro + gyro, key=lambda x: x.logMonoTime) + + elif scenario == Scenario.ACCEL_OFF: + logs = sorted([x for x in logs if x.which() != 'accelerometer'], key=lambda x: x.logMonoTime) + + elif scenario == Scenario.ACCEL_SPIKE_MIDWAY: + non_accel = [x for x in logs if x.which() not in 'accelerometer'] + accel = [x for x in logs if x.which() in 'accelerometer'] + temp = accel[len(accel) // 2].as_builder() + temp.accelerometer.acceleration.v[0] += 10.0 + accel[len(accel) // 2] = temp.as_reader() + logs = sorted(non_accel + accel, key=lambda x: x.logMonoTime) + + replayed_logs = replay_process_with_name(name='locationd', lr=logs) + return get_select_fields_data(logs), get_select_fields_data(replayed_logs) + + +@pytest.mark.xdist_group("test_locationd_scenarios") +@pytest.mark.shared_download_cache +class TestLocationdScenarios: + """ + Test locationd with different scenarios. In all these scenarios, we expect the following: + - locationd kalman filter should never go unstable (we care mostly about yaw_rate, roll, gpsOK, inputsOK, sensorsOK) + - faulty values should be ignored, with appropriate flags set + """ + + @classmethod + def setup_class(cls): + cls.logs = migrate_all(LogReader(TEST_ROUTE)) + + def test_base(self): + """ + Test: unchanged log + Expected Result: + - yaw_rate: unchanged + - roll: unchanged + """ + orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + + def test_gps_off(self): + """ + Test: no GPS message for the entire segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.all(replayed_data['gps_flag'] == 0.0) + + def test_gps_off_midway(self): + """ + Test: no GPS message for the second half of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: True for the first half, False for the second half + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_OFF_MIDWAY, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.diff(replayed_data['gps_flag'])[512] == -1.0 + + def test_gps_on_midway(self): + """ + Test: no GPS message for the first half of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False for the first half, True for the second half + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_ON_MIDWAY, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(1.5)) + assert np.diff(replayed_data['gps_flag'])[505] == 1.0 + + def test_gps_tunnel(self): + """ + Test: no GPS message for the middle section of the segment + Expected Result: + - yaw_rate: unchanged + - roll: + - gpsOK: False for the middle section, True for the rest + """ + orig_data, replayed_data = run_scenarios(Scenario.GPS_TUNNEL, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.diff(replayed_data['gps_flag'])[213] == -1.0 + assert np.diff(replayed_data['gps_flag'])[805] == 1.0 + + def test_gyro_off(self): + """ + Test: no gyroscope message for the entire segment + Expected Result: + - yaw_rate: 0 + - roll: 0 + - sensorsOK: False + """ + _, replayed_data = run_scenarios(Scenario.GYRO_OFF, self.logs) + assert np.allclose(replayed_data['yaw_rate'], 0.0) + assert np.allclose(replayed_data['roll'], 0.0) + assert np.all(replayed_data['sensors_flag'] == 0.0) + + def test_gyro_spikes(self): + """ + Test: a gyroscope spike in the middle of the segment + Expected Result: + - yaw_rate: unchanged + - roll: unchanged + - inputsOK: False for some time after the spike, True for the rest + """ + orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) + assert np.diff(replayed_data['inputs_flag'])[500] == -1.0 + assert np.diff(replayed_data['inputs_flag'])[694] == 1.0 + + def test_accel_off(self): + """ + Test: no accelerometer message for the entire segment + Expected Result: + - yaw_rate: 0 + - roll: 0 + - sensorsOK: False + """ + _, replayed_data = run_scenarios(Scenario.ACCEL_OFF, self.logs) + assert np.allclose(replayed_data['yaw_rate'], 0.0) + assert np.allclose(replayed_data['roll'], 0.0) + assert np.all(replayed_data['sensors_flag'] == 0.0) + + def test_accel_spikes(self): + """ + ToDo: + Test: an accelerometer spike in the middle of the segment + Expected Result: Right now, the kalman filter is not robust to small spikes like it is to gyroscope spikes. + """ + orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs) + assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.2)) + assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.5)) diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 69bab8d1fa..06f9044738 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -159,8 +159,10 @@ class TorqueEstimator(ParameterEstimator): def handle_log(self, t, which, msg): if which == "carControl": self.raw_points["carControl_t"].append(t + self.lag) - self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) self.raw_points["active"].append(msg.latActive) + elif which == "carOutput": + self.raw_points["carOutput_t"].append(t + self.lag) + self.raw_points["steer_torque"].append(-msg.actuatorsOutput.steer) elif which == "carState": self.raw_points["carState_t"].append(t + self.lag) self.raw_points["vego"].append(msg.vEgo) @@ -172,7 +174,7 @@ class TorqueEstimator(ParameterEstimator): active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carControl_t'], self.raw_points['active']).astype(bool) steer_override = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t, DT_MDL), self.raw_points['carState_t'], self.raw_points['steer_override']).astype(bool) vego = np.interp(t, self.raw_points['carState_t'], self.raw_points['vego']) - steer = np.interp(t, self.raw_points['carControl_t'], self.raw_points['steer_torque']) + steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']) lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY) if all(active) and (not any(steer_override)) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD) and (abs(lateral_acc) <= LAT_ACC_THRESHOLD): self.filtered_points.add_point(float(steer), float(lateral_acc)) @@ -218,7 +220,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveLocationKalman'], poll='liveLocationKalman') params = Params() with car.CarParams.from_bytes(params.get("CarParams", block=True)) as CP: diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 2d15223d1e..deb84d5952 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,3 +1,5 @@ +import glob + Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'transformations') lenv = env.Clone() lenvCython = envCython.Clone() @@ -50,11 +52,12 @@ lenvCython.Program('runners/runmodel_pyx.so', 'runners/runmodel_pyx.pyx', LIBS=c lenvCython.Program('runners/snpemodel_pyx.so', 'runners/snpemodel_pyx.pyx', LIBS=[snpemodel_lib, snpe_lib, *cython_libs], FRAMEWORKS=frameworks, RPATH=snpe_rpath) lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) +tinygrad_files = ["#"+x for x in glob.glob(env.Dir("#tinygrad_repo").relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath)] + # Get model metadata fn = File("models/supercombo").abspath cmd = f'python3 {Dir("#selfdrive/modeld").abspath}/get_model_metadata.py {fn}.onnx' -files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.endswith("get_model_metadata.py")], []) -lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"]+files, cmd) +lenv.Command(fn + "_metadata.pkl", [fn + ".onnx"] + tinygrad_files, cmd) # Build thneed model if arch == "larch64" or GetOption('pc_thneed'): @@ -64,7 +67,6 @@ if arch == "larch64" or GetOption('pc_thneed'): tinygrad_opts += ["FLOAT16=1", "PYOPENCL_NO_CACHE=1"] cmd = f"cd {Dir('#').abspath}/tinygrad_repo && " + ' '.join(tinygrad_opts) + f" python3 openpilot/compile2.py {fn}.onnx {fn}.thneed" - tinygrad_files = sum([lenv.Glob("#"+x) for x in open(File("#release/files_common").abspath).read().split("\n") if x.startswith("tinygrad_repo/")], []) lenv.Command(fn + ".thneed", [fn + ".onnx"] + tinygrad_files, cmd) thneed_lib = env.SharedLibrary('thneed', thneed_src, LIBS=[gpucommon, common, 'zmq', 'OpenCL', 'dl']) diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py index dda1ff5e33..ca81bffcc8 100644 --- a/selfdrive/modeld/constants.py +++ b/selfdrive/modeld/constants.py @@ -18,9 +18,6 @@ class ModelConstants: HISTORY_BUFFER_LEN = 99 DESIRE_LEN = 8 TRAFFIC_CONVENTION_LEN = 2 - NAV_FEATURE_LEN = 256 - NAV_INSTRUCTION_LEN = 150 - DRIVING_STYLE_LEN = 12 LAT_PLANNER_STATE_LEN = 4 LATERAL_CONTROL_PARAMS_LEN = 2 PREV_DESIRED_CURV_LEN = 1 diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 1e25964702..a388bf089c 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -6,11 +6,10 @@ import time import ctypes import numpy as np from pathlib import Path -from typing import Tuple, Dict from cereal import messaging from cereal.messaging import PubMaster, SubMaster -from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf +from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority @@ -53,7 +52,7 @@ class DMonitoringModelResult(ctypes.Structure): ("wheel_on_right_prob", ctypes.c_float)] class ModelState: - inputs: Dict[str, np.ndarray] + inputs: dict[str, np.ndarray] output: np.ndarray model: ModelRunner @@ -68,7 +67,7 @@ class ModelState: self.model.addInput("input_img", None) self.model.addInput("calib", self.inputs['calib']) - def run(self, buf:VisionBuf, calib:np.ndarray) -> Tuple[np.ndarray, float]: + def run(self, buf:VisionBuf, calib:np.ndarray) -> tuple[np.ndarray, float]: self.inputs['calib'][:] = calib v_offset = buf.height - MODEL_HEIGHT diff --git a/selfdrive/modeld/fill_model_msg.py b/selfdrive/modeld/fill_model_msg.py index c76966867a..f6a57f1a40 100644 --- a/selfdrive/modeld/fill_model_msg.py +++ b/selfdrive/modeld/fill_model_msg.py @@ -1,7 +1,6 @@ import os import capnp import numpy as np -from typing import Dict from cereal import log from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta @@ -42,10 +41,9 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std if a_std is not None: builder.aStd = a_std.tolist() -def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], publish_state: PublishState, +def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int, frame_id: int, frame_drop: float, - timestamp_eof: int, timestamp_llk: int, model_execution_time: float, - nav_enabled: bool, valid: bool) -> None: + timestamp_eof: int, model_execution_time: float, valid: bool) -> None: frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0 msg.valid = valid @@ -55,9 +53,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, modelV2.frameAge = frame_age modelV2.frameDropPerc = frame_drop * 100 modelV2.timestampEof = timestamp_eof - modelV2.locationMonoTime = timestamp_llk modelV2.modelExecutionTime = model_execution_time - modelV2.navEnabled = nav_enabled # plan position = modelV2.position @@ -174,7 +170,7 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, if SEND_RAW_PRED: modelV2.rawPredictions = net_output_data['raw_pred'].tobytes() -def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: Dict[str, np.ndarray], +def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], vipc_frame_id: int, vipc_dropped_frames: int, timestamp_eof: int, live_calib_seen: bool) -> None: msg.valid = live_calib_seen & (vipc_dropped_frames < 1) cameraOdometry = msg.cameraOdometry diff --git a/selfdrive/modeld/get_model_metadata.py b/selfdrive/modeld/get_model_metadata.py index 187f83399b..144860204f 100755 --- a/selfdrive/modeld/get_model_metadata.py +++ b/selfdrive/modeld/get_model_metadata.py @@ -4,9 +4,8 @@ import pathlib import onnx import codecs import pickle -from typing import Tuple -def get_name_and_shape(value_info:onnx.ValueInfoProto) -> Tuple[str, Tuple[int,...]]: +def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]: shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim]) name = value_info.name return name, shape diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f2842d94b7..3ac80aad91 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -6,16 +6,16 @@ import numpy as np import cereal.messaging as messaging from cereal import car, log from pathlib import Path -from typing import Dict, Optional from setproctitle import setproctitle from cereal.messaging import PubMaster, SubMaster -from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf +from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process +from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix -from openpilot.selfdrive import sentry +from openpilot.system import sentry from openpilot.selfdrive.car.car_helpers import get_demo_car_params from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime @@ -45,7 +45,7 @@ class FrameMeta: class ModelState: frame: ModelFrame wide_frame: ModelFrame - inputs: Dict[str, np.ndarray] + inputs: dict[str, np.ndarray] output: np.ndarray prev_desire: np.ndarray # for tracking the rising edge of the pulse model: ModelRunner @@ -59,8 +59,6 @@ class ModelState: 'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32), 'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32), 'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32), - 'nav_features': np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32), - 'nav_instructions': np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32), 'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32), } @@ -78,14 +76,14 @@ class ModelState: for k,v in self.inputs.items(): self.model.addInput(k, v) - def slice_outputs(self, model_outputs: np.ndarray) -> Dict[str, np.ndarray]: + def slice_outputs(self, model_outputs: np.ndarray) -> dict[str, np.ndarray]: parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in self.output_slices.items()} if SEND_RAW_PRED: parsed_model_outputs['raw_pred'] = model_outputs.copy() return parsed_model_outputs def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, - inputs: Dict[str, np.ndarray], prepare_only: bool) -> Optional[Dict[str, np.ndarray]]: + inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge inputs['desire'][0] = 0 self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:] @@ -94,8 +92,6 @@ class ModelState: self.inputs['traffic_convention'][:] = inputs['traffic_convention'] self.inputs['lateral_control_params'][:] = inputs['lateral_control_params'] - self.inputs['nav_features'][:] = inputs['nav_features'] - self.inputs['nav_instructions'][:] = inputs['nav_instructions'] # if getCLBuffer is not None, frame will be None self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))) @@ -154,7 +150,7 @@ def main(demo=False): # messaging pm = PubMaster(["modelV2", "cameraOdometry"]) - sm = SubMaster(["carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel", "navInstruction", "carControl"]) + sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"]) publish_state = PublishState() params = Params() @@ -168,8 +164,6 @@ def main(demo=False): model_transform_main = np.zeros((3, 3), dtype=np.float32) model_transform_extra = np.zeros((3, 3), dtype=np.float32) live_calib_seen = False - nav_features = np.zeros(ModelConstants.NAV_FEATURE_LEN, dtype=np.float32) - nav_instructions = np.zeros(ModelConstants.NAV_INSTRUCTION_LEN, dtype=np.float32) buf_main, buf_extra = None, None meta_main = FrameMeta() meta_extra = FrameMeta() @@ -196,7 +190,7 @@ def main(demo=False): break if buf_main is None: - cloudlog.error("vipc_client_main no frame") + cloudlog.debug("vipc_client_main no frame") continue if use_extra_client: @@ -208,13 +202,12 @@ def main(demo=False): break if buf_extra is None: - cloudlog.error("vipc_client_extra no frame") + cloudlog.debug("vipc_client_extra no frame") continue if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000: - cloudlog.error("frames out of sync! main: {} ({:.5f}), extra: {} ({:.5f})".format( - meta_main.frame_id, meta_main.timestamp_sof / 1e9, - meta_extra.frame_id, meta_extra.timestamp_sof / 1e9)) + cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\ + extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})") else: # Use single camera @@ -226,10 +219,11 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) - if sm.updated["liveCalibration"]: + if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) - model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32) - model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32) + dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] + model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) + model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) live_calib_seen = True traffic_convention = np.zeros(2) @@ -239,30 +233,6 @@ def main(demo=False): if desire >= 0 and desire < ModelConstants.DESIRE_LEN: vec_desire[desire] = 1 - # Enable/disable nav features - timestamp_llk = sm["navModel"].locationMonoTime - nav_valid = sm.valid["navModel"] # and (nanos_since_boot() - timestamp_llk < 1e9) - nav_enabled = nav_valid and params.get_bool("ExperimentalMode") - - if not nav_enabled: - nav_features[:] = 0 - nav_instructions[:] = 0 - - if nav_enabled and sm.updated["navModel"]: - nav_features = np.array(sm["navModel"].features) - - if nav_enabled and sm.updated["navInstruction"]: - nav_instructions[:] = 0 - for maneuver in sm["navInstruction"].allManeuvers: - distance_idx = 25 + int(maneuver.distance / 20) - direction_idx = 0 - if maneuver.modifier in ("left", "slight left", "sharp left"): - direction_idx = 1 - if maneuver.modifier in ("right", "slight right", "sharp right"): - direction_idx = 2 - if 0 <= distance_idx < 50: - nav_instructions[distance_idx*3 + direction_idx] = 1 - # tracked dropped frames vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1) frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10)) @@ -276,12 +246,11 @@ def main(demo=False): if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") - inputs:Dict[str, np.ndarray] = { + inputs:dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, 'lateral_control_params': lateral_control_params, - 'nav_features': nav_features, - 'nav_instructions': nav_instructions} + } mt1 = time.perf_counter() model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) @@ -292,7 +261,7 @@ def main(demo=False): modelv2_send = messaging.new_message('modelV2') posenet_send = messaging.new_message('cameraOdometry') fill_model_msg(modelv2_send, model_output, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, - meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, live_calib_seen) + meta_main.timestamp_eof, model_execution_time, live_calib_seen) desire_state = modelv2_send.modelV2.meta.desireState l_lane_change_prob = desire_state[log.Desire.laneChangeLeft] diff --git a/selfdrive/modeld/models/README.md b/selfdrive/modeld/models/README.md new file mode 100644 index 0000000000..9e11ca8255 --- /dev/null +++ b/selfdrive/modeld/models/README.md @@ -0,0 +1,62 @@ +## Neural networks in openpilot +To view the architecture of the ONNX networks, you can use [netron](https://netron.app/) + +## Supercombo +### Supercombo input format (Full size: 799906 x float32) +* **image stream** + * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256 + * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256 + * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2] + * Channel 4 represents the half-res U channel + * Channel 5 represents the half-res V channel +* **wide image stream** + * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256 + * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256 + * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2] + * Channel 4 represents the half-res U channel + * Channel 5 represents the half-res V channel +* **desire** + * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8 +* **traffic convention** + * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2 +* **feature buffer** + * A buffer of intermediate features that gets appended to the current feature to form a 5 seconds temporal context (at 20FPS) : 99 * 512 + + +### Supercombo output format (Full size: XXX x float32) +Read [here](https://github.com/commaai/openpilot/blob/90af436a121164a51da9fa48d093c29f738adf6a/selfdrive/modeld/models/driving.h#L236) for more. + + +## Driver Monitoring Model +* .onnx model can be run with onnx runtimes +* .dlc file is a pre-quantized model and only runs on qualcomm DSPs + +### input format +* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format: + * full input size is 1440 * 960 = 1382400 + * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner) +* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs + +### output format +* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33)) + * for each person in the front seats (2 * 41) + * face pose: 12 = 6 + 6 + * face orientation [pitch, yaw, roll] in camera frame: 3 + * face position [dx, dy] relative to image center: 2 + * normalized face size: 1 + * standard deviations for above outputs: 6 + * face visible probability: 1 + * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1 + * eye position and size, and their standard deviations: 8 + * eye visible probability: 1 + * eye closed probability: 1 + * wearing sunglasses probability: 1 + * face occluded probability: 1 + * touching wheel probability: 1 + * paying attention probability: 1 + * (deprecated) distracted probabilities: 2 + * using phone probability: 1 + * distracted probability: 1 + * common outputs 2 + * poor camera vision probability: 1 + * left hand drive probability: 1 diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc index 5e28e9b95d..523ce00e43 100644 --- a/selfdrive/modeld/models/commonmodel.cc +++ b/selfdrive/modeld/models/commonmodel.cc @@ -1,13 +1,10 @@ #include "selfdrive/modeld/models/commonmodel.h" -#include #include #include #include #include "common/clutil.h" -#include "common/mat.h" -#include "common/timing.h" ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) { input_frames = std::make_unique(buf_size); @@ -52,21 +49,6 @@ ModelFrame::~ModelFrame() { CL_CHECK(clReleaseCommandQueue(q)); } -void softmax(const float* input, float* output, size_t len) { - const float max_val = *std::max_element(input, input + len); - float denominator = 0; - for (int i = 0; i < len; i++) { - float const v_exp = expf(input[i] - max_val); - denominator += v_exp; - output[i] = v_exp; - } - - const float inv_denominator = 1. / denominator; - for (int i = 0; i < len; i++) { - output[i] *= inv_denominator; - } -} - float sigmoid(float input) { return 1 / (1 + expf(-input)); } diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h index 1a079da055..2cf79094a6 100644 --- a/selfdrive/modeld/models/commonmodel.h +++ b/selfdrive/modeld/models/commonmodel.h @@ -13,20 +13,11 @@ #endif #include "common/mat.h" -#include "cereal/messaging/messaging.h" #include "selfdrive/modeld/transforms/loadyuv.h" #include "selfdrive/modeld/transforms/transform.h" -const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; - -void softmax(const float* input, float* output, size_t len); float sigmoid(float input); -template -constexpr const kj::ArrayPtr to_kj_array_ptr(const std::array &arr) { - return kj::ArrayPtr(arr.data(), arr.size()); -} - class ModelFrame { public: ModelFrame(cl_device_id device_id, cl_context context); diff --git a/selfdrive/modeld/models/commonmodel.pxd b/selfdrive/modeld/models/commonmodel.pxd index 57b79aeafb..7c3eb0b3d7 100644 --- a/selfdrive/modeld/models/commonmodel.pxd +++ b/selfdrive/modeld/models/commonmodel.pxd @@ -1,6 +1,6 @@ # distutils: language = c++ -from cereal.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem +from msgq.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem cdef extern from "common/mat.h": cdef struct mat3: diff --git a/selfdrive/modeld/models/commonmodel_pyx.pxd b/selfdrive/modeld/models/commonmodel_pyx.pxd index 97e3914588..0bb798625b 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pxd +++ b/selfdrive/modeld/models/commonmodel_pyx.pxd @@ -1,7 +1,7 @@ # distutils: language = c++ -from cereal.visionipc.visionipc cimport cl_mem -from cereal.visionipc.visionipc_pyx cimport CLContext as BaseCLContext +from msgq.visionipc.visionipc cimport cl_mem +from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext cdef class CLContext(BaseCLContext): pass diff --git a/selfdrive/modeld/models/commonmodel_pyx.pyx b/selfdrive/modeld/models/commonmodel_pyx.pyx index e33d301aff..e292bb0d2d 100644 --- a/selfdrive/modeld/models/commonmodel_pyx.pyx +++ b/selfdrive/modeld/models/commonmodel_pyx.pyx @@ -5,8 +5,8 @@ import numpy as np cimport numpy as cnp from libc.string cimport memcpy -from cereal.visionipc.visionipc cimport cl_mem -from cereal.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext +from msgq.visionipc.visionipc cimport cl_mem +from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame @@ -37,7 +37,11 @@ cdef class ModelFrame: def prepare(self, VisionBuf buf, float[:] projection, CLMem output): cdef mat3 cprojection memcpy(cprojection.v, &projection[0], 9*sizeof(float)) - cdef float * data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem) + cdef float * data + if output is None: + data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL) + else: + data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem) if not data: return None return np.asarray( data) diff --git a/selfdrive/modeld/models/dmonitoring_model.current b/selfdrive/modeld/models/dmonitoring_model.current new file mode 100644 index 0000000000..f935ab06b3 --- /dev/null +++ b/selfdrive/modeld/models/dmonitoring_model.current @@ -0,0 +1,2 @@ +5ec97a39-0095-4cea-adfa-6d72b1966cc1 +26cac7a9757a27c783a365403040a1bd27ccdaea \ No newline at end of file diff --git a/third_party/snpe/x86_64/libHtpPrepare.so b/selfdrive/modeld/models/dmonitoring_model.onnx similarity index 54% rename from third_party/snpe/x86_64/libHtpPrepare.so rename to selfdrive/modeld/models/dmonitoring_model.onnx index 0b0657dd33..a9a1cede31 100644 Binary files a/third_party/snpe/x86_64/libHtpPrepare.so and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/dmonitoring_model_q.dlc b/selfdrive/modeld/models/dmonitoring_model_q.dlc index 7f911cb213..1ed22c82ee 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model_q.dlc and b/selfdrive/modeld/models/dmonitoring_model_q.dlc differ diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc deleted file mode 100644 index 505d5c556f..0000000000 Binary files a/selfdrive/modeld/models/navmodel_q.dlc and /dev/null differ diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index acd7490966..d0117f8756 100644 Binary files a/selfdrive/modeld/models/supercombo.onnx and b/selfdrive/modeld/models/supercombo.onnx differ diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py deleted file mode 100755 index ed0b597dfe..0000000000 --- a/selfdrive/modeld/navmodeld.py +++ /dev/null @@ -1,118 +0,0 @@ -#!/usr/bin/env python3 -import gc -import math -import time -import ctypes -import numpy as np -from pathlib import Path -from typing import Tuple, Dict - -from cereal import messaging -from cereal.messaging import PubMaster, SubMaster -from cereal.visionipc import VisionIpcClient, VisionStreamType -from openpilot.common.swaglog import cloudlog -from openpilot.common.params import Params -from openpilot.common.realtime import set_realtime_priority -from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime - -NAV_INPUT_SIZE = 256*256 -NAV_FEATURE_LEN = 256 -NAV_DESIRE_LEN = 32 -NAV_OUTPUT_SIZE = 2*2*ModelConstants.IDX_N + NAV_DESIRE_LEN + NAV_FEATURE_LEN -MODEL_PATHS = { - ModelRunner.SNPE: Path(__file__).parent / 'models/navmodel_q.dlc', - ModelRunner.ONNX: Path(__file__).parent / 'models/navmodel.onnx'} - -class NavModelOutputXY(ctypes.Structure): - _fields_ = [ - ("x", ctypes.c_float), - ("y", ctypes.c_float)] - -class NavModelOutputPlan(ctypes.Structure): - _fields_ = [ - ("mean", NavModelOutputXY*ModelConstants.IDX_N), - ("std", NavModelOutputXY*ModelConstants.IDX_N)] - -class NavModelResult(ctypes.Structure): - _fields_ = [ - ("plan", NavModelOutputPlan), - ("desire_pred", ctypes.c_float*NAV_DESIRE_LEN), - ("features", ctypes.c_float*NAV_FEATURE_LEN)] - -class ModelState: - inputs: Dict[str, np.ndarray] - output: np.ndarray - model: ModelRunner - - def __init__(self): - assert ctypes.sizeof(NavModelResult) == NAV_OUTPUT_SIZE * ctypes.sizeof(ctypes.c_float) - self.output = np.zeros(NAV_OUTPUT_SIZE, dtype=np.float32) - self.inputs = {'input_img': np.zeros(NAV_INPUT_SIZE, dtype=np.uint8)} - self.model = ModelRunner(MODEL_PATHS, self.output, Runtime.DSP, True, None) - self.model.addInput("input_img", None) - - def run(self, buf:np.ndarray) -> Tuple[np.ndarray, float]: - self.inputs['input_img'][:] = buf - - t1 = time.perf_counter() - self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32)) - self.model.execute() - t2 = time.perf_counter() - return self.output, t2 - t1 - -def get_navmodel_packet(model_output: np.ndarray, valid: bool, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float): - model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(NavModelResult)).contents - msg = messaging.new_message('navModel') - msg.valid = valid - msg.navModel.frameId = frame_id - msg.navModel.locationMonoTime = location_ts - msg.navModel.modelExecutionTime = execution_time - msg.navModel.dspExecutionTime = dsp_execution_time - msg.navModel.features = model_result.features[:] - msg.navModel.desirePrediction = model_result.desire_pred[:] - msg.navModel.position.x = [p.x for p in model_result.plan.mean] - msg.navModel.position.y = [p.y for p in model_result.plan.mean] - msg.navModel.position.xStd = [math.exp(p.x) for p in model_result.plan.std] - msg.navModel.position.yStd = [math.exp(p.y) for p in model_result.plan.std] - return msg - - -def main(): - gc.disable() - set_realtime_priority(1) - - # there exists a race condition when two processes try to create a - # SNPE model runner at the same time, wait for dmonitoringmodeld to finish - cloudlog.warning("waiting for dmonitoringmodeld to initialize") - if not Params().get_bool("DmModelInitialized", True): - return - - model = ModelState() - cloudlog.warning("models loaded, navmodeld starting") - - vipc_client = VisionIpcClient("navd", VisionStreamType.VISION_STREAM_MAP, True) - while not vipc_client.connect(False): - time.sleep(0.1) - assert vipc_client.is_connected() - cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}") - - sm = SubMaster(["navInstruction"]) - pm = PubMaster(["navModel"]) - - while True: - buf = vipc_client.recv() - if buf is None: - continue - - sm.update(0) - t1 = time.perf_counter() - model_output, dsp_execution_time = model.run(buf.data[:buf.uv_offset]) - t2 = time.perf_counter() - - valid = vipc_client.valid and sm.valid["navInstruction"] - pm.send("navModel", get_navmodel_packet(model_output, valid, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, dsp_execution_time)) - - -if __name__ == "__main__": - main() diff --git a/selfdrive/modeld/parse_model_outputs.py b/selfdrive/modeld/parse_model_outputs.py index 01cba29d1c..af57e11d03 100644 --- a/selfdrive/modeld/parse_model_outputs.py +++ b/selfdrive/modeld/parse_model_outputs.py @@ -1,5 +1,4 @@ import numpy as np -from typing import Dict from openpilot.selfdrive.modeld.constants import ModelConstants def sigmoid(x): @@ -82,7 +81,7 @@ class Parser: outs[name] = pred_mu_final.reshape(final_shape) outs[name + '_stds'] = pred_std_final.reshape(final_shape) - def parse_outputs(self, outs: Dict[str, np.ndarray]) -> Dict[str, np.ndarray]: + def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]: self.parse_mdn('plan', outs, in_N=ModelConstants.PLAN_MHP_N, out_N=ModelConstants.PLAN_MHP_SELECTION, out_shape=(ModelConstants.IDX_N,ModelConstants.PLAN_WIDTH)) self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH)) diff --git a/selfdrive/modeld/runners/onnxmodel.py b/selfdrive/modeld/runners/onnxmodel.py index f86bee3878..69b44a5a97 100644 --- a/selfdrive/modeld/runners/onnxmodel.py +++ b/selfdrive/modeld/runners/onnxmodel.py @@ -3,7 +3,7 @@ import itertools import os import sys import numpy as np -from typing import Tuple, Dict, Union, Any +from typing import Any from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel @@ -38,7 +38,7 @@ def create_ort_session(path, fp16_to_fp32): options = ort.SessionOptions() options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL - provider: Union[str, Tuple[str, Dict[Any, Any]]] + provider: str | tuple[str, dict[Any, Any]] if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: provider = 'OpenVINOExecutionProvider' elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: diff --git a/selfdrive/modeld/runners/snpemodel.pxd b/selfdrive/modeld/runners/snpemodel.pxd index 1f928da332..a911b43584 100644 --- a/selfdrive/modeld/runners/snpemodel.pxd +++ b/selfdrive/modeld/runners/snpemodel.pxd @@ -2,7 +2,7 @@ from libcpp.string cimport string -from cereal.visionipc.visionipc cimport cl_context +from msgq.visionipc.visionipc cimport cl_context cdef extern from "selfdrive/modeld/runners/snpemodel.h": cdef cppclass SNPEModel: diff --git a/selfdrive/modeld/runners/thneedmodel.pxd b/selfdrive/modeld/runners/thneedmodel.pxd index 90af972865..79e24dbdd6 100644 --- a/selfdrive/modeld/runners/thneedmodel.pxd +++ b/selfdrive/modeld/runners/thneedmodel.pxd @@ -2,7 +2,7 @@ from libcpp.string cimport string -from cereal.visionipc.visionipc cimport cl_context +from msgq.visionipc.visionipc cimport cl_context cdef extern from "selfdrive/modeld/runners/thneedmodel.h": cdef cppclass ThneedModel: diff --git a/selfdrive/modeld/tests/__init__.py b/selfdrive/modeld/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/modeld/tests/dmon_lag/repro.cc b/selfdrive/modeld/tests/dmon_lag/repro.cc new file mode 100644 index 0000000000..c4c1c65cbe --- /dev/null +++ b/selfdrive/modeld/tests/dmon_lag/repro.cc @@ -0,0 +1,101 @@ +// clang++ -O2 repro.cc && ./a.out + +#include +#include +#include + +#include +#include +#include +#include +#include + +static inline double millis_since_boot() { + struct timespec t; + clock_gettime(CLOCK_BOOTTIME, &t); + return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6; +} + +#define MODEL_WIDTH 320 +#define MODEL_HEIGHT 640 + +// null function still breaks it +#define input_lambda(x) x + +// this is copied from models/dmonitoring.cc, and is the code that triggers the issue +void inner(uint8_t *resized_buf, float *net_input_buf) { + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + // one shot conversion, O(n) anyway + // yuvframe2tensor, normalize + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + // Y_ul + net_input_buf[(c*MODEL_HEIGHT/2) + r] = input_lambda(resized_buf[(2*r*resized_width) + (2*c)]); + // Y_ur + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width) + (2*c+1)]); + // Y_dl + net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c)]); + // Y_dr + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r*resized_width+1) + (2*c+1)]); + // U + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c]); + // V + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c]); + } + } +} + +float trial() { + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v + + // allocate the buffers + uint8_t *resized_buf = (uint8_t*)malloc(resized_width*resized_height*3/2); + float *net_input_buf = (float*)malloc(yuv_buf_len*sizeof(float)); + printf("allocate -- %p 0x%x -- %p 0x%lx\n", resized_buf, resized_width*resized_height*3/2, net_input_buf, yuv_buf_len*sizeof(float)); + + // test for bad buffers + static int CNT = 20; + float avg = 0.0; + for (int i = 0; i < CNT; i++) { + double s4 = millis_since_boot(); + inner(resized_buf, net_input_buf); + double s5 = millis_since_boot(); + avg += s5-s4; + } + avg /= CNT; + + // once it's bad, it's reliably bad + if (avg > 10) { + printf("HIT %f\n", avg); + printf("BAD\n"); + + for (int i = 0; i < 200; i++) { + double s4 = millis_since_boot(); + inner(resized_buf, net_input_buf); + double s5 = millis_since_boot(); + printf("%.2f ", s5-s4); + } + printf("\n"); + + exit(0); + } + + // don't free so we get a different buffer each time + //free(resized_buf); + //free(net_input_buf); + + return avg; +} + +int main() { + while (true) { + float ret = trial(); + printf("got %f\n", ret); + } +} + diff --git a/selfdrive/modeld/tests/snpe_benchmark/.gitignore b/selfdrive/modeld/tests/snpe_benchmark/.gitignore new file mode 100644 index 0000000000..d83a1b2ff5 --- /dev/null +++ b/selfdrive/modeld/tests/snpe_benchmark/.gitignore @@ -0,0 +1 @@ +benchmark diff --git a/selfdrive/modeld/tests/snpe_benchmark/benchmark.cc b/selfdrive/modeld/tests/snpe_benchmark/benchmark.cc new file mode 100644 index 0000000000..f72f7fb1a7 --- /dev/null +++ b/selfdrive/modeld/tests/snpe_benchmark/benchmark.cc @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p) { + return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec); +} + +void PrintErrorStringAndExit() { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + std::exit(EXIT_FAILURE); +} + + +zdl::DlSystem::Runtime_t checkRuntime() { + static zdl::DlSystem::Version_t Version = zdl::SNPE::SNPEFactory::getLibraryVersion(); + static zdl::DlSystem::Runtime_t Runtime; + std::cout << "SNPE Version: " << Version.asString().c_str() << std::endl; //Print Version number + if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::DSP)) { + std::cout << "Using DSP runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::DSP; + } else if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::GPU)) { + std::cout << "Using GPU runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::GPU; + } else { + std::cout << "Using cpu runtime" << std::endl; + Runtime = zdl::DlSystem::Runtime_t::CPU; + } + return Runtime; +} + +void test(char *filename) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(filename); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + + cout << "get input tensor names done" << endl; + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + vector > inputs; + for (int i = 0; i < strList.size(); i++) { + cout << "input name: " << strList.at(i) << endl; + + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(i)); + const auto &inputShape = *inputDims_opt; + inputs.push_back(zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape)); + inputTensorMap.add(strList.at(i), inputs[i].get()); + } + + struct timespec start, end; + cout << "**** starting benchmark ****" << endl; + for (int i = 0; i < 50; i++) { + clock_gettime(CLOCK_MONOTONIC, &start); + int err = snpe->execute(inputTensorMap, outputTensorMap); + assert(err == true); + clock_gettime(CLOCK_MONOTONIC, &end); + uint64_t timeElapsed = timespecDiff(&end, &start); + printf("time: %f ms\n", timeElapsed*1.0/1e6); + } +} + +void get_testframe(int index, std::unique_ptr &input) { + FILE * pFile; + string filepath="/data/ipt/quantize_samples/sample_input_"+std::to_string(index); + pFile = fopen(filepath.c_str(), "rb"); + int length = 1*6*160*320*4; + float * frame_buffer = new float[length/4]; // 32/8 + fread(frame_buffer, length, 1, pFile); + // std::cout << *(frame_buffer+length/4-1) << std::endl; + std::copy(frame_buffer, frame_buffer+(length/4), input->begin()); + fclose(pFile); +} + +void SaveITensor(const std::string& path, const zdl::DlSystem::ITensor* tensor) +{ + std::ofstream os(path, std::ofstream::binary); + if (!os) + { + std::cerr << "Failed to open output file for writing: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + for ( auto it = tensor->cbegin(); it != tensor->cend(); ++it ) + { + float f = *it; + if (!os.write(reinterpret_cast(&f), sizeof(float))) + { + std::cerr << "Failed to write data to: " << path << "\n"; + std::exit(EXIT_FAILURE); + } + } +} + +void testrun(char* modelfile) { + static zdl::DlSystem::Runtime_t runtime = checkRuntime(); + std::unique_ptr container; + container = zdl::DlContainer::IDlContainer::open(modelfile); + + if (!container) { PrintErrorStringAndExit(); } + cout << "start build" << endl; + std::unique_ptr snpe; + { + snpe = NULL; + zdl::SNPE::SNPEBuilder snpeBuilder(container.get()); + snpe = snpeBuilder.setOutputLayers({}) + .setRuntimeProcessor(runtime) + .setUseUserSuppliedBuffers(false) + //.setDebugMode(true) + .build(); + if (!snpe) { + cout << "ERROR!" << endl; + const char* const errStr = zdl::DlSystem::getLastErrorString(); + std::cerr << errStr << std::endl; + } + cout << "ran snpeBuilder" << endl; + } + + const auto &strList_opt = snpe->getInputTensorNames(); + if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names"); + cout << "get input tensor names done" << endl; + + const auto &strList = *strList_opt; + static zdl::DlSystem::TensorMap inputTensorMap; + static zdl::DlSystem::TensorMap outputTensorMap; + + assert(strList.size() == 1); + const auto &inputDims_opt = snpe->getInputDimensions(strList.at(0)); + const auto &inputShape = *inputDims_opt; + std::cout << "winkwink" << std::endl; + + for (int i=0; i<10000; i++) { + std::unique_ptr input; + input = zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape); + get_testframe(i, input); + snpe->execute(input.get(), outputTensorMap); + zdl::DlSystem::StringList tensorNames = outputTensorMap.getTensorNames(); + std::for_each(tensorNames.begin(), tensorNames.end(), [&](const char* name) { + std::ostringstream path; + path << "/data/opt/Result_" << std::to_string(i) << ".raw"; + auto tensorPtr = outputTensorMap.getTensor(name); + SaveITensor(path.str(), tensorPtr); + }); + } +} + +int main(int argc, char* argv[]) { + if (argc < 2) { + printf("usage: %s \n", argv[0]); + return -1; + } + + if (argc == 2) { + while (true) test(argv[1]); + } else if (argc == 3) { + testrun(argv[1]); + } + return 0; +} + diff --git a/selfdrive/modeld/tests/snpe_benchmark/benchmark.sh b/selfdrive/modeld/tests/snpe_benchmark/benchmark.sh new file mode 100755 index 0000000000..a9d3f79786 --- /dev/null +++ b/selfdrive/modeld/tests/snpe_benchmark/benchmark.sh @@ -0,0 +1,4 @@ +#!/bin/sh -e +clang++ -I /data/openpilot/third_party/snpe/include/ -L/data/pythonpath/third_party/snpe/aarch64 -lSNPE benchmark.cc -o benchmark +export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$HOME/openpilot/third_party/snpe/x86_64/:$LD_LIBRARY_PATH" +exec ./benchmark $1 diff --git a/selfdrive/modeld/tests/test_modeld.py b/selfdrive/modeld/tests/test_modeld.py new file mode 100644 index 0000000000..1e19a591bf --- /dev/null +++ b/selfdrive/modeld/tests/test_modeld.py @@ -0,0 +1,101 @@ +import numpy as np +import random + +import cereal.messaging as messaging +from msgq.visionipc import VisionIpcServer, VisionStreamType +from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.car.car_helpers import write_car_param +from openpilot.system.manager.process_config import managed_processes +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state + +CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam +IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8) +IMG_BYTES = IMG.flatten().tobytes() + + +class TestModeld: + + def setup_method(self): + self.vipc_server = VisionIpcServer("camerad") + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, CAM.width, CAM.height) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, CAM.width, CAM.height) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, CAM.width, CAM.height) + self.vipc_server.start_listener() + write_car_param() + + self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry']) + self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration']) + + managed_processes['modeld'].start() + self.pm.wait_for_readers_to_update("roadCameraState", 10) + + def teardown_method(self): + managed_processes['modeld'].stop() + del self.vipc_server + + def _send_frames(self, frame_id, cams=None): + if cams is None: + cams = ('roadCameraState', 'wideRoadCameraState') + + cs = None + for cam in cams: + msg = messaging.new_message(cam) + cs = getattr(msg, cam) + cs.frameId = frame_id + cs.timestampSof = int((frame_id * DT_MDL) * 1e9) + cs.timestampEof = int(cs.timestampSof + (DT_MDL * 1e9)) + cam_meta = meta_from_camera_state(cam) + + self.pm.send(msg.which(), msg) + self.vipc_server.send(cam_meta.stream, IMG_BYTES, cs.frameId, + cs.timestampSof, cs.timestampEof) + return cs + + def _wait(self): + self.sm.update(5000) + if self.sm['modelV2'].frameId != self.sm['cameraOdometry'].frameId: + self.sm.update(1000) + + def test_modeld(self): + for n in range(1, 500): + cs = self._send_frames(n) + self._wait() + + mdl = self.sm['modelV2'] + assert mdl.frameId == n + assert mdl.frameIdExtra == n + assert mdl.timestampEof == cs.timestampEof + assert mdl.frameAge == 0 + assert mdl.frameDropPerc == 0 + + odo = self.sm['cameraOdometry'] + assert odo.frameId == n + assert odo.timestampEof == cs.timestampEof + + def test_dropped_frames(self): + """ + modeld should only run on consecutive road frames + """ + frame_id = -1 + road_frames = list() + for n in range(1, 50): + if (random.random() < 0.1) and n > 3: + cams = random.choice([(), ('wideRoadCameraState', )]) + self._send_frames(n, cams) + else: + self._send_frames(n) + road_frames.append(n) + self._wait() + + if len(road_frames) < 3 or road_frames[-1] - road_frames[-2] == 1: + frame_id = road_frames[-1] + + mdl = self.sm['modelV2'] + odo = self.sm['cameraOdometry'] + assert mdl.frameId == frame_id + assert mdl.frameIdExtra == frame_id + assert odo.frameId == frame_id + if n != frame_id: + assert not self.sm.updated['modelV2'] + assert not self.sm.updated['cameraOdometry'] diff --git a/selfdrive/modeld/tests/tf_test/build.sh b/selfdrive/modeld/tests/tf_test/build.sh new file mode 100755 index 0000000000..4e92ca0698 --- /dev/null +++ b/selfdrive/modeld/tests/tf_test/build.sh @@ -0,0 +1,2 @@ +#!/bin/bash +clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow diff --git a/selfdrive/modeld/tests/tf_test/main.cc b/selfdrive/modeld/tests/tf_test/main.cc new file mode 100644 index 0000000000..b00f7f95e8 --- /dev/null +++ b/selfdrive/modeld/tests/tf_test/main.cc @@ -0,0 +1,69 @@ +#include +#include +#include +#include "tensorflow/c/c_api.h" + +void* read_file(const char* path, size_t* out_len) { + FILE* f = fopen(path, "r"); + if (!f) { + return NULL; + } + fseek(f, 0, SEEK_END); + long f_len = ftell(f); + rewind(f); + + char* buf = (char*)calloc(f_len, 1); + assert(buf); + + size_t num_read = fread(buf, f_len, 1, f); + fclose(f); + + if (num_read != 1) { + free(buf); + return NULL; + } + + if (out_len) { + *out_len = f_len; + } + + return buf; +} + +static void DeallocateBuffer(void* data, size_t) { + free(data); +} + +int main(int argc, char* argv[]) { + TF_Buffer* buf; + TF_Graph* graph; + TF_Status* status; + char *path = argv[1]; + + // load model + { + size_t model_size; + char tmp[1024]; + snprintf(tmp, sizeof(tmp), "%s.pb", path); + printf("loading model %s\n", tmp); + uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size); + buf = TF_NewBuffer(); + buf->data = model_data; + buf->length = model_size; + buf->data_deallocator = DeallocateBuffer; + printf("loaded model of size %d\n", model_size); + } + + // import graph + status = TF_NewStatus(); + graph = TF_NewGraph(); + TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions(); + TF_GraphImportGraphDef(graph, buf, opts, status); + TF_DeleteImportGraphDefOptions(opts); + TF_DeleteBuffer(buf); + if (TF_GetCode(status) != TF_OK) { + printf("FAIL: %s\n", TF_Message(status)); + } else { + printf("SUCCESS\n"); + } +} diff --git a/selfdrive/modeld/tests/tf_test/pb_loader.py b/selfdrive/modeld/tests/tf_test/pb_loader.py new file mode 100755 index 0000000000..3e476628eb --- /dev/null +++ b/selfdrive/modeld/tests/tf_test/pb_loader.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 +import sys +import tensorflow as tf + +with open(sys.argv[1], "rb") as f: + graph_def = tf.compat.v1.GraphDef() + graph_def.ParseFromString(f.read()) + #tf.io.write_graph(graph_def, '', sys.argv[1]+".try") diff --git a/selfdrive/modeld/tests/timing/benchmark.py b/selfdrive/modeld/tests/timing/benchmark.py new file mode 100755 index 0000000000..c629ec2fff --- /dev/null +++ b/selfdrive/modeld/tests/timing/benchmark.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +# type: ignore + +import os +import time +import numpy as np + +import cereal.messaging as messaging +from openpilot.system.manager.process_config import managed_processes + + +N = int(os.getenv("N", "5")) +TIME = int(os.getenv("TIME", "30")) + +if __name__ == "__main__": + sock = messaging.sub_sock('modelV2', conflate=False, timeout=1000) + + execution_times = [] + + for _ in range(N): + os.environ['LOGPRINT'] = 'debug' + managed_processes['modeld'].start() + time.sleep(5) + + t = [] + start = time.monotonic() + while time.monotonic() - start < TIME: + msgs = messaging.drain_sock(sock, wait_for_one=True) + for m in msgs: + t.append(m.modelV2.modelExecutionTime) + + execution_times.append(np.array(t[10:]) * 1000) + managed_processes['modeld'].stop() + + print("\n\n") + print(f"ran modeld {N} times for {TIME}s each") + 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") diff --git a/selfdrive/modeld/thneed/README b/selfdrive/modeld/thneed/README new file mode 100644 index 0000000000..f3bc66d8fc --- /dev/null +++ b/selfdrive/modeld/thneed/README @@ -0,0 +1,8 @@ +thneed is an SNPE accelerator. I know SNPE is already an accelerator, but sometimes things need to go even faster.. + +It runs on the local device, and caches a single model run. Then it replays it, but fast. + +thneed slices through abstraction layers like a fish. + +You need a thneed. + diff --git a/selfdrive/modeld/thneed/thneed_pc.cc b/selfdrive/modeld/thneed/thneed_pc.cc new file mode 100644 index 0000000000..8d0037628e --- /dev/null +++ b/selfdrive/modeld/thneed/thneed_pc.cc @@ -0,0 +1,32 @@ +#include "selfdrive/modeld/thneed/thneed.h" + +#include + +#include "common/clutil.h" +#include "common/timing.h" + +Thneed::Thneed(bool do_clinit, cl_context _context) { + context = _context; + if (do_clinit) clinit(); + char *thneed_debug_env = getenv("THNEED_DEBUG"); + debug = (thneed_debug_env != NULL) ? atoi(thneed_debug_env) : 0; +} + +void Thneed::execute(float **finputs, float *foutput, bool slow) { + uint64_t tb, te; + if (debug >= 1) tb = nanos_since_boot(); + + // ****** copy inputs + copy_inputs(finputs); + + // ****** run commands + clexec(); + + // ****** copy outputs + copy_output(foutput); + + if (debug >= 1) { + te = nanos_since_boot(); + printf("model exec in %lu us\n", (te-tb)/1000); + } +} diff --git a/selfdrive/monitoring/README.md b/selfdrive/monitoring/README.md new file mode 100644 index 0000000000..2a29ea06b5 --- /dev/null +++ b/selfdrive/monitoring/README.md @@ -0,0 +1,15 @@ +# driver monitoring (DM) + +Uploading driver-facing camera footage is opt-in, but it is encouraged to opt-in to improve the DM model. You can always change your preference using the "Record and Upload Driver Camera" toggle. + +## Troubleshooting + +Before creating a bug report, go through these troubleshooting steps. + +* Ensure the driver-facing camera has a good view of the driver in normal driving positions. + * This can be checked in Settings -> Device -> Preview Driver Camera (when car is off). +* If the camera can't see the driver, the device should be re-mounted. + +## Bug report + +In order for us to look into DM bug reports, we'll need the driver-facing camera footage. If you don't normally have this enabled, simply enable the toggle for a single drive. Also ensure the "Upload Raw Logs" toggle is enabled before going for a drive. diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 579e79093f..80af7b71d0 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -2,11 +2,9 @@ import gc import cereal.messaging as messaging -from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority -from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.monitoring.driver_monitor import DriverStatus +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring def dmonitoringd_thread(): @@ -17,70 +15,32 @@ def dmonitoringd_thread(): pm = messaging.PubMaster(['driverMonitoringState']) sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll='driverStateV2') - driver_status = DriverStatus(rhd_saved=params.get_bool("IsRhdDetected")) + DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM")) - v_cruise_last = 0 - driver_engaged = False - - # 10Hz <- dmonitoringmodeld + # 20Hz <- dmonitoringmodeld while True: sm.update() if not sm.updated['driverStateV2']: + # iterate when model has new output continue - # Get interaction - if sm.updated['carState']: - v_cruise = sm['carState'].cruiseState.speed - driver_engaged = len(sm['carState'].buttonEvents) > 0 or \ - v_cruise != v_cruise_last or \ - sm['carState'].steeringPressed or \ - sm['carState'].gasPressed - v_cruise_last = v_cruise - - if sm.updated['modelV2']: - driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo) - - # Get data from dmonitoringmodeld - events = Events() - - if sm.all_checks() and len(sm['liveCalibration'].rpyCalib): - driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + valid = sm.all_checks() + if valid: + DM.run_step(sm) - # Block engaging after max number of distrations - if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ - driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION: - events.add(car.CarEvent.EventName.tooDistracted) - - # Update events from driver state - driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) - - # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks()) - dat.driverMonitoringState = { - "events": events.to_msg(), - "faceDetected": driver_status.face_detected, - "isDistracted": driver_status.driver_distracted, - "distractedType": sum(driver_status.distracted_types), - "awarenessStatus": driver_status.awareness, - "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(), - "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n, - "poseYawOffset": driver_status.pose.yaw_offseter.filtered_stat.mean(), - "poseYawValidCount": driver_status.pose.yaw_offseter.filtered_stat.n, - "stepChange": driver_status.step_change, - "awarenessActive": driver_status.awareness_active, - "awarenessPassive": driver_status.awareness_passive, - "isLowStd": driver_status.pose.low_std, - "hiStdCount": driver_status.hi_stds, - "isActiveMode": driver_status.active_monitoring_mode, - "isRHD": driver_status.wheel_on_right, - } + # publish + dat = DM.get_state_packet(valid=valid) pm.send('driverMonitoringState', dat) + # load live always-on toggle + if sm['driverStateV2'].frameId % 40 == 1: + DM.always_on = params.get_bool("AlwaysOnDM") + # save rhd virtual toggle every 5 mins if (sm['driverStateV2'].frameId % 6000 == 0 and - driver_status.wheelpos_learner.filtered_stat.n > driver_status.settings._WHEELPOS_FILTER_MIN_COUNT and - driver_status.wheel_on_right == (driver_status.wheelpos_learner.filtered_stat.M > driver_status.settings._WHEELPOS_THRESHOLD)): - params.put_bool_nonblocking("IsRhdDetected", driver_status.wheel_on_right) + DM.wheelpos_learner.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and + DM.wheel_on_right == (DM.wheelpos_learner.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)): + params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right) def main(): dmonitoringd_thread() diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/helpers.py similarity index 76% rename from selfdrive/monitoring/driver_monitor.py rename to selfdrive/monitoring/helpers.py index 2279002f35..dfb35182b2 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/helpers.py @@ -1,11 +1,13 @@ from math import atan2 from cereal import car +import cereal.messaging as messaging +from openpilot.selfdrive.controls.lib.events import Events from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter -from openpilot.common.transformations.camera import tici_d_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS EventName = car.CarEvent.EventName @@ -15,7 +17,7 @@ EventName = car.CarEvent.EventName # We recommend that you do not change these numbers from the defaults. # ****************************************************************************************** -class DRIVER_MONITOR_SETTINGS(): +class DRIVER_MONITOR_SETTINGS: def __init__(self): self._DT_DMON = DT_DMON # ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2 @@ -31,8 +33,8 @@ class DRIVER_MONITOR_SETTINGS(): self._SG_THRESHOLD = 0.9 self._BLINK_THRESHOLD = 0.865 - self._EE_THRESH11 = 0.241 - self._EE_THRESH12 = 4.7 + self._EE_THRESH11 = 0.25 + self._EE_THRESH12 = 7.5 self._EE_MAX_OFFSET1 = 0.06 self._EE_MIN_OFFSET1 = 0.025 self._EE_THRESH21 = 0.01 @@ -55,6 +57,7 @@ class DRIVER_MONITOR_SETTINGS(): self._POSESTD_THRESHOLD = 0.3 self._HI_STD_FALLBACK_TIME = int(10 / self._DT_DMON) # fall back to wheel touch if model is uncertain for 10s self._DISTRACTED_FILTER_TS = 0.25 # 0.6Hz + self._ALWAYS_ON_ALERT_MIN_SPEED = 7 self._POSE_CALIB_MIN_SPEED = 13 # 30 mph self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative @@ -70,16 +73,37 @@ class DRIVER_MONITOR_SETTINGS(): self._MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts +class DistractedType: + NOT_DISTRACTED = 0 + DISTRACTED_POSE = 1 << 0 + DISTRACTED_BLINK = 1 << 1 + DISTRACTED_E2E = 1 << 2 + +class DriverPose: + def __init__(self, max_trackable): + self.yaw = 0. + self.pitch = 0. + self.roll = 0. + self.yaw_std = 0. + self.pitch_std = 0. + self.roll_std = 0. + self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) + self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) + self.calibrated = False + self.low_std = True + self.cfactor_pitch = 1. + self.cfactor_yaw = 1. + +class DriverBlink: + def __init__(self): + self.left = 0. + self.right = 0. + # model output refers to center of undistorted+leveled image EFL = 598.0 # focal length in K -W, H = tici_d_frame_size # corrected image has same size as raw - -class DistractedType: - NOT_DISTRACTED = 0 - DISTRACTED_POSE = 1 - DISTRACTED_BLINK = 2 - DISTRACTED_E2E = 4 +cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw +W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): # the output of these angles are in device frame @@ -99,27 +123,9 @@ def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): yaw -= rpy_calib[2] return roll_net, pitch, yaw -class DriverPose(): - def __init__(self, max_trackable): - self.yaw = 0. - self.pitch = 0. - self.roll = 0. - self.yaw_std = 0. - self.pitch_std = 0. - self.roll_std = 0. - self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable) - self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable) - self.low_std = True - self.cfactor_pitch = 1. - self.cfactor_yaw = 1. - -class DriverBlink(): - def __init__(self): - self.left_blink = 0. - self.right_blink = 0. -class DriverStatus(): - def __init__(self, rhd_saved=False, settings=None): +class DriverMonitoring: + def __init__(self, rhd_saved=False, settings=None, always_on=False): if settings is None: settings = DRIVER_MONITOR_SETTINGS() # init policy settings @@ -128,7 +134,6 @@ class DriverStatus(): # init driver status self.wheelpos_learner = RunningStatFilter() self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT) - self.pose_calibrated = False self.blink = DriverBlink() self.eev1 = 0. self.eev2 = 1. @@ -137,9 +142,7 @@ class DriverStatus(): self.ee1_calibrated = False self.ee2_calibrated = False - self.awareness = 1. - self.awareness_active = 1. - self.awareness_passive = 1. + self.always_on = always_on self.distracted_types = [] self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON) @@ -156,13 +159,18 @@ class DriverStatus(): self.threshold_pre = self.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME self.threshold_prompt = self.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL / self.settings._DISTRACTED_TIME + self._reset_awareness() self._set_timers(active_monitoring=True) + self._reset_events() def _reset_awareness(self): self.awareness = 1. self.awareness_active = 1. self.awareness_passive = 1. + def _reset_events(self): + self.current_events = Events() + def _set_timers(self, active_monitoring): if self.active_monitoring_mode and self.awareness <= self.threshold_prompt: if active_monitoring: @@ -193,10 +201,21 @@ class DriverStatus(): self.step_change = self.settings._DT_DMON / self.settings._AWARENESS_TIME self.active_monitoring_mode = False + def _set_policy(self, model_data, car_speed): + bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s + k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) + bp_normal = max(min(bp / k1, 0.5),0) + self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], + [self.settings._POSE_PITCH_THRESHOLD_SLACK, + self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD + self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5], + [self.settings._POSE_YAW_THRESHOLD_SLACK, + self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD + def _get_distracted_types(self): distracted_types = [] - if not self.pose_calibrated: + if not self.pose.calibrated: pitch_error = self.pose.pitch - self.settings._PITCH_NATURAL_OFFSET yaw_error = self.pose.yaw - self.settings._YAW_NATURAL_OFFSET else: @@ -206,11 +225,11 @@ class DriverStatus(): self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET) pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit yaw_error = abs(yaw_error) - if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose_calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \ + if pitch_error > (self.settings._POSE_PITCH_THRESHOLD*self.pose.cfactor_pitch if self.pose.calibrated else self.settings._PITCH_NATURAL_THRESHOLD) or \ yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw: distracted_types.append(DistractedType.DISTRACTED_POSE) - if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD: + if (self.blink.left + self.blink.right)*0.5 > self.settings._BLINK_THRESHOLD: distracted_types.append(DistractedType.DISTRACTED_BLINK) if self.ee1_calibrated: @@ -218,27 +237,12 @@ class DriverStatus(): * self.settings._EE_THRESH12 else: ee1_dist = self.eev1 > self.settings._EE_THRESH11 - # if self.ee2_calibrated: - # ee2_dist = self.eev2 < self.ee2_offseter.filtered_stat.M * self.settings._EE_THRESH22 - # else: - # ee2_dist = self.eev2 < self.settings._EE_THRESH21 if ee1_dist: distracted_types.append(DistractedType.DISTRACTED_E2E) return distracted_types - def set_policy(self, model_data, car_speed): - bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s - k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2) - bp_normal = max(min(bp / k1, 0.5),0) - self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5], - [self.settings._POSE_PITCH_THRESHOLD_SLACK, - self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD - self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5], - [self.settings._POSE_YAW_THRESHOLD_SLACK, - self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD - - def update_states(self, driver_state, cal_rpy, car_speed, op_engaged): + def _update_states(self, driver_state, cal_rpy, car_speed, op_engaged): rhd_pred = driver_state.wheelOnRightProb # calibrates only when there's movement and either face detected if car_speed > self.settings._WHEELPOS_CALIB_MIN_SPEED and (driver_state.leftDriverData.faceProb > self.settings._FACE_THRESHOLD or @@ -266,9 +270,9 @@ class DriverStatus(): self.pose.yaw_std = driver_data.faceOrientationStd[1] model_std_max = max(self.pose.pitch_std, self.pose.yaw_std) self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD - self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ + self.blink.left = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) \ * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) - self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ + self.blink.right = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) \ * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD) self.eev1 = driver_data.notReadyProb[0] self.eev2 = driver_data.readyProb[0] @@ -287,7 +291,7 @@ class DriverStatus(): self.ee1_offseter.push_and_update(self.eev1) self.ee2_offseter.push_and_update(self.eev2) - self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ + self.pose.calibrated = self.pose.pitch_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT and \ self.pose.yaw_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT self.ee1_calibrated = self.ee1_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT self.ee2_calibrated = self.ee2_offseter.filtered_stat.n > self.settings._POSE_OFFSET_MIN_COUNT @@ -299,8 +303,19 @@ class DriverStatus(): elif self.face_detected and self.pose.low_std: self.hi_stds = 0 - def update_events(self, events, driver_engaged, ctrl_active, standstill): - if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or not ctrl_active: # reset only when on disengagement if red reached + def _update_events(self, driver_engaged, op_engaged, standstill, wrong_gear, car_speed): + self._reset_events() + # Block engaging after max number of distrations or when alert active + if self.terminal_alert_cnt >= self.settings._MAX_TERMINAL_ALERTS or \ + self.terminal_time >= self.settings._MAX_TERMINAL_DURATION or \ + self.always_on and self.awareness <= self.threshold_prompt: + self.current_events.add(EventName.tooDistracted) + + always_on_valid = self.always_on and not wrong_gear + if (driver_engaged and self.awareness > 0 and not self.active_monitoring_mode) or \ + (not always_on_valid and not op_engaged) or \ + (always_on_valid and not op_engaged and self.awareness <= 0): + # always reset on disengage with normal mode; disengage resets only on red if always on self._reset_awareness() return @@ -320,12 +335,19 @@ class DriverStatus(): if self.awareness > self.threshold_prompt: return - standstill_exemption = standstill and self.awareness - self.step_change <= self.threshold_prompt + _reaching_audible = self.awareness - self.step_change <= self.threshold_prompt + _reaching_terminal = self.awareness - self.step_change <= 0 + standstill_exemption = standstill and _reaching_audible + always_on_red_exemption = always_on_valid and not op_engaged and _reaching_terminal + always_on_lowspeed_exemption = always_on_valid and not op_engaged and car_speed < self.settings._ALWAYS_ON_ALERT_MIN_SPEED and _reaching_audible + certainly_distracted = self.driver_distraction_filter.x > 0.63 and self.driver_distracted and self.face_detected maybe_distracted = self.hi_stds > self.settings._HI_STD_FALLBACK_TIME or not self.face_detected + if certainly_distracted or maybe_distracted: - # should always be counting if distracted unless at standstill and reaching orange - if not standstill_exemption: + # should always be counting if distracted unless at standstill (lowspeed for always-on) and reaching orange + # also will not be reaching 0 if DM is active when not engaged + if not (standstill_exemption or always_on_red_exemption or always_on_lowspeed_exemption): self.awareness = max(self.awareness - self.step_change, -0.1) alert = None @@ -343,4 +365,52 @@ class DriverStatus(): alert = EventName.preDriverDistracted if self.active_monitoring_mode else EventName.preDriverUnresponsive if alert is not None: - events.add(alert) + self.current_events.add(alert) + + + def get_state_packet(self, valid=True): + # build driverMonitoringState packet + dat = messaging.new_message('driverMonitoringState', valid=valid) + dat.driverMonitoringState = { + "events": self.current_events.to_msg(), + "faceDetected": self.face_detected, + "isDistracted": self.driver_distracted, + "distractedType": sum(self.distracted_types), + "awarenessStatus": self.awareness, + "posePitchOffset": self.pose.pitch_offseter.filtered_stat.mean(), + "posePitchValidCount": self.pose.pitch_offseter.filtered_stat.n, + "poseYawOffset": self.pose.yaw_offseter.filtered_stat.mean(), + "poseYawValidCount": self.pose.yaw_offseter.filtered_stat.n, + "stepChange": self.step_change, + "awarenessActive": self.awareness_active, + "awarenessPassive": self.awareness_passive, + "isLowStd": self.pose.low_std, + "hiStdCount": self.hi_stds, + "isActiveMode": self.active_monitoring_mode, + "isRHD": self.wheel_on_right, + } + return dat + + def run_step(self, sm): + # Set strictness + self._set_policy( + model_data=sm['modelV2'], + car_speed=sm['carState'].vEgo + ) + + # Parse data from dmonitoringmodeld + self._update_states( + driver_state=sm['driverStateV2'], + cal_rpy=sm['liveCalibration'].rpyCalib, + car_speed=sm['carState'].vEgo, + op_engaged=sm['controlsState'].enabled + ) + + # Update distraction events + self._update_events( + driver_engaged=sm['carState'].steeringPressed or sm['carState'].gasPressed, + op_engaged=sm['controlsState'].enabled, + standstill=sm['carState'].standstill, + wrong_gear=sm['carState'].gearShifter in [car.CarState.GearShifter.reverse, car.CarState.GearShifter.park], + car_speed=sm['carState'].vEgo + ) diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py new file mode 100644 index 0000000000..f750437dc3 --- /dev/null +++ b/selfdrive/monitoring/test_monitoring.py @@ -0,0 +1,206 @@ +import numpy as np + +from cereal import car, log +from openpilot.common.realtime import DT_DMON +from openpilot.selfdrive.monitoring.helpers import DriverMonitoring, DRIVER_MONITOR_SETTINGS + +EventName = car.CarEvent.EventName +dm_settings = DRIVER_MONITOR_SETTINGS() + +TEST_TIMESPAN = 120 # seconds +DISTRACTED_SECONDS_TO_ORANGE = dm_settings._DISTRACTED_TIME - dm_settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + 1 +DISTRACTED_SECONDS_TO_RED = dm_settings._DISTRACTED_TIME + 1 +INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + 1 +INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1 + +def make_msg(face_detected, distracted=False, model_uncertain=False): + ds = log.DriverStateV2.new_message() + ds.leftDriverData.faceOrientation = [0., 0., 0.] + ds.leftDriverData.facePosition = [0., 0.] + ds.leftDriverData.faceProb = 1. * face_detected + ds.leftDriverData.leftEyeProb = 1. + ds.leftDriverData.rightEyeProb = 1. + ds.leftDriverData.leftBlinkProb = 1. * distracted + ds.leftDriverData.rightBlinkProb = 1. * distracted + ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain] + ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain] + # TODO: test both separately when e2e is used + ds.leftDriverData.readyProb = [0., 0., 0., 0.] + ds.leftDriverData.notReadyProb = [0., 0.] + return ds + + +# driver state from neural net, 10Hz +msg_NO_FACE_DETECTED = make_msg(False) +msg_ATTENTIVE = make_msg(True) +msg_DISTRACTED = make_msg(True, distracted=True) +msg_ATTENTIVE_UNCERTAIN = make_msg(True, model_uncertain=True) +msg_DISTRACTED_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=True) +msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN = make_msg(True, distracted=True, model_uncertain=dm_settings._POSESTD_THRESHOLD*1.5) + +# driver interaction with car +car_interaction_DETECTED = True +car_interaction_NOT_DETECTED = False + +# some common state vectors +always_no_face = [msg_NO_FACE_DETECTED] * int(TEST_TIMESPAN / DT_DMON) +always_attentive = [msg_ATTENTIVE] * int(TEST_TIMESPAN / DT_DMON) +always_distracted = [msg_DISTRACTED] * int(TEST_TIMESPAN / DT_DMON) +always_true = [True] * int(TEST_TIMESPAN / DT_DMON) +always_false = [False] * int(TEST_TIMESPAN / DT_DMON) + +class TestMonitoring: + def _run_seq(self, msgs, interaction, engaged, standstill): + DM = DriverMonitoring() + events = [] + for idx in range(len(msgs)): + DM._update_states(msgs[idx], [0, 0, 0], 0, engaged[idx]) + # cal_rpy and car_speed don't matter here + + # evaluate events at 10Hz for tests + DM._update_events(interaction[idx], engaged[idx], standstill[idx], 0, 0) + events.append(DM.current_events) + assert len(events) == len(msgs), f"got {len(events)} for {len(msgs)} driverState input msgs" + return events, DM + + def _assert_no_events(self, events): + assert all(not len(e) for e in events) + + # engaged, driver is attentive all the time + def test_fully_aware_driver(self): + events, _ = self._run_seq(always_attentive, always_false, always_true, always_false) + self._assert_no_events(events) + + # engaged, driver is distracted and does nothing + def test_fully_distracted_driver(self): + events, d_status = self._run_seq(always_distracted, always_false, always_true, always_false) + assert len(events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.preDriverDistracted + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._DISTRACTED_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((d_status.settings._DISTRACTED_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._DISTRACTED_TIME)/2))/DT_DMON)].names[0] == EventName.driverDistracted + assert isinstance(d_status.awareness, float) + + # engaged, no face detected the whole time, no action + def test_fully_invisible_driver(self): + events, d_status = self._run_seq(always_no_face, always_false, always_true, always_false) + assert len(events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL)/2/DT_DMON)]) == 0 + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PRE_TIME_TILL_TERMINAL-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == \ + EventName.preDriverUnresponsive + assert events[int((d_status.settings._AWARENESS_TIME-d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL + \ + ((d_status.settings._AWARENESS_PROMPT_TIME_TILL_TERMINAL)/2))/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((d_status.settings._AWARENESS_TIME + \ + ((TEST_TIMESPAN-10-d_status.settings._AWARENESS_TIME)/2))/DT_DMON)].names[0] == EventName.driverUnresponsive + + # engaged, down to orange, driver pays attention, back to normal; then down to orange, driver touches wheel + # - should have short orange recovery time and no green afterwards; wheel touch only recovers when paying attention + def test_normal_driver(self): + ds_vector = [msg_DISTRACTED] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_ATTENTIVE] * int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ + [msg_DISTRACTED] * int((DISTRACTED_SECONDS_TO_ORANGE+2)/DT_DMON) + \ + [msg_ATTENTIVE] * (int(TEST_TIMESPAN/DT_DMON)-int((DISTRACTED_SECONDS_TO_ORANGE*3+2)/DT_DMON)) + interaction_vector = [car_interaction_NOT_DETECTED] * int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON) + \ + [car_interaction_DETECTED] * (int(TEST_TIMESPAN/DT_DMON)-int(DISTRACTED_SECONDS_TO_ORANGE*3/DT_DMON)) + events, _ = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int(DISTRACTED_SECONDS_TO_ORANGE*1.5/DT_DMON)]) == 0 + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3-0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_ORANGE*3+0.1)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert len(events[int((DISTRACTED_SECONDS_TO_ORANGE*3+2.5)/DT_DMON)]) == 0 + + # engaged, down to orange, driver dodges camera, then comes back still distracted, down to red, \ + # driver dodges, and then touches wheel to no avail, disengages and reengages + # - orange/red alert should remain after disappearance, and only disengaging clears red + def test_biggest_comma_fan(self): + _invisible_time = 2 # seconds + ds_vector = always_distracted[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(DISTRACTED_SECONDS_TO_ORANGE/DT_DMON):int((DISTRACTED_SECONDS_TO_ORANGE+_invisible_time)/DT_DMON)] \ + = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + ds_vector[int((DISTRACTED_SECONDS_TO_RED+_invisible_time)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time)/DT_DMON)] \ + = [msg_NO_FACE_DETECTED] * int(_invisible_time/DT_DMON) + interaction_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+0.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)] \ + = [True] * int(1/DT_DMON) + op_vector[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+2.5)/DT_DMON):int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3)/DT_DMON)] \ + = [False] * int(0.5/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert events[int((DISTRACTED_SECONDS_TO_ORANGE+0.5*_invisible_time)/DT_DMON)].names[0] == EventName.promptDriverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+1.5*_invisible_time)/DT_DMON)].names[0] == EventName.driverDistracted + assert events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+1.5)/DT_DMON)].names[0] == EventName.driverDistracted + assert len(events[int((DISTRACTED_SECONDS_TO_RED+2*_invisible_time+3.5)/DT_DMON)]) == 0 + + # engaged, invisible driver, down to orange, driver touches wheel; then down to orange again, driver appears + # - both actions should clear the alert, but momentary appearance should not + def test_sometimes_transparent_commuter(self): + _visible_time = np.random.choice([0.5, 10]) + ds_vector = always_no_face[:]*2 + interaction_vector = always_false[:]*2 + ds_vector[int((2*INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON):int((2*INVISIBLE_SECONDS_TO_ORANGE+1+_visible_time)/DT_DMON)] = \ + [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_ORANGE)/DT_DMON):int((INVISIBLE_SECONDS_TO_ORANGE+1)/DT_DMON)] = [True] * int(1/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, 2*always_true, 2*always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE+0.1)/DT_DMON)]) == 0 + if _visible_time == 0.5: + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)].names[0] == EventName.preDriverUnresponsive + elif _visible_time == 10: + assert events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_ORANGE*2+1+0.1+_visible_time)/DT_DMON)]) == 0 + + # engaged, invisible driver, down to red, driver appears and then touches wheel, then disengages/reengages + # - only disengage will clear the alert + def test_last_second_responder(self): + _visible_time = 2 # seconds + ds_vector = always_no_face[:] + interaction_vector = always_false[:] + op_vector = always_true[:] + ds_vector[int(INVISIBLE_SECONDS_TO_RED/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON)] = [msg_ATTENTIVE] * int(_visible_time/DT_DMON) + interaction_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON)] = [True] * int(1/DT_DMON) + op_vector[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1)/DT_DMON):int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)] = [False] * int(0.5/DT_DMON) + events, _ = self._run_seq(ds_vector, interaction_vector, op_vector, always_false) + assert len(events[int(INVISIBLE_SECONDS_TO_ORANGE*0.5/DT_DMON)]) == 0 + assert events[int((INVISIBLE_SECONDS_TO_ORANGE-0.1)/DT_DMON)].names[0] == EventName.promptDriverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED-0.1)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+0.5*_visible_time)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+0.5)/DT_DMON)].names[0] == EventName.driverUnresponsive + assert len(events[int((INVISIBLE_SECONDS_TO_RED+_visible_time+1+0.1)/DT_DMON)]) == 0 + + # disengaged, always distracted driver + # - dm should stay quiet when not engaged + def test_pure_dashcam_user(self): + events, _ = self._run_seq(always_distracted, always_false, always_false, always_false) + assert sum(len(event) for event in events) == 0 + + # engaged, car stops at traffic light, down to orange, no action, then car starts moving + # - should only reach green when stopped, but continues counting down on launch + def test_long_traffic_light_victim(self): + _redlight_time = 60 # seconds + standstill_vector = always_true[:] + standstill_vector[int(_redlight_time/DT_DMON):] = [False] * int((TEST_TIMESPAN-_redlight_time)/DT_DMON) + events, d_status = self._run_seq(always_distracted, always_false, always_true, standstill_vector) + assert events[int((d_status.settings._DISTRACTED_TIME-d_status.settings._DISTRACTED_PRE_TIME_TILL_TERMINAL+1)/DT_DMON)].names[0] == \ + EventName.preDriverDistracted + assert events[int((_redlight_time-0.1)/DT_DMON)].names[0] == EventName.preDriverDistracted + assert events[int((_redlight_time+0.5)/DT_DMON)].names[0] == EventName.promptDriverDistracted + + # engaged, model is somehow uncertain and driver is distracted + # - should fall back to wheel touch after uncertain alert + def test_somehow_indecisive_model(self): + ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(TEST_TIMESPAN/DT_DMON) + interaction_vector = always_false[:] + events, d_status = self._run_seq(ds_vector, interaction_vector, always_true, always_false) + assert EventName.preDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names + assert EventName.promptDriverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_ORANGE-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + assert EventName.driverUnresponsive in \ + events[int((INVISIBLE_SECONDS_TO_RED-1+DT_DMON*d_status.settings._HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names + diff --git a/selfdrive/navd/README.md b/selfdrive/navd/README.md index 3047b7f8eb..81de8ae39f 100644 --- a/selfdrive/navd/README.md +++ b/selfdrive/navd/README.md @@ -8,7 +8,7 @@ This directory contains two daemons, `navd` and `mapsd`, which support navigatio ### map renderer -The map renderer listens for the `navRoute` and publishes a rendered map view over VisionIPC for the navigation model, which lives in `selfdrive/modeld/`. The rendered maps look like this: +The map renderer listens for the `navRoute` and publishes a simplified rendered map view over VisionIPC. The rendered maps look like this: ![](https://i.imgur.com/oZLfmwq.png) diff --git a/selfdrive/navd/SConscript b/selfdrive/navd/SConscript index 5a173c0351..295e8127db 100644 --- a/selfdrive/navd/SConscript +++ b/selfdrive/navd/SConscript @@ -1,8 +1,8 @@ -Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'cereal', 'transformations') +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations') map_env = qt_env.Clone() -libs = ['qt_widgets', 'qt_util', 'QMapLibre', common, messaging, cereal, visionipc, transformations, - 'zmq', 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"] +libs = ['qt_widgets', 'qt_util', 'QMapLibre', common, messaging, visionipc, transformations, + 'm', 'OpenCL', 'ssl', 'crypto', 'pthread', 'json11'] + map_env["LIBS"] if arch == 'larch64': libs.append(':libEGL_mesa.so.0') @@ -17,4 +17,4 @@ if arch in ['larch64', 'aarch64', 'x86_64']: map_env["RPATH"].append(Dir('.').abspath) map_env["LIBPATH"].append(Dir('.').abspath) maplib = map_env.SharedLibrary("maprender", ["map_renderer.cc"], LIBS=libs) - map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs) + # map_env.Program("mapsd", ["main.cc", ], LIBS=[maplib[0].get_path(), ] + libs) diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index 55c3f88a9a..0f0410c2c7 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -2,7 +2,7 @@ from __future__ import annotations import json import math -from typing import Any, Dict, List, Optional, Tuple, Union, cast +from typing import Any, cast from openpilot.common.conversions import Conversions from openpilot.common.numpy_fast import clip @@ -22,13 +22,13 @@ class Coordinate: def __init__(self, latitude: float, longitude: float) -> None: self.latitude = latitude self.longitude = longitude - self.annotations: Dict[str, float] = {} + self.annotations: dict[str, float] = {} @classmethod - def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: + def from_mapbox_tuple(cls, t: tuple[float, float]) -> Coordinate: return cls(t[1], t[0]) - def as_dict(self) -> Dict[str, float]: + def as_dict(self) -> dict[str, float]: return {'latitude': self.latitude, 'longitude': self.longitude} def __str__(self) -> str: @@ -83,7 +83,7 @@ def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate): return projection.distance_to(p) -def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> float: +def distance_along_geometry(geometry: list[Coordinate], pos: Coordinate) -> float: if len(geometry) <= 2: return geometry[0].distance_to(pos) @@ -106,7 +106,7 @@ def distance_along_geometry(geometry: List[Coordinate], pos: Coordinate) -> floa return total_distance_closest -def coordinate_from_param(param: str, params: Optional[Params] = None) -> Optional[Coordinate]: +def coordinate_from_param(param: str, params: Params = None) -> Coordinate | None: if params is None: params = Params() @@ -130,7 +130,7 @@ def string_to_direction(direction: str) -> str: return 'none' -def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: +def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float: unit = cast(str, maxspeed['unit']) speed = cast(float, maxspeed['speed']) return SPEED_CONVERSIONS[unit] * speed @@ -140,7 +140,7 @@ def field_valid(dat: dict, field: str) -> bool: return field in dat and dat[field] is not None -def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> Optional[Dict[str, Any]]: +def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None: if not len(banners): return None diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index d52ee162bd..1e57ad3e7c 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -41,6 +41,8 @@ MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : m_s QSurfaceFormat fmt; fmt.setRenderableType(QSurfaceFormat::OpenGLES); + m_settings.setMapMode(QMapLibre::Settings::MapMode::Static); + ctx = std::make_unique(); ctx->setFormat(fmt); ctx->create(); @@ -87,6 +89,18 @@ MapRenderer::MapRenderer(const QMapLibre::Settings &settings, bool online) : m_s LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str()); }); + QObject::connect(m_map.data(), &QMapLibre::Map::staticRenderFinished, [=](const QString &error) { + rendering = false; + + if (!error.isEmpty()) { + LOGE("Static map rendering failed with error: '%s'\n", error.toStdString().c_str()); + } else if (vipc_server != nullptr) { + double end_render_t = millis_since_boot(); + publish((end_render_t - start_render_t) / 1000.0, true); + last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime(); + } + }); + if (online) { vipc_server.reset(new VisionIpcServer("navd")); vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT); @@ -114,22 +128,16 @@ void MapRenderer::msgUpdate() { float bearing = RAD2DEG(orientation.getValue()[2]); updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing); - // TODO: use the static rendering mode instead - // retry render a few times - for (int i = 0; i < 5 && !rendered(); i++) { - QApplication::processEvents(QEventLoop::AllEvents, 100); + if (!rendering) { update(); - if (rendered()) { - LOGW("rendered after %d retries", i+1); - break; - } } - // fallback to sending a blank frame if (!rendered()) { publish(0, false); } } + + } if (sm->updated("navRoute")) { @@ -157,7 +165,9 @@ void MapRenderer::updatePosition(QMapLibre::Coordinate position, float bearing) m_map->setCoordinate(position); m_map->setBearing(bearing); m_map->setZoom(zoom); - update(); + if (!rendering) { + update(); + } } bool MapRenderer::loaded() { @@ -165,16 +175,10 @@ bool MapRenderer::loaded() { } void MapRenderer::update() { - double start_t = millis_since_boot(); + rendering = true; gl_functions->glClear(GL_COLOR_BUFFER_BIT); - m_map->render(); - gl_functions->glFlush(); - double end_t = millis_since_boot(); - - if ((vipc_server != nullptr) && loaded()) { - publish((end_t - start_t) / 1000.0, true); - last_llk_rendered = (*sm)["liveLocationKalman"].getLogMonoTime(); - } + start_render_t = millis_since_boot(); + m_map->startStaticRender(); } void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array &buf) { diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index fd5922b668..956c1d54bc 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -12,7 +12,7 @@ #include #include -#include "cereal/visionipc/visionipc_server.h" +#include "msgq/visionipc/visionipc_server.h" #include "cereal/messaging/messaging.h" @@ -43,8 +43,10 @@ private: void initLayers(); + double start_render_t; uint32_t frame_id = 0; uint64_t last_llk_rendered = 0; + bool rendering = false; bool rendered() { return last_llk_rendered == (*sm)["liveLocationKalman"].getLogMonoTime(); } diff --git a/selfdrive/navd/map_renderer.py b/selfdrive/navd/map_renderer.py index 8d525ac73e..e44b883436 100755 --- a/selfdrive/navd/map_renderer.py +++ b/selfdrive/navd/map_renderer.py @@ -31,7 +31,8 @@ void map_renderer_free_image(void *inst, uint8_t *buf); return ffi, ffi.dlopen(lib) -def wait_ready(lib, renderer): +def wait_ready(lib, renderer, timeout=None): + st = time.time() while not lib.map_renderer_loaded(renderer): lib.map_renderer_update(renderer) @@ -40,6 +41,9 @@ def wait_ready(lib, renderer): time.sleep(0.01) + if timeout is not None and time.time() - st > timeout: + raise TimeoutError("Timeout waiting for map renderer to be ready") + def get_image(lib, renderer): buf = lib.map_renderer_get_image(renderer) diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 2be703cdb2..8cfc495f27 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -48,15 +48,14 @@ class RouteEngine: self.reroute_counter = 0 + + self.api = None + self.mapbox_token = None if "MAPBOX_TOKEN" in os.environ: self.mapbox_token = os.environ["MAPBOX_TOKEN"] self.mapbox_host = "https://api.mapbox.com" else: - try: - self.mapbox_token = Api(self.params.get("DongleId", encoding='utf8')).get_token(expiry_hours=4 * 7 * 24) - except FileNotFoundError: - cloudlog.exception("Failed to generate mapbox token due to missing private key. Ensure device is registered.") - self.mapbox_token = "" + self.api = Api(self.params.get("DongleId", encoding='utf8')) self.mapbox_host = "https://maps.comma.ai" def update(self): @@ -122,8 +121,12 @@ class RouteEngine: if lang is not None: lang = lang.replace('main_', '') + token = self.mapbox_token + if token is None: + token = self.api.get_token() + params = { - 'access_token': self.mapbox_token, + 'access_token': token, 'annotations': 'maxspeed', 'geometries': 'geojson', 'overview': 'full', diff --git a/selfdrive/navd/tests/test_map_renderer.py b/selfdrive/navd/tests/test_map_renderer.py old mode 100755 new mode 100644 index b5f186dbb0..04363883b2 --- a/selfdrive/navd/tests/test_map_renderer.py +++ b/selfdrive/navd/tests/test_map_renderer.py @@ -1,16 +1,14 @@ -#!/usr/bin/env python3 import time import numpy as np import os import pytest -import unittest import requests import threading import http.server import cereal.messaging as messaging from typing import Any -from cereal.visionipc import VisionIpcClient, VisionStreamType +from msgq.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.mock.generators import LLK_DECIMATION, LOCATION1, LOCATION2, generate_liveLocationKalman from openpilot.selfdrive.test.helpers import with_processes @@ -65,11 +63,12 @@ class MapBoxInternetDisabledServer(threading.Thread): MapBoxInternetDisabledRequestHandler.INTERNET_ACTIVE = True -class TestMapRenderer(unittest.TestCase): +@pytest.mark.skip(reason="not used") +class TestMapRenderer: server: MapBoxInternetDisabledServer @classmethod - def setUpClass(cls): + def setup_class(cls): assert "MAPBOX_TOKEN" in os.environ cls.original_token = os.environ["MAPBOX_TOKEN"] cls.server = MapBoxInternetDisabledServer() @@ -77,10 +76,10 @@ class TestMapRenderer(unittest.TestCase): time.sleep(0.5) # wait for server to startup @classmethod - def tearDownClass(cls) -> None: + def teardown_class(cls) -> None: cls.server.stop() - def setUp(self): + def setup_method(self): self.server.enable_internet() os.environ['MAPS_HOST'] = f'http://localhost:{self.server.port}' @@ -135,7 +134,7 @@ class TestMapRenderer(unittest.TestCase): invalid_and_not_previously_valid = (expect_valid and not self.sm.valid['mapRenderState'] and not prev_valid) valid_and_not_previously_invalid = (not expect_valid and self.sm.valid['mapRenderState'] and prev_valid) - if (invalid_and_not_previously_valid or valid_and_not_previously_invalid) and frames_since_test_start < 5: + if (invalid_and_not_previously_valid or valid_and_not_previously_invalid) and frames_since_test_start < 20: continue # check output @@ -202,15 +201,12 @@ class TestMapRenderer(unittest.TestCase): def assert_stat(stat, nominal, tol=0.3): tol = (nominal / (1+tol)), (nominal * (1+tol)) - self.assertTrue(tol[0] < stat < tol[1], f"{stat} not in tolerance {tol}") + assert tol[0] < stat < tol[1], f"{stat} not in tolerance {tol}" assert_stat(_mean, 0.030) assert_stat(_median, 0.027) assert_stat(_stddev, 0.0078) - self.assertLess(_max, 0.065) - self.assertGreater(_min, 0.015) + assert _max < 0.065 + assert _min > 0.015 - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/navd/tests/test_navd.py b/selfdrive/navd/tests/test_navd.py old mode 100755 new mode 100644 index 61be6cc387..b6580acff1 --- a/selfdrive/navd/tests/test_navd.py +++ b/selfdrive/navd/tests/test_navd.py @@ -1,22 +1,20 @@ -#!/usr/bin/env python3 import json import random -import unittest import numpy as np from parameterized import parameterized import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes -class TestNavd(unittest.TestCase): - def setUp(self): +class TestNavd: + def setup_method(self): self.params = Params() self.sm = messaging.SubMaster(['navRoute', 'navInstruction']) - def tearDown(self): + def teardown_method(self): managed_processes['navd'].stop() def _check_route(self, start, end, check_coords=True): @@ -57,7 +55,3 @@ class TestNavd(unittest.TestCase): start = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)} end = {"latitude": random.uniform(-90, 90), "longitude": random.uniform(-180, 180)} self._check_route(start, end, check_coords=False) - - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/pandad/.gitignore b/selfdrive/pandad/.gitignore new file mode 100644 index 0000000000..f7226cdb87 --- /dev/null +++ b/selfdrive/pandad/.gitignore @@ -0,0 +1,3 @@ +pandad +pandad_api_impl.cpp +tests/test_pandad_usbprotocol diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript new file mode 100644 index 0000000000..63a2c1e650 --- /dev/null +++ b/selfdrive/pandad/SConscript @@ -0,0 +1,11 @@ +Import('env', 'envCython', 'common', 'messaging') + +libs = ['usb-1.0', common, messaging, 'pthread'] +panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) + +env.Program('pandad', ['main.cc', 'pandad.cc'], LIBS=[panda] + libs) +env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) + +envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) +if GetOption('extras'): + env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/boardd/boardd.py b/selfdrive/pandad/__init__.py similarity index 81% rename from selfdrive/boardd/boardd.py rename to selfdrive/pandad/__init__.py index 0cdaf5e912..8081a62dd0 100644 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/pandad/__init__.py @@ -1,5 +1,5 @@ # Cython, now uses scons to build -from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp +from openpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp assert can_list_to_can_capnp def can_capnp_to_can_list(can, src_filter=None): diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/pandad/can_list_to_can_capnp.cc similarity index 95% rename from selfdrive/boardd/can_list_to_can_capnp.cc rename to selfdrive/pandad/can_list_to_can_capnp.cc index 72ca72688a..9fc2648da2 100644 --- a/selfdrive/boardd/can_list_to_can_capnp.cc +++ b/selfdrive/pandad/can_list_to_can_capnp.cc @@ -1,5 +1,5 @@ #include "cereal/messaging/messaging.h" -#include "selfdrive/boardd/panda.h" +#include "selfdrive/pandad/panda.h" void can_list_to_can_capnp_cpp(const std::vector &can_list, std::string &out, bool sendCan, bool valid) { MessageBuilder msg; diff --git a/selfdrive/boardd/main.cc b/selfdrive/pandad/main.cc similarity index 71% rename from selfdrive/boardd/main.cc rename to selfdrive/pandad/main.cc index cb17a584bd..b63d884a45 100644 --- a/selfdrive/boardd/main.cc +++ b/selfdrive/pandad/main.cc @@ -1,22 +1,22 @@ #include -#include "selfdrive/boardd/boardd.h" +#include "selfdrive/pandad/pandad.h" #include "common/swaglog.h" #include "common/util.h" #include "system/hardware/hw.h" int main(int argc, char *argv[]) { - LOGW("starting boardd"); + LOGW("starting pandad"); if (!Hardware::PC()) { int err; err = util::set_realtime_priority(54); assert(err == 0); - err = util::set_core_affinity({4}); + err = util::set_core_affinity({3}); assert(err == 0); } std::vector serials(argv + 1, argv + argc); - boardd_main_thread(serials); + pandad_main_thread(serials); return 0; } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/pandad/panda.cc similarity index 82% rename from selfdrive/boardd/panda.cc rename to selfdrive/pandad/panda.cc index e075887a4d..a404ad3880 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/pandad/panda.cc @@ -1,4 +1,4 @@ -#include "selfdrive/boardd/panda.h" +#include "selfdrive/pandad/panda.h" #include @@ -25,10 +25,6 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { } hw_type = get_hw_type(); - has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || - (hw_type == cereal::PandaState::PandaType::DOS) || - (hw_type == cereal::PandaState::PandaType::TRES); - can_reset_communications(); return; @@ -77,41 +73,6 @@ cereal::PandaState::PandaType Panda::get_hw_type() { return (cereal::PandaState::PandaType)(hw_query[0]); } -void Panda::set_rtc(struct tm sys_time) { - // tm struct has year defined as years since 1900 - handle->control_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); - handle->control_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); - handle->control_write(0xa3, (uint16_t)sys_time.tm_mday, 0); - // handle->control_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); - handle->control_write(0xa5, (uint16_t)sys_time.tm_hour, 0); - handle->control_write(0xa6, (uint16_t)sys_time.tm_min, 0); - handle->control_write(0xa7, (uint16_t)sys_time.tm_sec, 0); -} - -struct tm Panda::get_rtc() { - struct __attribute__((packed)) timestamp_t { - uint16_t year; // Starts at 0 - uint8_t month; - uint8_t day; - uint8_t weekday; - uint8_t hour; - uint8_t minute; - uint8_t second; - } rtc_time = {0}; - - handle->control_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); - - struct tm new_time = { 0 }; - new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 1900 - new_time.tm_mon = rtc_time.month - 1; - new_time.tm_mday = rtc_time.day; - new_time.tm_hour = rtc_time.hour; - new_time.tm_min = rtc_time.minute; - new_time.tm_sec = rtc_time.second; - - return new_time; -} - void Panda::set_fan_speed(uint16_t fan_speed) { handle->control_write(0xb1, fan_speed, 0); } @@ -211,7 +172,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data for (auto cmsg : can_data_list) { // check if the message is intended for this panda uint8_t bus = cmsg.getSrc(); - if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) { + if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_OFFSET)) { continue; } auto can_data = cmsg.getDat(); @@ -259,12 +220,18 @@ bool Panda::can_receive(std::vector& out_vec) { if (!comms_healthy()) { return false; } - if (recv == RECV_SIZE) { - LOGW("Panda receive buffer full"); + + if (getenv("PANDAD_MAXOUT") != NULL) { + static uint8_t junk[RECV_SIZE]; + handle->bulk_read(0xab, junk, RECV_SIZE - recv); } - receive_buffer_size += recv; - return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); + bool ret = true; + if (recv > 0) { + receive_buffer_size += recv; + ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); + } + return ret; } void Panda::can_reset_communications() { @@ -284,6 +251,13 @@ bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector #include diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/pandad/panda_comms.h similarity index 93% rename from selfdrive/boardd/panda_comms.h rename to selfdrive/pandad/panda_comms.h index e61d25402f..9c452faf6d 100644 --- a/selfdrive/boardd/panda_comms.h +++ b/selfdrive/pandad/panda_comms.h @@ -56,6 +56,13 @@ private: }; #ifndef __APPLE__ +struct __attribute__((packed)) spi_header { + uint8_t sync; + uint8_t endpoint; + uint16_t tx_len; + uint16_t max_rx_len; +}; + class PandaSpiHandle : public PandaCommsHandle { public: PandaSpiHandle(std::string serial); @@ -78,5 +85,9 @@ private: int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout); int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); + int lltransfer(spi_ioc_transfer &t); + + spi_header header; + uint32_t xfer_count = 0; }; #endif diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/pandad/pandad.cc similarity index 90% rename from selfdrive/boardd/boardd.cc rename to selfdrive/pandad/pandad.cc index b2b59d3752..095a7893b4 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/pandad/pandad.cc @@ -1,4 +1,4 @@ -#include "selfdrive/boardd/boardd.h" +#include "selfdrive/pandad/pandad.h" #include #include @@ -25,7 +25,7 @@ // - The internal panda will always be the first panda // - Consecutive pandas will be sorted based on panda type, and then serial number // Connecting: -// - If a panda connection is dropped, boardd will reconnect to all pandas +// - If a panda connection is dropped, pandad will reconnect to all pandas // - If a panda is added, we will only reconnect when we are offroad // CAN buses: // - Each panda will have it's block of 4 buses. E.g.: the second panda will use @@ -49,12 +49,6 @@ std::atomic ignition(false); ExitHandler do_exit; -static std::string get_time_str(const struct tm &time) { - char s[30] = {'\0'}; - std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time); - return s; -} - bool check_all_connected(const std::vector &pandas) { for (const auto& panda : pandas) { if (!panda->connected()) { @@ -65,36 +59,6 @@ bool check_all_connected(const std::vector &pandas) { return true; } -enum class SyncTimeDir { TO_PANDA, FROM_PANDA }; - -void sync_time(Panda *panda, SyncTimeDir dir) { - if (!panda->has_rtc) return; - - setenv("TZ", "UTC", 1); - struct tm sys_time = util::get_time(); - struct tm rtc_time = panda->get_rtc(); - - if (dir == SyncTimeDir::TO_PANDA) { - if (util::time_valid(sys_time)) { - // Write time to RTC if it looks reasonable - double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); - if (std::abs(seconds) > 1.1) { - panda->set_rtc(sys_time); - LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s", - seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); - } - } - } else if (dir == SyncTimeDir::FROM_PANDA) { - LOGW("System time: %s, RTC time: %s", get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); - - if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) { - const struct timeval tv = {mktime(&rtc_time), 0}; - settimeofday(&tv, 0); - LOGE("System time wrong, setting from RTC."); - } - } -} - bool safety_setter_thread(std::vector pandas) { LOGD("Starting safety setter thread"); @@ -180,7 +144,7 @@ bool safety_setter_thread(std::vector pandas) { Panda *connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { - panda = std::make_unique(serial, (index * PANDA_BUS_CNT)); + panda = std::make_unique(serial, (index * PANDA_BUS_OFFSET)); } catch (std::exception &e) { return nullptr; } @@ -195,12 +159,11 @@ Panda *connect(std::string serial="", uint32_t index=0) { throw std::runtime_error("Panda firmware out of date. Run pandad.py to update."); } - sync_time(panda.get(), SyncTimeDir::FROM_PANDA); return panda.release(); } void can_send_thread(std::vector pandas, bool fake_send) { - util::set_thread_name("boardd_can_send"); + util::set_thread_name("pandad_can_send"); AlignedBuffer aligned_buf; std::unique_ptr context(Context::create()); @@ -235,12 +198,12 @@ void can_send_thread(std::vector pandas, bool fake_send) { } void can_recv_thread(std::vector pandas) { - util::set_thread_name("boardd_can_recv"); + util::set_thread_name("pandad_can_recv"); PubMaster pm({"can"}); // run at 100Hz - RateKeeper rk("boardd_can_recv", 100); + RateKeeper rk("pandad_can_recv", 100); std::vector raw_can_data; while (!do_exit && check_all_connected(pandas)) { @@ -353,7 +316,6 @@ std::optional send_panda_states(PubMaster *pm, const std::vector ps.setControlsAllowed(health.controls_allowed_pkt); ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); - ps.setGmlanSendErrs(health.gmlan_send_errs_pkt); ps.setPandaType(panda->hw_type); ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); ps.setSafetyParam(health.safety_param_pkt); @@ -443,7 +405,7 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { } void panda_state_thread(std::vector pandas, bool spoofing_started) { - util::set_thread_name("boardd_panda_state"); + util::set_thread_name("pandad_panda_state"); Params params; SubMaster sm({"controlsState"}); @@ -533,7 +495,7 @@ void panda_state_thread(std::vector pandas, bool spoofing_started) { void peripheral_control_thread(Panda *panda, bool no_fan_control) { - util::set_thread_name("boardd_peripheral_control"); + util::set_thread_name("pandad_peripheral_control"); SubMaster sm({"deviceState", "driverCameraState"}); @@ -581,16 +543,11 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { panda->set_ir_pwr(ir_pwr); prev_ir_pwr = ir_pwr; } - - // Write to rtc once per minute when no ignition present - if (!ignition && (sm.frame % 120 == 1)) { - sync_time(panda, SyncTimeDir::TO_PANDA); - } } } -void boardd_main_thread(std::vector serials) { - LOGW("launching boardd"); +void pandad_main_thread(std::vector serials) { + LOGW("launching pandad"); if (serials.size() == 0) { serials = Panda::list(); diff --git a/selfdrive/boardd/boardd.h b/selfdrive/pandad/pandad.h similarity index 53% rename from selfdrive/boardd/boardd.h rename to selfdrive/pandad/pandad.h index 0646fc6189..9d35949a8f 100644 --- a/selfdrive/boardd/boardd.h +++ b/selfdrive/pandad/pandad.h @@ -3,7 +3,7 @@ #include #include -#include "selfdrive/boardd/panda.h" +#include "selfdrive/pandad/panda.h" bool safety_setter_thread(std::vector pandas); -void boardd_main_thread(std::vector serials); +void pandad_main_thread(std::vector serials); diff --git a/selfdrive/boardd/pandad.py b/selfdrive/pandad/pandad.py similarity index 82% rename from selfdrive/boardd/pandad.py rename to selfdrive/pandad/pandad.py index a87613c803..12accdbf5e 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -1,16 +1,14 @@ #!/usr/bin/env python3 -# simple boardd wrapper that updates the panda first +# simple pandad wrapper that updates the panda first import os import usb1 import time import subprocess -from typing import List, NoReturn -from functools import cmp_to_key +from typing import NoReturn from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params -from openpilot.selfdrive.boardd.set_time import set_time from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog @@ -63,24 +61,6 @@ def flash_panda(panda_serial: str) -> Panda: return panda -def panda_sort_cmp(a: Panda, b: Panda): - a_type = a.get_type() - b_type = b.get_type() - - # make sure the internal one is always first - if a.is_internal() and not b.is_internal(): - return -1 - if not a.is_internal() and b.is_internal(): - return 1 - - # sort by hardware type - if a_type != b_type: - return a_type < b_type - - # last resort: sort by serial number - return a.get_usb_serial() < b.get_usb_serial() - - def main() -> NoReturn: count = 0 first_run = True @@ -124,7 +104,7 @@ def main() -> NoReturn: cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}") # Flash pandas - pandas: List[Panda] = [] + pandas: list[Panda] = [] for serial in panda_serials: pandas.append(flash_panda(serial)) @@ -137,7 +117,10 @@ def main() -> NoReturn: no_internal_panda_count = 0 # sort pandas to have deterministic order - pandas.sort(key=cmp_to_key(panda_sort_cmp)) + # * the internal one is always first + # * then sort by hardware type + # * as a last resort, sort by serial number + pandas.sort(key=lambda x: (not x.is_internal(), x.get_type(), x.get_usb_serial())) panda_serials = [p.get_usb_serial() for p in pandas] # log panda fw versions @@ -154,16 +137,9 @@ def main() -> NoReturn: cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial()) if first_run: - if panda.is_internal(): - # update time from RTC - set_time(cloudlog) - # reset panda to ensure we're in a good state cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - if panda.is_internal(): - HARDWARE.reset_internal_panda() - else: - panda.reset(reconnect=False) + panda.reset(reconnect=True) for p in pandas: p.close() @@ -181,10 +157,10 @@ def main() -> NoReturn: first_run = False - # run boardd with all connected serials as arguments - os.environ['MANAGER_DAEMON'] = 'boardd' - os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) - subprocess.run(["./boardd", *panda_serials], check=True) + # run pandad with all connected serials as arguments + os.environ['MANAGER_DAEMON'] = 'pandad' + os.chdir(os.path.join(BASEDIR, "selfdrive/pandad")) + subprocess.run(["./pandad", *panda_serials], check=True) if __name__ == "__main__": main() diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/pandad/pandad_api_impl.pyx similarity index 93% rename from selfdrive/boardd/boardd_api_impl.pyx rename to selfdrive/pandad/pandad_api_impl.pyx index 6a552bb447..cbac8cc5a3 100644 --- a/selfdrive/boardd/boardd_api_impl.pyx +++ b/selfdrive/pandad/pandad_api_impl.pyx @@ -15,16 +15,17 @@ cdef extern from "can_list_to_can_capnp.cc": void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid) def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): + cdef can_frame *f cdef vector[can_frame] can_list - can_list.reserve(len(can_msgs)) - cdef can_frame f + can_list.reserve(len(can_msgs)) for can_msg in can_msgs: + f = &(can_list.emplace_back()) f.address = can_msg[0] f.busTime = can_msg[1] f.dat = can_msg[2] f.src = can_msg[3] - can_list.push_back(f) + cdef string out can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid) return out diff --git a/selfdrive/boardd/spi.cc b/selfdrive/pandad/spi.cc similarity index 74% rename from selfdrive/boardd/spi.cc rename to selfdrive/pandad/spi.cc index d11e955c49..8f1e29689b 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/pandad/spi.cc @@ -13,7 +13,7 @@ #include "common/timing.h" #include "common/swaglog.h" #include "panda/board/comms_definitions.h" -#include "selfdrive/boardd/panda_comms.h" +#include "selfdrive/pandad/panda_comms.h" #define SPI_SYNC 0x5AU @@ -28,13 +28,6 @@ enum SpiError { ACK_TIMEOUT = -3, }; -struct __attribute__((packed)) spi_header { - uint8_t sync; - uint8_t endpoint; - uint16_t tx_len; - uint16_t max_rx_len; -}; - const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds const std::string SPI_DEVICE = "/dev/spidev0.0"; @@ -55,6 +48,12 @@ private: std::recursive_mutex &m; }; +#define SPILOG(fn, fmt, ...) do { \ + fn(fmt, ## __VA_ARGS__); \ + fn(" %d / 0x%x / %d / %d / tx: %s", \ + xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \ + util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \ + } while (0) PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { int ret; @@ -178,7 +177,7 @@ int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t t } if (d < 0) { - LOGE("SPI: bulk transfer failed with %d", d); + SPILOG(LOGE, "SPI: bulk transfer failed with %d", d); comms_healthy = false; return d; } @@ -240,6 +239,7 @@ int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint1 // due to full TX buffers nack_count += 1; if (nack_count > 3) { + SPILOG(LOGE, "NACK sleep %d", nack_count); usleep(std::clamp(nack_count*10, 200, 2000)); } } @@ -247,7 +247,7 @@ int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint1 } while (ret < 0 && connected && !timed_out); if (ret < 0) { - LOGE("transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time); + SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time); } return ret; @@ -258,32 +258,32 @@ int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, if (timeout == 0) { timeout = SPI_ACK_TIMEOUT; } - timeout = std::clamp(timeout, 100U, SPI_ACK_TIMEOUT); + timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT); spi_ioc_transfer transfer = { .tx_buf = (uint64_t)tx_buf, .rx_buf = (uint64_t)rx_buf, - .len = length + .len = length, }; - tx_buf[0] = tx; + memset(tx_buf, tx, length); while (true) { - int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + int ret = lltransfer(transfer); if (ret < 0) { - LOGE("SPI: failed to send ACK request"); + SPILOG(LOGE, "SPI: failed to send ACK request"); return ret; } if (rx_buf[0] == ack) { break; } else if (rx_buf[0] == SPI_NACK) { - LOGD("SPI: got NACK"); + SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack); return SpiError::NACK; } // handle timeout if (millis_since_boot() - start_millis > timeout) { - LOGD("SPI: timed out waiting for ACK"); + SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack); return SpiError::ACK_TIMEOUT; } } @@ -291,6 +291,40 @@ int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, return 0; } +int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) { + static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1")); + + if (err_prob > 0) { + if ((static_cast(rand()) / RAND_MAX) < err_prob) { + printf("transfer len error\n"); + t.len = rand() % SPI_BUF_SIZE; + } + if ((static_cast(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) { + printf("corrupting TX\n"); + for (int i = 0; i < t.len; i++) { + if ((static_cast(rand()) / RAND_MAX) > 0.9) { + ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256); + } + } + } + } + + int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t); + + if (err_prob > 0) { + if ((static_cast(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) { + printf("corrupting RX\n"); + for (int i = 0; i < t.len; i++) { + if ((static_cast(rand()) / RAND_MAX) > 0.9) { + ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256); + } + } + } + } + + return ret; +} + int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) { int ret; uint16_t rx_data_len; @@ -300,7 +334,8 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx assert(tx_len < SPI_BUF_SIZE); assert(max_rx_len < SPI_BUF_SIZE); - spi_header header = { + xfer_count++; + header = { .sync = SPI_SYNC, .endpoint = endpoint, .tx_len = tx_len, @@ -316,16 +351,16 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx memcpy(tx_buf, &header, sizeof(header)); add_checksum(tx_buf, sizeof(header)); transfer.len = sizeof(header) + 1; - ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + ret = lltransfer(transfer); if (ret < 0) { - LOGE("SPI: failed to send header"); - goto transfer_fail; + SPILOG(LOGE, "SPI: failed to send header"); + goto fail; } // Wait for (N)ACK ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1); if (ret < 0) { - goto transfer_fail; + goto fail; } // Send data @@ -334,35 +369,35 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } add_checksum(tx_buf, tx_len); transfer.len = tx_len + 1; - ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + ret = lltransfer(transfer); if (ret < 0) { - LOGE("SPI: failed to send data"); - goto transfer_fail; + SPILOG(LOGE, "SPI: failed to send data"); + goto fail; } // Wait for (N)ACK ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3); if (ret < 0) { - goto transfer_fail; + goto fail; } // Read data rx_data_len = *(uint16_t *)(rx_buf+1); if (rx_data_len >= SPI_BUF_SIZE) { - LOGE("SPI: RX data len larger than buf size %d", rx_data_len); - goto transfer_fail; + SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len); + goto fail; } transfer.len = rx_data_len + 1; transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1); - ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer); + ret = lltransfer(transfer); if (ret < 0) { - LOGE("SPI: failed to read rx data"); - goto transfer_fail; + SPILOG(LOGE, "SPI: failed to read rx data"); + goto fail; } if (!check_checksum(rx_buf, rx_data_len + 4)) { - LOGE("SPI: bad checksum"); - goto transfer_fail; + SPILOG(LOGE, "SPI: bad checksum"); + goto fail; } if (rx_data != NULL) { @@ -371,7 +406,19 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx return rx_data_len; -transfer_fail: +fail: + // ensure slave is in a consistent state + // and ready for the next transfer + int nack_cnt = 0; + while (nack_cnt < 3) { + if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) { + nack_cnt += 1; + } else { + nack_cnt = 0; + } + } + + if (ret > 0) ret = -1; return ret; } #endif diff --git a/selfdrive/pandad/tests/__init__.py b/selfdrive/pandad/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/pandad/tests/bootstub.panda.bin b/selfdrive/pandad/tests/bootstub.panda.bin new file mode 100755 index 0000000000..43db537061 Binary files /dev/null and b/selfdrive/pandad/tests/bootstub.panda.bin differ diff --git a/selfdrive/pandad/tests/bootstub.panda_h7.bin b/selfdrive/pandad/tests/bootstub.panda_h7.bin new file mode 100755 index 0000000000..1d9445004e Binary files /dev/null and b/selfdrive/pandad/tests/bootstub.panda_h7.bin differ diff --git a/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin b/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin new file mode 100755 index 0000000000..5cf2fa4519 Binary files /dev/null and b/selfdrive/pandad/tests/bootstub.panda_h7_spiv0.bin differ diff --git a/selfdrive/pandad/tests/test_pandad.py b/selfdrive/pandad/tests/test_pandad.py new file mode 100644 index 0000000000..467c7f04c9 --- /dev/null +++ b/selfdrive/pandad/tests/test_pandad.py @@ -0,0 +1,128 @@ +import os +import pytest +import time + +import cereal.messaging as messaging +from cereal import log +from openpilot.common.gpio import gpio_set, gpio_init +from panda import Panda, PandaDFU, PandaProtocolMismatch +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.hardware import HARDWARE +from openpilot.system.hardware.tici.pins import GPIO + +HERE = os.path.dirname(os.path.realpath(__file__)) + + +@pytest.mark.tici +class TestPandad: + + def setup_method(self): + # ensure panda is up + if len(Panda.list()) == 0: + self._run_test(60) + + self.spi = HARDWARE.get_device_type() != 'tici' + + def teardown_method(self): + managed_processes['pandad'].stop() + + def _run_test(self, timeout=30) -> float: + st = time.monotonic() + sm = messaging.SubMaster(['pandaStates']) + + managed_processes['pandad'].start() + while (time.monotonic() - st) < timeout: + sm.update(100) + if len(sm['pandaStates']) and sm['pandaStates'][0].pandaType != log.PandaState.PandaType.unknown: + break + dt = time.monotonic() - st + managed_processes['pandad'].stop() + + if len(sm['pandaStates']) == 0 or sm['pandaStates'][0].pandaType == log.PandaState.PandaType.unknown: + raise Exception("pandad failed to start") + + return dt + + def _go_to_dfu(self): + HARDWARE.recover_internal_panda() + assert Panda.wait_for_dfu(None, 10) + + def _assert_no_panda(self): + assert not Panda.wait_for_dfu(None, 3) + assert not Panda.wait_for_panda(None, 3) + + def _flash_bootstub_and_test(self, fn, expect_mismatch=False): + self._go_to_dfu() + pd = PandaDFU(None) + if fn is None: + fn = os.path.join(HERE, pd.get_mcu_type().config.bootstub_fn) + with open(fn, "rb") as f: + pd.program_bootstub(f.read()) + pd.reset() + HARDWARE.reset_internal_panda() + + assert Panda.wait_for_panda(None, 10) + if expect_mismatch: + with pytest.raises(PandaProtocolMismatch): + Panda() + else: + with Panda() as p: + assert p.bootstub + + self._run_test(45) + + def test_in_dfu(self): + HARDWARE.recover_internal_panda() + self._run_test(60) + + def test_in_bootstub(self): + with Panda() as p: + p.reset(enter_bootstub=True) + assert p.bootstub + self._run_test() + + def test_internal_panda_reset(self): + gpio_init(GPIO.STM_RST_N, True) + gpio_set(GPIO.STM_RST_N, 1) + time.sleep(0.5) + assert all(not Panda(s).is_internal() for s in Panda.list()) + self._run_test() + + assert any(Panda(s).is_internal() for s in Panda.list()) + + def test_best_case_startup_time(self): + # run once so we're up to date + self._run_test(60) + + ts = [] + for _ in range(10): + # should be nearly instant this time + dt = self._run_test(5) + ts.append(dt) + + # 5s for USB (due to enumeration) + # - 0.2s pandad -> pandad + # - plus some buffer + assert 0.1 < (sum(ts)/len(ts)) < (0.5 if self.spi else 5.0) + print("startup times", ts, sum(ts) / len(ts)) + + + def test_protocol_version_check(self): + if not self.spi: + pytest.skip("SPI test") + # flash old fw + fn = os.path.join(HERE, "bootstub.panda_h7_spiv0.bin") + self._flash_bootstub_and_test(fn, expect_mismatch=True) + + def test_release_to_devel_bootstub(self): + self._flash_bootstub_and_test(None) + + def test_recover_from_bad_bootstub(self): + self._go_to_dfu() + with PandaDFU(None) as pd: + pd.program_bootstub(b"\x00"*1024) + pd.reset() + HARDWARE.reset_internal_panda() + self._assert_no_panda() + + self._run_test(60) diff --git a/selfdrive/pandad/tests/test_pandad_loopback.py b/selfdrive/pandad/tests/test_pandad_loopback.py new file mode 100644 index 0000000000..d2b6d047d5 --- /dev/null +++ b/selfdrive/pandad/tests/test_pandad_loopback.py @@ -0,0 +1,114 @@ +import os +import copy +import random +import time +import pytest +from collections import defaultdict +from pprint import pprint + +import cereal.messaging as messaging +from cereal import car, log +from openpilot.common.retry import retry +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.selfdrive.pandad import can_list_to_can_capnp +from openpilot.selfdrive.car import make_can_msg +from openpilot.system.hardware import TICI +from openpilot.selfdrive.test.helpers import phone_only, with_processes + + +@retry(attempts=3) +def setup_pandad(num_pandas): + params = Params() + params.clear_all() + params.put_bool("IsOnroad", False) + + sm = messaging.SubMaster(['pandaStates']) + with Timeout(90, "pandad didn't start"): + while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ + any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): + sm.update(1000) + + found_pandas = len(sm['pandaStates']) + assert num_pandas == found_pandas, "connected pandas ({found_pandas}) doesn't match expected panda count ({num_pandas}). \ + connect another panda for multipanda tests." + + # pandad safety setting relies on these params + cp = car.CarParams.new_message() + + safety_config = car.CarParams.SafetyConfig.new_message() + safety_config.safetyModel = car.CarParams.SafetyModel.allOutput + cp.safetyConfigs = [safety_config]*num_pandas + + params.put_bool("IsOnroad", True) + params.put_bool("FirmwareQueryDone", True) + params.put_bool("ControlsReady", True) + params.put("CarParams", cp.to_bytes()) + + with Timeout(90, "pandad didn't set safety mode"): + while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']): + sm.update(1000) + +def send_random_can_messages(sendcan, count, num_pandas=1): + sent_msgs = defaultdict(set) + for _ in range(count): + to_send = [] + for __ in range(random.randrange(20)): + bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) + addr = random.randrange(1, 1<<29) + dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) + if (addr, dat) in sent_msgs[bus]: + continue + sent_msgs[bus].add((addr, dat)) + to_send.append(make_can_msg(addr, dat, bus)) + sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) + return sent_msgs + + +@pytest.mark.tici +class TestBoarddLoopback: + @classmethod + def setup_class(cls): + os.environ['STARTED'] = '1' + os.environ['BOARDD_LOOPBACK'] = '1' + + @phone_only + @with_processes(['pandad']) + def test_loopback(self): + num_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1 + setup_pandad(num_pandas) + + sendcan = messaging.pub_sock('sendcan') + can = messaging.sub_sock('can', conflate=False, timeout=100) + sm = messaging.SubMaster(['pandaStates']) + time.sleep(1) + + n = 200 + for i in range(n): + print(f"pandad loopback {i}/{n}") + + sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100), num_pandas) + + sent_loopback = copy.deepcopy(sent_msgs) + sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()}) + sent_total = {k: len(v) for k, v in sent_loopback.items()} + for _ in range(100 * 5): + sm.update(0) + recvd = messaging.drain_sock(can, wait_for_one=True) + for msg in recvd: + for m in msg.can: + key = (m.address, m.dat) + assert key in sent_loopback[m.src], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}" + sent_loopback[m.src].discard(key) + + if all(len(v) == 0 for v in sent_loopback.values()): + break + + # if a set isn't empty, messages got dropped + pprint(sent_msgs) + pprint(sent_loopback) + print({k: len(x) for k, x in sent_loopback.items()}) + print(sum([len(x) for x in sent_loopback.values()])) + pprint(sm['pandaStates']) # may drop messages due to RX buffer overflow + for bus in sent_loopback.keys(): + assert not len(sent_loopback[bus]), f"loop {i}: bus {bus} missing {len(sent_loopback[bus])} out of {sent_total[bus]} messages" diff --git a/selfdrive/pandad/tests/test_pandad_spi.py b/selfdrive/pandad/tests/test_pandad_spi.py new file mode 100644 index 0000000000..11e20e72cc --- /dev/null +++ b/selfdrive/pandad/tests/test_pandad_spi.py @@ -0,0 +1,106 @@ +import os +import time +import numpy as np +import pytest +import random + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.system.hardware import HARDWARE +from openpilot.selfdrive.test.helpers import phone_only, with_processes +from openpilot.selfdrive.pandad.tests.test_pandad_loopback import setup_pandad, send_random_can_messages + +JUNGLE_SPAM = "JUNGLE_SPAM" in os.environ + +@pytest.mark.tici +class TestBoarddSpi: + @classmethod + def setup_class(cls): + if HARDWARE.get_device_type() == 'tici': + pytest.skip("only for spi pandas") + os.environ['STARTED'] = '1' + os.environ['SPI_ERR_PROB'] = '0.001' + if not JUNGLE_SPAM: + os.environ['BOARDD_LOOPBACK'] = '1' + + @phone_only + @with_processes(['pandad']) + def test_spi_corruption(self, subtests): + setup_pandad(1) + + sendcan = messaging.pub_sock('sendcan') + socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')} + time.sleep(2) + for s in socks.values(): + messaging.drain_sock_raw(s) + + total_recv_count = 0 + total_sent_count = 0 + sent_msgs = {bus: list() for bus in range(3)} + + st = time.monotonic() + ts = {s: list() for s in socks.keys()} + for _ in range(int(os.getenv("TEST_TIME", "20"))): + # send some CAN messages + if not JUNGLE_SPAM: + sent = send_random_can_messages(sendcan, random.randrange(2, 20)) + for k, v in sent.items(): + sent_msgs[k].extend(list(v)) + total_sent_count += len(v) + + for service, sock in socks.items(): + for m in messaging.drain_sock(sock): + ts[service].append(m.logMonoTime) + + # sanity check for corruption + assert m.valid or (service == "can") + if service == "can": + for msg in m.can: + if JUNGLE_SPAM: + # PandaJungle.set_generated_can(True) + i = msg.address - 0x200 + assert msg.address >= 0x200 + assert msg.src == (i%3) + assert msg.dat == b"\xff"*(i%8) + total_recv_count += 1 + continue + + if msg.src > 4: + continue + key = (msg.address, msg.dat) + assert key in sent_msgs[msg.src], f"got unexpected msg: {msg.src=} {msg.address=} {msg.dat=}" + # TODO: enable this + #sent_msgs[msg.src].remove(key) + total_recv_count += 1 + elif service == "pandaStates": + assert len(m.pandaStates) == 1 + ps = m.pandaStates[0] + assert ps.uptime < 1000 + assert ps.pandaType == "tres" + assert ps.ignitionLine + assert not ps.ignitionCan + assert 4000 < ps.voltage < 14000 + elif service == "peripheralState": + ps = m.peripheralState + assert ps.pandaType == "tres" + assert 4000 < ps.voltage < 14000 + assert 100 < ps.current < 1000 + assert ps.fanSpeedRpm < 8000 + + time.sleep(0.5) + et = time.monotonic() - st + + print("\n======== timing report ========") + for service, times in ts.items(): + dts = np.diff(times)/1e6 + print(service.ljust(17), f"{np.mean(dts):7.2f} {np.min(dts):7.2f} {np.max(dts):7.2f}") + with subtests.test(msg="timing check", service=service): + edt = 1e3 / SERVICE_LIST[service].frequency + assert edt*0.9 < np.mean(dts) < edt*1.1 + assert np.max(dts) < edt*8 + assert np.min(dts) < edt + assert len(dts) >= ((et-0.5)*SERVICE_LIST[service].frequency*0.8) + + with subtests.test(msg="CAN traffic"): + print(f"Sent {total_sent_count} CAN messages, got {total_recv_count} back. {total_recv_count/(total_sent_count+1e-4):.2%} received") + assert total_recv_count > 20 diff --git a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc b/selfdrive/pandad/tests/test_pandad_usbprotocol.cc new file mode 100644 index 0000000000..11f7184efd --- /dev/null +++ b/selfdrive/pandad/tests/test_pandad_usbprotocol.cc @@ -0,0 +1,135 @@ +#define CATCH_CONFIG_MAIN +#define CATCH_CONFIG_ENABLE_BENCHMARKING + +#include "catch2/catch.hpp" +#include "cereal/messaging/messaging.h" +#include "common/util.h" +#include "selfdrive/pandad/panda.h" + +struct PandaTest : public Panda { + PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type); + void test_can_send(); + void test_can_recv(uint32_t chunk_size = 0); + void test_chunked_can_recv(); + + std::map test_data; + int can_list_size = 0; + int total_pakets_size = 0; + MessageBuilder msg; + capnp::List::Reader can_data_list; +}; + +PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) { + this->hw_type = hw_type; + int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8); + // prepare test data + for (int i = 0; i < data_limit; ++i) { + std::random_device rd; + std::independent_bits_engine rbe(rd()); + + int data_len = dlc_to_len[i]; + std::string bytes(data_len, '\0'); + std::generate(bytes.begin(), bytes.end(), std::ref(rbe)); + test_data[data_len] = bytes; + } + + // generate can messages for this panda + 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 = 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(util::random_int(0, 2) + bus_offset); + can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); + total_pakets_size += sizeof(can_header) + dat.size(); + } + + can_data_list = can_list.asReader(); + INFO("test " << can_list_size << " packets, total size " << total_pakets_size); +} + +void PandaTest::test_can_send() { + std::vector unpacked_data; + this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { + unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]); + }); + REQUIRE(unpacked_data.size() == total_pakets_size); + + int cnt = 0; + INFO("test can message integrity"); + for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) { + can_header header; + memcpy(&header, &unpacked_data[pos], sizeof(can_header)); + const uint8_t data_len = dlc_to_len[header.data_len_code]; + pckt_len = sizeof(can_header) + data_len; + + REQUIRE(header.addr == cnt); + REQUIRE(test_data.find(data_len) != test_data.end()); + const std::string &dat = test_data[data_len]; + REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0); + ++cnt; + } + REQUIRE(cnt == can_list_size); +} + +void PandaTest::test_can_recv(uint32_t rx_chunk_size) { + std::vector frames; + this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) { + if (rx_chunk_size == 0) { + REQUIRE(this->unpack_can_buffer(data, size, frames)); + } else { + this->receive_buffer_size = 0; + uint32_t pos = 0; + + while (pos < size) { + uint32_t chunk_size = std::min(rx_chunk_size, size - pos); + memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size); + this->receive_buffer_size += chunk_size; + pos += chunk_size; + + REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames)); + } + } + }); + + REQUIRE(frames.size() == can_list_size); + for (int i = 0; i < frames.size(); ++i) { + REQUIRE(frames[i].address == i); + REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end()); + const std::string &dat = test_data[frames[i].dat.size()]; + REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0); + } +} + +TEST_CASE("send/recv CAN 2.0 packets") { + auto bus_offset = GENERATE(0, 4); + auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); + PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS); + + SECTION("can_send") { + test.test_can_send(); + } + SECTION("can_receive") { + test.test_can_recv(); + } + SECTION("chunked_can_receive") { + test.test_can_recv(0x40); + } +} + +TEST_CASE("send/recv CAN FD packets") { + auto bus_offset = GENERATE(0, 4); + auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); + PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA); + + SECTION("can_send") { + test.test_can_send(); + } + SECTION("can_receive") { + test.test_can_recv(); + } + SECTION("chunked_can_receive") { + test.test_can_recv(0x40); + } +} diff --git a/selfdrive/test/.gitignore b/selfdrive/test/.gitignore new file mode 100644 index 0000000000..5801faadf4 --- /dev/null +++ b/selfdrive/test/.gitignore @@ -0,0 +1,9 @@ +out/ +docker_out/ + +process_replay/diff.txt +process_replay/model_diff.txt +valgrind_logs.txt + +*.bz2 +*.hevc diff --git a/selfdrive/test/ci_shell.sh b/selfdrive/test/ci_shell.sh new file mode 100755 index 0000000000..a5ff714b2d --- /dev/null +++ b/selfdrive/test/ci_shell.sh @@ -0,0 +1,19 @@ +#!/bin/bash -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" +OP_ROOT="$DIR/../../" + +if [ -z "$BUILD" ]; then + docker pull ghcr.io/commaai/openpilot-base:latest +else + docker build --cache-from ghcr.io/commaai/openpilot-base:latest -t ghcr.io/commaai/openpilot-base:latest -f $OP_ROOT/Dockerfile.openpilot_base . +fi + +docker run \ + -it \ + --rm \ + --volume $OP_ROOT:$OP_ROOT \ + --workdir $PWD \ + --env PYTHONPATH=$OP_ROOT \ + ghcr.io/commaai/openpilot-base:latest \ + /bin/bash diff --git a/selfdrive/test/ciui.py b/selfdrive/test/ciui.py new file mode 100755 index 0000000000..f3b0c1a98f --- /dev/null +++ b/selfdrive/test/ciui.py @@ -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 +from PyQt5.QtWidgets import QApplication, QWidget, QVBoxLayout, QLabel +from openpilot.selfdrive.ui.qt.python_helpers import set_main_window + +class Window(QWidget): + def __init__(self, parent=None): + super().__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, capture_output=True, + shell=True, check=False, encoding='utf8').stdout + label.setText(out.strip()) + +if __name__ == "__main__": + app = QApplication([]) + w = Window() + set_main_window(w) + app.exec_() diff --git a/selfdrive/test/cpp_harness.py b/selfdrive/test/cpp_harness.py new file mode 100755 index 0000000000..f9d3e681a5 --- /dev/null +++ b/selfdrive/test/cpp_harness.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +import subprocess +import sys + +from openpilot.common.prefix import OpenpilotPrefix + + +with OpenpilotPrefix(): + ret = subprocess.call(sys.argv[1:]) + +exit(ret) diff --git a/selfdrive/test/docker_build.sh b/selfdrive/test/docker_build.sh new file mode 100755 index 0000000000..4d58a1507c --- /dev/null +++ b/selfdrive/test/docker_build.sh @@ -0,0 +1,26 @@ +#!/usr/bin/env bash +set -e + +# To build sim and docs, you can run the following to mount the scons cache to the same place as in CI: +# mkdir -p .ci_cache/scons_cache +# sudo mount --bind /tmp/scons_cache/ .ci_cache/scons_cache + +SCRIPT_DIR=$(dirname "$0") +OPENPILOT_DIR=$SCRIPT_DIR/../../ +if [ -n "$TARGET_ARCHITECTURE" ]; then + PLATFORM="linux/$TARGET_ARCHITECTURE" + TAG_SUFFIX="-$TARGET_ARCHITECTURE" +else + PLATFORM="linux/$(uname -m)" + TAG_SUFFIX="" +fi + +source $SCRIPT_DIR/docker_common.sh $1 "$TAG_SUFFIX" + +DOCKER_BUILDKIT=1 docker buildx build --provenance false --pull --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $DOCKER_IMAGE:latest -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR + +if [ -n "$PUSH_IMAGE" ]; then + docker push $REMOTE_TAG + docker tag $REMOTE_TAG $REMOTE_SHA_TAG + docker push $REMOTE_SHA_TAG +fi diff --git a/selfdrive/test/docker_common.sh b/selfdrive/test/docker_common.sh new file mode 100644 index 0000000000..2887fff74b --- /dev/null +++ b/selfdrive/test/docker_common.sh @@ -0,0 +1,18 @@ +if [ "$1" = "base" ]; then + export DOCKER_IMAGE=openpilot-base + export DOCKER_FILE=Dockerfile.openpilot_base +elif [ "$1" = "prebuilt" ]; then + export DOCKER_IMAGE=openpilot-prebuilt + export DOCKER_FILE=Dockerfile.openpilot +else + echo "Invalid docker build image: '$1'" + exit 1 +fi + +export DOCKER_REGISTRY=ghcr.io/commaai +export COMMIT_SHA=$(git rev-parse HEAD) + +TAG_SUFFIX=$2 +LOCAL_TAG=$DOCKER_IMAGE$TAG_SUFFIX +REMOTE_TAG=$DOCKER_REGISTRY/$LOCAL_TAG +REMOTE_SHA_TAG=$DOCKER_REGISTRY/$LOCAL_TAG:$COMMIT_SHA diff --git a/selfdrive/test/docker_tag_multiarch.sh b/selfdrive/test/docker_tag_multiarch.sh new file mode 100755 index 0000000000..c1761802c7 --- /dev/null +++ b/selfdrive/test/docker_tag_multiarch.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash +set -e + +if [ $# -lt 2 ]; then + echo "Usage: $0 ..." + exit 1 +fi + +SCRIPT_DIR=$(dirname "$0") +ARCHS=("${@:2}") + +source $SCRIPT_DIR/docker_common.sh $1 + +MANIFEST_AMENDS="" +for ARCH in ${ARCHS[@]}; do + MANIFEST_AMENDS="$MANIFEST_AMENDS --amend $REMOTE_TAG-$ARCH:$COMMIT_SHA" +done + +docker manifest create $REMOTE_TAG $MANIFEST_AMENDS +docker manifest create $REMOTE_SHA_TAG $MANIFEST_AMENDS + +if [[ -n "$PUSH_IMAGE" ]]; then + docker manifest push $REMOTE_TAG + docker manifest push $REMOTE_SHA_TAG +fi diff --git a/selfdrive/test/fuzzy_generation.py b/selfdrive/test/fuzzy_generation.py index 28c70a0ff4..94eb0dfaa6 100644 --- a/selfdrive/test/fuzzy_generation.py +++ b/selfdrive/test/fuzzy_generation.py @@ -1,6 +1,8 @@ import capnp import hypothesis.strategies as st -from typing import Any, Callable, Dict, List, Optional, Union +from typing import Any +from collections.abc import Callable +from functools import cache from cereal import log @@ -10,75 +12,70 @@ DrawType = Callable[[st.SearchStrategy], Any] class FuzzyGenerator: def __init__(self, draw: DrawType, real_floats: bool): self.draw = draw - self.real_floats = real_floats + self.native_type_map = FuzzyGenerator._get_native_type_map(real_floats) - def generate_native_type(self, field: str) -> st.SearchStrategy[Union[bool, int, float, str, bytes]]: - def floats(**kwargs) -> st.SearchStrategy[float]: - allow_nan = not self.real_floats - allow_infinity = not self.real_floats - return st.floats(**kwargs, allow_nan=allow_nan, allow_infinity=allow_infinity) - - if field == 'bool': - return st.booleans() - elif field == 'int8': - return st.integers(min_value=-2**7, max_value=2**7-1) - elif field == 'int16': - return st.integers(min_value=-2**15, max_value=2**15-1) - elif field == 'int32': - return st.integers(min_value=-2**31, max_value=2**31-1) - elif field == 'int64': - return st.integers(min_value=-2**63, max_value=2**63-1) - elif field == 'uint8': - return st.integers(min_value=0, max_value=2**8-1) - elif field == 'uint16': - return st.integers(min_value=0, max_value=2**16-1) - elif field == 'uint32': - return st.integers(min_value=0, max_value=2**32-1) - elif field == 'uint64': - return st.integers(min_value=0, max_value=2**64-1) - elif field == 'float32': - return floats(width=32) - elif field == 'float64': - return floats(width=64) - elif field == 'text': - return st.text(max_size=1000) - elif field == 'data': - return st.binary(max_size=1000) - elif field == 'anyPointer': - return st.text() + def generate_native_type(self, field: str) -> st.SearchStrategy[bool | int | float | str | bytes]: + value_func = self.native_type_map.get(field) + if value_func is not None: + return value_func else: - raise NotImplementedError(f'Invalid type : {field}') + raise NotImplementedError(f'Invalid type: {field}') def generate_field(self, field: capnp.lib.capnp._StructSchemaField) -> st.SearchStrategy: def rec(field_type: capnp.lib.capnp._DynamicStructReader) -> st.SearchStrategy: - if field_type.which() == 'struct': + type_which = field_type.which() + if type_which == 'struct': return self.generate_struct(field.schema.elementType if base_type == 'list' else field.schema) - elif field_type.which() == 'list': + elif type_which == 'list': return st.lists(rec(field_type.list.elementType)) - elif field_type.which() == 'enum': + elif type_which == 'enum': schema = field.schema.elementType if base_type == 'list' else field.schema return st.sampled_from(list(schema.enumerants.keys())) else: - return self.generate_native_type(field_type.which()) + return self.generate_native_type(type_which) - if 'slot' in field.proto.to_dict(): - base_type = field.proto.slot.type.which() - return rec(field.proto.slot.type) - else: + try: + if hasattr(field.proto, 'slot'): + slot_type = field.proto.slot.type + base_type = slot_type.which() + return rec(slot_type) + else: + return self.generate_struct(field.schema) + except capnp.lib.capnp.KjException: return self.generate_struct(field.schema) - def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: Optional[str] = None) -> st.SearchStrategy[Dict[str, Any]]: - full_fill: List[str] = list(schema.non_union_fields) - single_fill: List[str] = [event] if event else [self.draw(st.sampled_from(schema.union_fields))] if schema.union_fields else [] - return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in full_fill + single_fill}) + def generate_struct(self, schema: capnp.lib.capnp._StructSchema, event: str = None) -> st.SearchStrategy[dict[str, Any]]: + single_fill: tuple[str, ...] = (event,) if event else (self.draw(st.sampled_from(schema.union_fields)),) if schema.union_fields else () + fields_to_generate = schema.non_union_fields + single_fill + return st.fixed_dictionaries({field: self.generate_field(schema.fields[field]) for field in fields_to_generate if not field.endswith('DEPRECATED')}) + + @staticmethod + @cache + def _get_native_type_map(real_floats: bool) -> dict[str, st.SearchStrategy]: + return { + 'bool': st.booleans(), + 'int8': st.integers(min_value=-2**7, max_value=2**7-1), + 'int16': st.integers(min_value=-2**15, max_value=2**15-1), + 'int32': st.integers(min_value=-2**31, max_value=2**31-1), + 'int64': st.integers(min_value=-2**63, max_value=2**63-1), + 'uint8': st.integers(min_value=0, max_value=2**8-1), + 'uint16': st.integers(min_value=0, max_value=2**16-1), + 'uint32': st.integers(min_value=0, max_value=2**32-1), + 'uint64': st.integers(min_value=0, max_value=2**64-1), + 'float32': st.floats(width=32, allow_nan=not real_floats, allow_infinity=not real_floats), + 'float64': st.floats(width=64, allow_nan=not real_floats, allow_infinity=not real_floats), + 'text': st.text(max_size=1000), + 'data': st.binary(max_size=1000), + 'anyPointer': st.text(), # Note: No need to define a separate function for anyPointer + } @classmethod - def get_random_msg(cls, draw: DrawType, struct: capnp.lib.capnp._StructModule, real_floats: bool = False) -> Dict[str, Any]: + def get_random_msg(cls, draw: DrawType, struct: capnp.lib.capnp._StructModule, real_floats: bool = False) -> dict[str, Any]: fg = cls(draw, real_floats=real_floats) - data: Dict[str, Any] = draw(fg.generate_struct(struct.schema)) + data: dict[str, Any] = draw(fg.generate_struct(struct.schema)) return data @classmethod - def get_random_event_msg(cls, draw: DrawType, events: List[str], real_floats: bool = False) -> List[Dict[str, Any]]: + def get_random_event_msg(cls, draw: DrawType, events: list[str], real_floats: bool = False) -> list[dict[str, Any]]: fg = cls(draw, real_floats=real_floats) return [draw(fg.generate_struct(log.Event.schema, e)) for e in sorted(events)] diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 0e7912a989..b7f5bb183b 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -1,17 +1,21 @@ +import contextlib +import http.server import os +import threading import time +import pytest from functools import wraps import cereal.messaging as messaging from openpilot.common.params import Params -from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes from openpilot.system.hardware import PC from openpilot.system.version import training_version, terms_version def set_params_enabled(): - os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" + os.environ['FINGERPRINT'] = "TOYOTA_COROLLA_TSS2" os.environ['LOGPRINT'] = "debug" params = Params() @@ -29,39 +33,46 @@ def phone_only(f): @wraps(f) def wrap(self, *args, **kwargs): if PC: - self.skipTest("This test is not meant to run on PC") - f(self, *args, **kwargs) + pytest.skip("This test is not meant to run on PC") + return f(self, *args, **kwargs) return wrap def release_only(f): @wraps(f) def wrap(self, *args, **kwargs): if "RELEASE" not in os.environ: - self.skipTest("This test is only for release branches") + pytest.skip("This test is only for release branches") f(self, *args, **kwargs) return wrap -def with_processes(processes, init_time=0, ignore_stopped=None): + +@contextlib.contextmanager +def processes_context(processes, init_time=0, ignore_stopped=None): ignore_stopped = [] if ignore_stopped is None else ignore_stopped + # start and assert started + for n, p in enumerate(processes): + managed_processes[p].start() + if n < len(processes) - 1: + time.sleep(init_time) + + assert all(managed_processes[name].proc.exitcode is None for name in processes) + + try: + yield [managed_processes[name] for name in processes] + # assert processes are still started + assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) + finally: + for p in processes: + managed_processes[p].stop() + + +def with_processes(processes, init_time=0, ignore_stopped=None): def wrapper(func): @wraps(func) def wrap(*args, **kwargs): - # start and assert started - for n, p in enumerate(processes): - managed_processes[p].start() - if n < len(processes) - 1: - time.sleep(init_time) - assert all(managed_processes[name].proc.exitcode is None for name in processes) - - # call the function - try: - func(*args, **kwargs) - # assert processes are still started - assert all(managed_processes[name].proc.exitcode is None for name in processes if name not in ignore_stopped) - finally: - for p in processes: - managed_processes[p].stop() + with processes_context(processes, init_time, ignore_stopped): + return func(*args, **kwargs) return wrap return wrapper @@ -72,7 +83,55 @@ def noop(*args, **kwargs): def read_segment_list(segment_list_path): - with open(segment_list_path, "r") as f: + with open(segment_list_path) as f: seg_list = f.read().splitlines() return [(platform[2:], segment) for platform, segment in zip(seg_list[::2], seg_list[1::2], strict=True)] + + +@contextlib.contextmanager +def http_server_context(handler, setup=None): + host = '127.0.0.1' + server = http.server.HTTPServer((host, 0), handler) + port = server.server_port + t = threading.Thread(target=server.serve_forever) + t.start() + + if setup is not None: + setup(host, port) + + try: + yield (host, port) + finally: + server.shutdown() + server.server_close() + t.join() + + +def with_http_server(func, handler=http.server.BaseHTTPRequestHandler, setup=None): + @wraps(func) + def inner(*args, **kwargs): + with http_server_context(handler, setup) as (host, port): + return func(*args, f"http://{host}:{port}", **kwargs) + return inner + + +def DirectoryHttpServer(directory) -> type[http.server.SimpleHTTPRequestHandler]: + # creates an http server that serves files from directory + class Handler(http.server.SimpleHTTPRequestHandler): + API_NO_RESPONSE = False + API_BAD_RESPONSE = False + + def do_GET(self): + if self.API_NO_RESPONSE: + return + + if self.API_BAD_RESPONSE: + self.send_response(500, "") + return + super().do_GET() + + def __init__(self, *args, **kwargs): + super().__init__(*args, directory=str(directory), **kwargs) + + return Handler diff --git a/selfdrive/test/longitudinal_maneuvers/.gitignore b/selfdrive/test/longitudinal_maneuvers/.gitignore new file mode 100644 index 0000000000..d42ab353ab --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/.gitignore @@ -0,0 +1 @@ +out/* diff --git a/selfdrive/test/longitudinal_maneuvers/__init__.py b/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/longitudinal_maneuvers/maneuver.py b/selfdrive/test/longitudinal_maneuvers/maneuver.py new file mode 100644 index 0000000000..6c8495cc3b --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/maneuver.py @@ -0,0 +1,73 @@ +import numpy as np +from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant + + +class Maneuver: + def __init__(self, title, duration, **kwargs): + # Was tempted to make a builder class + self.distance_lead = kwargs.get("initial_distance_lead", 200.0) + self.speed = kwargs.get("initial_speed", 0.0) + self.lead_relevancy = kwargs.get("lead_relevancy", 0) + + self.breakpoints = kwargs.get("breakpoints", [0.0, duration]) + self.speed_lead_values = kwargs.get("speed_lead_values", [0.0 for i in range(len(self.breakpoints))]) + self.prob_lead_values = kwargs.get("prob_lead_values", [1.0 for i in range(len(self.breakpoints))]) + self.cruise_values = kwargs.get("cruise_values", [50.0 for i in range(len(self.breakpoints))]) + + self.only_lead2 = kwargs.get("only_lead2", False) + self.only_radar = kwargs.get("only_radar", False) + self.ensure_start = kwargs.get("ensure_start", False) + self.enabled = kwargs.get("enabled", True) + self.e2e = kwargs.get("e2e", False) + self.personality = kwargs.get("personality", 0) + self.force_decel = kwargs.get("force_decel", False) + + self.duration = duration + self.title = title + + def evaluate(self): + plant = Plant( + lead_relevancy=self.lead_relevancy, + speed=self.speed, + distance_lead=self.distance_lead, + enabled=self.enabled, + only_lead2=self.only_lead2, + only_radar=self.only_radar, + e2e=self.e2e, + personality=self.personality, + force_decel=self.force_decel, + ) + + valid = True + logs = [] + while plant.current_time < self.duration: + speed_lead = np.interp(plant.current_time, self.breakpoints, self.speed_lead_values) + prob = np.interp(plant.current_time, self.breakpoints, self.prob_lead_values) + cruise = np.interp(plant.current_time, self.breakpoints, self.cruise_values) + log = plant.step(speed_lead, prob, cruise) + + d_rel = log['distance_lead'] - log['distance'] if self.lead_relevancy else 200. + v_rel = speed_lead - log['speed'] if self.lead_relevancy else 0. + log['d_rel'] = d_rel + log['v_rel'] = v_rel + logs.append(np.array([plant.current_time, + log['distance'], + log['distance_lead'], + log['speed'], + speed_lead, + log['acceleration']])) + + if d_rel < .4 and (self.only_radar or prob > 0.5): + print("Crashed!!!!") + valid = False + + if self.ensure_start and log['v_rel'] > 0 and log['speeds'][-1] <= 0.1: + print('LongitudinalPlanner not starting!') + valid = False + if self.force_decel and log['speed'] > 1e-1 and log['acceleration'] > -0.04: + print('Not stopping with force decel') + valid = False + + + print("maneuver end", valid) + return valid, np.array(logs) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100755 index 0000000000..daf7cec32b --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +import time +import numpy as np + +from cereal import log +import cereal.messaging as messaging +from openpilot.common.realtime import Ratekeeper, DT_MDL +from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner +from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU + + +class Plant: + messaging_initialized = False + + def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0, + enabled=True, only_lead2=False, only_radar=False, e2e=False, personality=0, force_decel=False): + self.rate = 1. / DT_MDL + + if not Plant.messaging_initialized: + Plant.radar = messaging.pub_sock('radarState') + Plant.controls_state = messaging.pub_sock('controlsState') + Plant.car_state = messaging.pub_sock('carState') + Plant.plan = messaging.sub_sock('longitudinalPlan') + Plant.messaging_initialized = True + + self.v_lead_prev = 0.0 + + self.distance = 0. + self.speed = speed + self.acceleration = 0.0 + self.speeds = [] + + # lead car + self.lead_relevancy = lead_relevancy + self.distance_lead = distance_lead + self.enabled = enabled + self.only_lead2 = only_lead2 + self.only_radar = only_radar + self.e2e = e2e + self.personality = personality + self.force_decel = force_decel + + self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) + self.ts = 1. / self.rate + time.sleep(0.1) + self.sm = messaging.SubMaster(['longitudinalPlan']) + + from openpilot.selfdrive.car.honda.values import CAR + from openpilot.selfdrive.car.honda.interface import CarInterface + + self.planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=self.speed) + + @property + def current_time(self): + return float(self.rk.frame) / self.rate + + def step(self, v_lead=0.0, prob=1.0, v_cruise=50.): + # ******** publish a fake model going straight and fake calibration ******** + # note that this is worst case for MPC, since model will delay long mpc by one time step + radar = messaging.new_message('radarState') + control = messaging.new_message('controlsState') + car_state = messaging.new_message('carState') + model = messaging.new_message('modelV2') + a_lead = (v_lead - self.v_lead_prev)/self.ts + self.v_lead_prev = v_lead + + if self.lead_relevancy: + d_rel = np.maximum(0., self.distance_lead - self.distance) + v_rel = v_lead - self.speed + if self.only_radar: + status = True + elif prob > .5: + status = True + else: + status = False + else: + d_rel = 200. + v_rel = 0. + prob = 0.0 + status = False + + lead = log.RadarState.LeadData.new_message() + lead.dRel = float(d_rel) + lead.yRel = 0.0 + lead.vRel = float(v_rel) + lead.aRel = float(a_lead - self.acceleration) + lead.vLead = float(v_lead) + lead.vLeadK = float(v_lead) + lead.aLeadK = float(a_lead) + # TODO use real radard logic for this + lead.aLeadTau = float(_LEAD_ACCEL_TAU) + lead.status = status + lead.modelProb = float(prob) + if not self.only_lead2: + radar.radarState.leadOne = lead + radar.radarState.leadTwo = lead + + # Simulate model predicting slightly faster speed + # this is to ensure lead policy is effective when model + # does not predict slowdown in e2e mode + position = log.XYZTData.new_message() + position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + velocity = log.XYZTData.new_message() + velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)] + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)] + model.modelV2.acceleration = acceleration + + control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off + control.controlsState.vCruise = float(v_cruise * 3.6) + control.controlsState.experimentalMode = self.e2e + control.controlsState.personality = self.personality + control.controlsState.forceDecel = self.force_decel + car_state.carState.vEgo = float(self.speed) + car_state.carState.standstill = self.speed < 0.01 + + # ******** get controlsState messages for plotting *** + sm = {'radarState': radar.radarState, + 'carState': car_state.carState, + 'controlsState': control.controlsState, + 'modelV2': model.modelV2} + self.planner.update(sm) + self.speed = self.planner.v_desired_filter.x + self.acceleration = self.planner.a_desired + self.speeds = self.planner.v_desired_trajectory.tolist() + fcw = self.planner.fcw + self.distance_lead = self.distance_lead + v_lead * self.ts + + # ******** run the car ******** + #print(self.distance, speed) + if self.speed <= 0: + self.speed = 0 + self.acceleration = 0 + self.distance = self.distance + self.speed * self.ts + + # *** radar model *** + if self.lead_relevancy: + d_rel = np.maximum(0., self.distance_lead - self.distance) + v_rel = v_lead - self.speed + else: + d_rel = 200. + v_rel = 0. + + # print at 5hz + # if (self.rk.frame % (self.rate // 5)) == 0: + # print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s" + # % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel)) + + + # ******** update prevs ******** + self.rk.monitor_time() + + return { + "distance": self.distance, + "speed": self.speed, + "acceleration": self.acceleration, + "speeds": self.speeds, + "distance_lead": self.distance_lead, + "fcw": fcw, + } + +# simple engage in standalone mode +def plant_thread(): + plant = Plant() + while 1: + plant.step() + + +if __name__ == "__main__": + plant_thread() diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py new file mode 100644 index 0000000000..62a95babeb --- /dev/null +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -0,0 +1,154 @@ +import itertools +from parameterized import parameterized_class + +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE +from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver + + +# TODO: make new FCW tests +def create_maneuvers(kwargs): + maneuvers = [ + Maneuver( + 'approach stopped car at 25m/s, initial distance: 120m', + duration=20., + initial_speed=25., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[30., 0.], + breakpoints=[0., 1.], + **kwargs, + ), + Maneuver( + 'approach stopped car at 20m/s, initial distance 90m', + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=90., + speed_lead_values=[20., 0.], + breakpoints=[0., 1.], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 1m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 35.0], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 2m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 25.0], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3m/s^2', + duration=50., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + breakpoints=[0., 15., 21.66], + **kwargs, + ), + Maneuver( + 'steady state following a car at 20m/s, then lead decel to 0mph at 3+m/s^2', + duration=40., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=35., + speed_lead_values=[20., 20., 0.], + prob_lead_values=[0., 1., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[2., 2.01, 8.8], + **kwargs, + ), + Maneuver( + "approach stopped car at 20m/s, with prob_lead_values", + duration=30., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=120., + speed_lead_values=[0.0, 0., 0.], + prob_lead_values=[0.0, 0., 1.], + cruise_values=[20., 20., 20.], + breakpoints=[0.0, 2., 2.01], + **kwargs, + ), + Maneuver( + "approach slower cut-in car at 20m/s", + duration=20., + initial_speed=20., + lead_relevancy=True, + initial_distance_lead=50., + speed_lead_values=[15., 15.], + breakpoints=[1., 11.], + only_lead2=True, + **kwargs, + ), + Maneuver( + "stay stopped behind radar override lead", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=10., + speed_lead_values=[0., 0.], + prob_lead_values=[0., 0.], + breakpoints=[1., 11.], + only_radar=True, + **kwargs, + ), + Maneuver( + "NaN recovery", + duration=30., + initial_speed=15., + lead_relevancy=True, + initial_distance_lead=60., + speed_lead_values=[0., 0., 0.0], + breakpoints=[1., 1.01, 11.], + cruise_values=[float("nan"), 15., 15.], + **kwargs, + ), + Maneuver( + 'cruising at 25 m/s while disabled', + duration=20., + initial_speed=25., + lead_relevancy=False, + enabled=False, + **kwargs, + ), + ] + if not kwargs['force_decel']: + # controls relies on planner commanding to move for stock-ACC resume spamming + maneuvers.append(Maneuver( + "resume from a stop", + duration=20., + initial_speed=0., + lead_relevancy=True, + initial_distance_lead=STOP_DISTANCE, + speed_lead_values=[0., 0., 2.], + breakpoints=[1., 10., 15.], + ensure_start=True, + **kwargs, + )) + return maneuvers + + +@parameterized_class(("e2e", "force_decel"), itertools.product([True, False], repeat=2)) +class TestLongitudinalControl: + e2e: bool + force_decel: bool + + def test_maneuver(self, subtests): + for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}): + with subtests.test(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel): + print(maneuver.title, f'in {"e2e" if maneuver.e2e else "acc"} mode') + valid, _ = maneuver.evaluate() + assert valid diff --git a/selfdrive/test/loop_until_fail.sh b/selfdrive/test/loop_until_fail.sh new file mode 100755 index 0000000000..b73009dba6 --- /dev/null +++ b/selfdrive/test/loop_until_fail.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +set -e + +# Loop something forever until it fails, for verifying new tests + +while true; do + $@ +done diff --git a/selfdrive/test/process_replay/.gitignore b/selfdrive/test/process_replay/.gitignore new file mode 100644 index 0000000000..a35cd58d41 --- /dev/null +++ b/selfdrive/test/process_replay/.gitignore @@ -0,0 +1 @@ +fakedata/ diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md new file mode 100644 index 0000000000..008a901010 --- /dev/null +++ b/selfdrive/test/process_replay/README.md @@ -0,0 +1,126 @@ +# Process replay + +Process replay is a regression test designed to identify any changes in the output of a process. This test replays a segment through individual processes and compares the output to a known good replay. Each make is represented in the test with a segment. + +If the test fails, make sure that you didn't unintentionally change anything. If there are intentional changes, the reference logs will be updated. + +Use `test_processes.py` to run the test locally. +Use `FILEREADER_CACHE='1' test_processes.py` to cache log files. + +Currently the following processes are tested: + +* controlsd +* radard +* plannerd +* calibrationd +* dmonitoringd +* locationd +* paramsd +* ubloxd +* torqued + +### Usage +``` +Usage: test_processes.py [-h] [--whitelist-procs PROCS] [--whitelist-cars CARS] [--blacklist-procs PROCS] + [--blacklist-cars CARS] [--ignore-fields FIELDS] [--ignore-msgs MSGS] [--update-refs] [--upload-only] +Regression test to identify changes in a process's output +optional arguments: + -h, --help show this help message and exit + --whitelist-procs PROCS Whitelist given processes from the test (e.g. controlsd) + --whitelist-cars WHITELIST_CARS Whitelist given cars from the test (e.g. HONDA) + --blacklist-procs BLACKLIST_PROCS Blacklist given processes from the test (e.g. controlsd) + --blacklist-cars BLACKLIST_CARS Blacklist given cars from the test (e.g. HONDA) + --ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. carState.events) + --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents) + --update-refs Updates reference logs using current commit + --upload-only Skips testing processes and uploads logs from previous test run +``` + +## Forks + +openpilot forks can use this test with their own reference logs, by default `test_proccess.py` saves logs locally. + +To generate new logs: + +`./test_processes.py` + +Then, check in the new logs using git-lfs. Make sure to also update the `ref_commit` file to the current commit. + +## API + +Process replay test suite exposes programmatic APIs for simultaneously running processes or groups of processes on provided logs. + +```py +def replay_process_with_name(name: Union[str, Iterable[str]], lr: LogIterable, *args, **kwargs) -> List[capnp._DynamicStructReader]: + +def replay_process( + cfg: Union[ProcessConfig, Iterable[ProcessConfig]], lr: LogIterable, frs: Optional[Dict[str, Any]] = None, + fingerprint: Optional[str] = None, return_all_logs: bool = False, custom_params: Optional[Dict[str, Any]] = None, disable_progress: bool = False +) -> List[capnp._DynamicStructReader]: +``` + +Example usage: +```py +from openpilot.selfdrive.test.process_replay import replay_process_with_name +from openpilot.tools.lib.logreader import LogReader + +lr = LogReader(...) + +# provide a name of the process to replay +output_logs = replay_process_with_name('locationd', lr) + +# or list of names +output_logs = replay_process_with_name(['ubloxd', 'locationd'], lr) +``` + +Supported processes: +* controlsd +* radard +* plannerd +* calibrationd +* dmonitoringd +* locationd +* paramsd +* ubloxd +* torqued +* modeld +* dmonitoringmodeld + +Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. + +```py +from openpilot.selfdrive.test.process_replay import get_custom_params_from_lr + +previous_segment_lr = LogReader(...) +current_segment_lr = LogReader(...) + +custom_params = get_custom_params_from_lr(previous_segment_lr, 'last') + +output_logs = replay_process_with_name('calibrationd', lr, custom_params=custom_params) +``` + +Replaying processes that use VisionIPC (e.g. modeld, dmonitoringmodeld) require additional `frs` dictionary with camera states as keys and `FrameReader` objects as values. + +```py +from openpilot.tools.lib.framereader import FrameReader + +frs = { + 'roadCameraState': FrameReader(...), + 'wideRoadCameraState': FrameReader(...), + 'driverCameraState': FrameReader(...), +} + +output_logs = replay_process_with_name(['modeld', 'dmonitoringmodeld'], lr, frs=frs) +``` + +To capture stdout/stderr of the replayed process, `captured_output_store` can be provided. + +```py +output_store = dict() +# pass dictionary by reference, it will be filled with standard outputs - even if process replay fails +output_logs = replay_process_with_name(['radard', 'plannerd'], lr, captured_output_store=output_store) + +# entries with captured output in format { 'out': '...', 'err': '...' } will be added to provided dictionary for each replayed process +print(output_store['radard']['out']) # radard stdout +print(output_store['radard']['err']) # radard stderr +``` diff --git a/selfdrive/test/process_replay/__init__.py b/selfdrive/test/process_replay/__init__.py new file mode 100644 index 0000000000..b994277186 --- /dev/null +++ b/selfdrive/test/process_replay/__init__.py @@ -0,0 +1,2 @@ +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, get_process_config, get_custom_params_from_lr, \ + replay_process, replay_process_with_name # noqa: F401 diff --git a/selfdrive/test/process_replay/capture.py b/selfdrive/test/process_replay/capture.py new file mode 100644 index 0000000000..90c279ef35 --- /dev/null +++ b/selfdrive/test/process_replay/capture.py @@ -0,0 +1,59 @@ +import os +import sys + +from typing import no_type_check + +class FdRedirect: + def __init__(self, file_prefix: str, fd: int): + fname = os.path.join("/tmp", f"{file_prefix}.{fd}") + if os.path.exists(fname): + os.unlink(fname) + self.dest_fd = os.open(fname, os.O_WRONLY | os.O_CREAT) + self.dest_fname = fname + self.source_fd = fd + os.set_inheritable(self.dest_fd, True) + + def __del__(self): + os.close(self.dest_fd) + + def purge(self) -> None: + os.unlink(self.dest_fname) + + def read(self) -> bytes: + with open(self.dest_fname, "rb") as f: + return f.read() or b"" + + def link(self) -> None: + os.dup2(self.dest_fd, self.source_fd) + + +class ProcessOutputCapture: + def __init__(self, proc_name: str, prefix: str): + prefix = f"{proc_name}_{prefix}" + self.stdout_redirect = FdRedirect(prefix, 1) + self.stderr_redirect = FdRedirect(prefix, 2) + + def __del__(self): + self.stdout_redirect.purge() + self.stderr_redirect.purge() + + @no_type_check # ipython classes have incompatible signatures + def link_with_current_proc(self) -> None: + try: + # prevent ipykernel from redirecting stdout/stderr of python subprocesses + from ipykernel.iostream import OutStream + if isinstance(sys.stdout, OutStream): + sys.stdout = sys.__stdout__ + if isinstance(sys.stderr, OutStream): + sys.stderr = sys.__stderr__ + except ImportError: + pass + + # link stdout/stderr to the fifo + self.stdout_redirect.link() + self.stderr_redirect.link() + + def read_outerr(self) -> tuple[str, str]: + out_str = self.stdout_redirect.read().decode() + err_str = self.stderr_redirect.read().decode() + return out_str, err_str diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py new file mode 100755 index 0000000000..673f3b484c --- /dev/null +++ b/selfdrive/test/process_replay/compare_logs.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +import sys +import math +import capnp +import numbers +import dictdiffer +from collections import Counter + +from openpilot.tools.lib.logreader import LogReader + +EPSILON = sys.float_info.epsilon + + +def remove_ignored_fields(msg, ignore): + msg = msg.as_builder() + for key in ignore: + attr = msg + keys = key.split(".") + if msg.which() != keys[0] and len(keys) > 1: + continue + + for k in keys[:-1]: + # indexing into list + if k.isdigit(): + attr = attr[int(k)] + else: + attr = getattr(attr, k) + + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + elif isinstance(v, (list, capnp.lib.capnp._DynamicListBuilder)): + val = [] + else: + raise NotImplementedError(f"Unknown type: {type(v)}") + setattr(attr, keys[-1], val) + return msg + + +def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=None,): + if ignore_fields is None: + ignore_fields = [] + if ignore_msgs is None: + ignore_msgs = [] + tolerance = EPSILON if tolerance is None else tolerance + + log1, log2 = ( + [m for m in log if m.which() not in ignore_msgs] + for log in (log1, log2) + ) + + if len(log1) != len(log2): + cnt1 = Counter(m.which() for m in log1) + cnt2 = Counter(m.which() for m in log2) + 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, strict=True): + if msg1.which() != msg2.which(): + raise Exception("msgs not aligned between logs") + + msg1 = remove_ignored_fields(msg1, ignore_fields) + msg2 = remove_ignored_fields(msg2, ignore_fields) + + if msg1.to_bytes() != msg2.to_bytes(): + msg1_dict = msg1.as_reader().to_dict(verbose=True) + msg2_dict = msg2.as_reader().to_dict(verbose=True) + + dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields) + + # Dictdiffer only supports relative tolerance, we also want to check for absolute + # TODO: add this to dictdiffer + def outside_tolerance(diff): + try: + if diff[0] == "change": + a, b = diff[2] + finite = math.isfinite(a) and math.isfinite(b) + if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): + return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + except TypeError: + pass + return True + + dd = list(filter(outside_tolerance, dd)) + + diff.extend(dd) + return diff + + +def format_process_diff(diff): + diff_short, diff_long = "", "" + + if isinstance(diff, str): + diff_short += f" {diff}\n" + diff_long += f"\t{diff}\n" + else: + cnt: dict[str, int] = {} + for d in diff: + diff_long += f"\t{str(d)}\n" + + k = str(d[1]) + cnt[k] = 1 if k not in cnt else cnt[k] + 1 + + for k, v in sorted(cnt.items()): + diff_short += f" {k}: {v}\n" + + return diff_short, diff_long + + +def format_diff(results, log_paths, ref_commit): + diff_short, diff_long = "", "" + diff_long += f"***** tested against commit {ref_commit} *****\n" + + failed = False + for segment, result in list(results.items()): + diff_short += f"***** results for segment {segment} *****\n" + diff_long += f"***** differences for segment {segment} *****\n" + + for proc, diff in list(result.items()): + diff_long += f"*** process: {proc} ***\n" + diff_long += f"\tref: {log_paths[segment][proc]['ref']}\n" + diff_long += f"\tnew: {log_paths[segment][proc]['new']}\n\n" + + diff_short += f" {proc}\n" + + if isinstance(diff, str) or len(diff): + diff_short += f" ref: {log_paths[segment][proc]['ref']}\n" + diff_short += f" new: {log_paths[segment][proc]['new']}\n\n" + failed = True + + proc_diff_short, proc_diff_long = format_process_diff(diff) + + diff_long += proc_diff_long + diff_short += proc_diff_short + + return diff_short, diff_long, failed + + +if __name__ == "__main__": + log1 = list(LogReader(sys.argv[1])) + log2 = list(LogReader(sys.argv[2])) + ignore_fields = sys.argv[3:] or ["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"] + results = {"segment": {"proc": compare_logs(log1, log2, ignore_fields)}} + log_paths = {"segment": {"proc": {"ref": sys.argv[1], "new": sys.argv[2]}}} + diff_short, diff_long, failed = format_diff(results, log_paths, None) + + print(diff_long) + print(diff_short) diff --git a/selfdrive/test/process_replay/imgproc_replay_ref_hash b/selfdrive/test/process_replay/imgproc_replay_ref_hash new file mode 100644 index 0000000000..defcb3681c --- /dev/null +++ b/selfdrive/test/process_replay/imgproc_replay_ref_hash @@ -0,0 +1 @@ +707434c540e685bbe2886b3ff7c82fd61939d362 \ No newline at end of file diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py new file mode 100644 index 0000000000..0715fcf7b4 --- /dev/null +++ b/selfdrive/test/process_replay/migration.py @@ -0,0 +1,254 @@ +from collections import defaultdict + +from cereal import messaging +from openpilot.selfdrive.car.fingerprints import MIGRATION +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_encode_index +from openpilot.selfdrive.car.toyota.values import EPS_SCALE +from openpilot.system.manager.process_config import managed_processes +from panda import Panda + + +# TODO: message migration should happen in-place +def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, camera_states=False): + msgs = migrate_sensorEvents(lr, old_logtime) + msgs = migrate_carParams(msgs, old_logtime) + msgs = migrate_gpsLocation(msgs) + msgs = migrate_deviceState(msgs) + msgs = migrate_carOutput(msgs) + if manager_states: + msgs = migrate_managerState(msgs) + if panda_states: + msgs = migrate_pandaStates(msgs) + msgs = migrate_peripheralState(msgs) + if camera_states: + msgs = migrate_cameraStates(msgs) + + return msgs + + +def migrate_managerState(lr): + all_msgs = [] + for msg in lr: + if msg.which() != "managerState": + all_msgs.append(msg) + continue + + new_msg = msg.as_builder() + new_msg.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] + all_msgs.append(new_msg.as_reader()) + + return all_msgs + + +def migrate_gpsLocation(lr): + all_msgs = [] + for msg in lr: + if msg.which() in ('gpsLocation', 'gpsLocationExternal'): + new_msg = msg.as_builder() + g = getattr(new_msg, new_msg.which()) + # hasFix is a newer field + if not g.hasFix and g.flags == 1: + g.hasFix = True + all_msgs.append(new_msg.as_reader()) + else: + all_msgs.append(msg) + return all_msgs + + +def migrate_deviceState(lr): + all_msgs = [] + dt = None + for msg in lr: + if msg.which() == 'initData': + dt = msg.initData.deviceType + if msg.which() == 'deviceState': + n = msg.as_builder() + n.deviceState.deviceType = dt + all_msgs.append(n.as_reader()) + else: + all_msgs.append(msg) + return all_msgs + + +def migrate_carOutput(lr): + # migration needed only for routes before carOutput + if any(msg.which() == 'carOutput' for msg in lr): + return lr + + all_msgs = [] + for msg in lr: + if msg.which() == 'carControl': + co = messaging.new_message('carOutput') + co.valid = msg.valid + co.logMonoTime = msg.logMonoTime + co.carOutput.actuatorsOutput = msg.carControl.actuatorsOutputDEPRECATED + all_msgs.append(co.as_reader()) + all_msgs.append(msg) + return all_msgs + + +def migrate_pandaStates(lr): + all_msgs = [] + # TODO: safety param migration should be handled automatically + safety_param_migration = { + "TOYOTA_PRIUS": EPS_SCALE["TOYOTA_PRIUS"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL, + "TOYOTA_RAV4": EPS_SCALE["TOYOTA_RAV4"] | Panda.FLAG_TOYOTA_ALT_BRAKE, + "KIA_EV6": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2, + } + + # Migrate safety param base on carState + CP = next((m.carParams for m in lr if m.which() == 'carParams'), None) + assert CP is not None, "carParams message not found" + if CP.carFingerprint in safety_param_migration: + safety_param = safety_param_migration[CP.carFingerprint] + elif len(CP.safetyConfigs): + safety_param = CP.safetyConfigs[0].safetyParam + if CP.safetyConfigs[0].safetyParamDEPRECATED != 0: + safety_param = CP.safetyConfigs[0].safetyParamDEPRECATED + else: + safety_param = CP.safetyParamDEPRECATED + + for msg in lr: + if msg.which() == 'pandaStateDEPRECATED': + new_msg = messaging.new_message('pandaStates', 1) + new_msg.valid = msg.valid + new_msg.logMonoTime = msg.logMonoTime + new_msg.pandaStates[0] = msg.pandaStateDEPRECATED + new_msg.pandaStates[0].safetyParam = safety_param + all_msgs.append(new_msg.as_reader()) + elif msg.which() == 'pandaStates': + new_msg = msg.as_builder() + new_msg.pandaStates[-1].safetyParam = safety_param + all_msgs.append(new_msg.as_reader()) + else: + all_msgs.append(msg) + + return all_msgs + + +def migrate_peripheralState(lr): + if any(msg.which() == "peripheralState" for msg in lr): + return lr + + all_msg = [] + for msg in lr: + all_msg.append(msg) + if msg.which() not in ["pandaStates", "pandaStateDEPRECATED"]: + continue + + new_msg = messaging.new_message("peripheralState") + new_msg.valid = msg.valid + new_msg.logMonoTime = msg.logMonoTime + all_msg.append(new_msg.as_reader()) + + return all_msg + + +def migrate_cameraStates(lr): + all_msgs = [] + frame_to_encode_id = defaultdict(dict) + # just for encodeId fallback mechanism + min_frame_id = defaultdict(lambda: float('inf')) + + for msg in lr: + if msg.which() not in ["roadEncodeIdx", "wideRoadEncodeIdx", "driverEncodeIdx"]: + continue + + encode_index = getattr(msg, msg.which()) + meta = meta_from_encode_index(msg.which()) + + assert encode_index.segmentId < 1200, f"Encoder index segmentId greater that 1200: {msg.which()} {encode_index.segmentId}" + frame_to_encode_id[meta.camera_state][encode_index.frameId] = encode_index.segmentId + + for msg in lr: + if msg.which() not in ["roadCameraState", "wideRoadCameraState", "driverCameraState"]: + all_msgs.append(msg) + continue + + camera_state = getattr(msg, msg.which()) + min_frame_id[msg.which()] = min(min_frame_id[msg.which()], camera_state.frameId) + + encode_id = frame_to_encode_id[msg.which()].get(camera_state.frameId) + if encode_id is None: + print(f"Missing encoded frame for camera feed {msg.which()} with frameId: {camera_state.frameId}") + if len(frame_to_encode_id[msg.which()]) != 0: + continue + + # fallback mechanism for logs without encodeIdx (e.g. logs from before 2022 with dcamera recording disabled) + # try to fake encode_id by subtracting lowest frameId + encode_id = camera_state.frameId - min_frame_id[msg.which()] + print(f"Faking encodeId to {encode_id} for camera feed {msg.which()} with frameId: {camera_state.frameId}") + + new_msg = messaging.new_message(msg.which()) + new_camera_state = getattr(new_msg, new_msg.which()) + new_camera_state.frameId = encode_id + new_camera_state.encodeId = encode_id + # timestampSof was added later so it might be missing on some old segments + if camera_state.timestampSof == 0 and camera_state.timestampEof > 25000000: + new_camera_state.timestampSof = camera_state.timestampEof - 18000000 + else: + new_camera_state.timestampSof = camera_state.timestampSof + new_camera_state.timestampEof = camera_state.timestampEof + new_msg.logMonoTime = msg.logMonoTime + new_msg.valid = msg.valid + + all_msgs.append(new_msg.as_reader()) + + return all_msgs + + +def migrate_carParams(lr, old_logtime=False): + all_msgs = [] + for msg in lr: + if msg.which() == 'carParams': + CP = msg.as_builder() + CP.carParams.carFingerprint = MIGRATION.get(CP.carParams.carFingerprint, CP.carParams.carFingerprint) + for car_fw in CP.carParams.carFw: + car_fw.brand = CP.carParams.carName + if old_logtime: + CP.logMonoTime = msg.logMonoTime + msg = CP.as_reader() + all_msgs.append(msg) + + return all_msgs + + +def migrate_sensorEvents(lr, old_logtime=False): + all_msgs = [] + for msg in lr: + if msg.which() != 'sensorEventsDEPRECATED': + all_msgs.append(msg) + continue + + # migrate to split sensor events + for evt in msg.sensorEventsDEPRECATED: + # build new message for each sensor type + sensor_service = '' + if evt.which() == 'acceleration': + sensor_service = 'accelerometer' + elif evt.which() == 'gyro' or evt.which() == 'gyroUncalibrated': + sensor_service = 'gyroscope' + elif evt.which() == 'light' or evt.which() == 'proximity': + sensor_service = 'lightSensor' + elif evt.which() == 'magnetic' or evt.which() == 'magneticUncalibrated': + sensor_service = 'magnetometer' + elif evt.which() == 'temperature': + sensor_service = 'temperatureSensor' + + m = messaging.new_message(sensor_service) + m.valid = True + if old_logtime: + m.logMonoTime = msg.logMonoTime + + m_dat = getattr(m, sensor_service) + m_dat.version = evt.version + m_dat.sensor = evt.sensor + m_dat.type = evt.type + m_dat.source = evt.source + if old_logtime: + m_dat.timestamp = evt.timestamp + setattr(m_dat, evt.which(), getattr(evt, evt.which())) + + all_msgs.append(m.as_reader()) + + return all_msgs diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py new file mode 100755 index 0000000000..cba2edfd58 --- /dev/null +++ b/selfdrive/test/process_replay/model_replay.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +import os +import sys +from collections import defaultdict +from typing import Any + +from openpilot.common.git import get_commit +from openpilot.system.hardware import PC +from openpilot.tools.lib.openpilotci import BASE_URL, get_url +from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff +from openpilot.selfdrive.test.process_replay.process_replay import get_process_config, replay_process +from openpilot.tools.lib.framereader import FrameReader +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.helpers import save_log + +TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30" +SEGMENT = 6 +MAX_FRAMES = 100 if PC else 600 + +NO_MODEL = "NO_MODEL" in os.environ +SEND_EXTRA_INPUTS = bool(int(os.getenv("SEND_EXTRA_INPUTS", "0"))) + + +def get_log_fn(ref_commit, test_route): + return f"{test_route}_model_tici_{ref_commit}.bz2" + + +def trim_logs_to_max_frames(logs, max_frames, frs_types, include_all_types): + all_msgs = [] + cam_state_counts = defaultdict(int) + # keep adding messages until cam states are equal to MAX_FRAMES + for msg in sorted(logs, key=lambda m: m.logMonoTime): + all_msgs.append(msg) + if msg.which() in frs_types: + cam_state_counts[msg.which()] += 1 + + if all(cam_state_counts[state] == max_frames for state in frs_types): + break + + if len(include_all_types) != 0: + other_msgs = [m for m in logs if m.which() in include_all_types] + all_msgs.extend(other_msgs) + + return all_msgs + + +def model_replay(lr, frs): + # modeld is using frame pairs + modeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"roadCameraState", "wideRoadCameraState"}, {"roadEncodeIdx", "wideRoadEncodeIdx", "carParams"}) + dmodeld_logs = trim_logs_to_max_frames(lr, MAX_FRAMES, {"driverCameraState"}, {"driverEncodeIdx", "carParams"}) + + if not SEND_EXTRA_INPUTS: + modeld_logs = [msg for msg in modeld_logs if msg.which() != 'liveCalibration'] + dmodeld_logs = [msg for msg in dmodeld_logs if msg.which() != 'liveCalibration'] + + # initial setup + for s in ('liveCalibration', 'deviceState'): + msg = next(msg for msg in lr if msg.which() == s).as_builder() + msg.logMonoTime = lr[0].logMonoTime + modeld_logs.insert(1, msg.as_reader()) + dmodeld_logs.insert(1, msg.as_reader()) + + modeld = get_process_config("modeld") + dmonitoringmodeld = get_process_config("dmonitoringmodeld") + + modeld_msgs = replay_process(modeld, modeld_logs, frs) + dmonitoringmodeld_msgs = replay_process(dmonitoringmodeld, dmodeld_logs, frs) + return modeld_msgs + dmonitoringmodeld_msgs + + +if __name__ == "__main__": + update = "--update" in sys.argv + replay_dir = os.path.dirname(os.path.abspath(__file__)) + ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit") + + # load logs + lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) + frs = { + 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera"), readahead=True), + 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera"), readahead=True), + 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera"), readahead=True) + } + + # Update tile refs + if update: + import urllib + import requests + import threading + import http.server + from openpilot.tools.lib.openpilotci import upload_bytes + os.environ['MAPS_HOST'] = 'http://localhost:5000' + + class HTTPRequestHandler(http.server.BaseHTTPRequestHandler): + def do_GET(self): + assert len(self.path) > 10 # Sanity check on path length + r = requests.get(f'https://api.mapbox.com{self.path}', timeout=30) + upload_bytes(r.content, urllib.parse.urlparse(self.path).path.lstrip('/')) + self.send_response(r.status_code) + self.send_header('Content-type','text/html') + self.end_headers() + self.wfile.write(r.content) + + server = http.server.HTTPServer(("127.0.0.1", 5000), HTTPRequestHandler) + thread = threading.Thread(None, server.serve_forever, daemon=True) + thread.start() + else: + os.environ['MAPS_HOST'] = BASE_URL.rstrip('/') + + log_msgs = [] + # run replays + if not NO_MODEL: + log_msgs += model_replay(lr, frs) + + # get diff + failed = False + if not update: + with open(ref_commit_fn) as f: + ref_commit = f.read().strip() + log_fn = get_log_fn(ref_commit, TEST_ROUTE) + try: + all_logs = list(LogReader(BASE_URL + log_fn)) + cmp_log = [] + + # logs are ordered based on type: modelV2, driverStateV2 + if not NO_MODEL: + model_start_index = next(i for i, m in enumerate(all_logs) if m.which() in ("modelV2", "cameraOdometry")) + cmp_log += all_logs[model_start_index:model_start_index + MAX_FRAMES*2] + dmon_start_index = next(i for i, m in enumerate(all_logs) if m.which() == "driverStateV2") + cmp_log += all_logs[dmon_start_index:dmon_start_index + MAX_FRAMES] + + ignore = [ + 'logMonoTime', + 'modelV2.frameDropPerc', + 'modelV2.modelExecutionTime', + 'driverStateV2.modelExecutionTime', + 'driverStateV2.dspExecutionTime' + ] + if PC: + ignore += [ + 'modelV2.laneLines.0.t', + 'modelV2.laneLines.1.t', + 'modelV2.laneLines.2.t', + 'modelV2.laneLines.3.t', + 'modelV2.roadEdges.0.t', + 'modelV2.roadEdges.1.t', + ] + # TODO this tolerance is absurdly large + tolerance = 2.0 if PC else None + results: Any = {TEST_ROUTE: {}} + log_paths: Any = {TEST_ROUTE: {"models": {'ref': BASE_URL + log_fn, 'new': log_fn}}} + results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) + diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) + + if "CI" in os.environ: + print(diff_long) + print('-------------\n'*5) + print(diff_short) + with open("model_diff.txt", "w") as f: + f.write(diff_long) + except Exception as e: + print(str(e)) + failed = True + + # upload new refs + if (update or failed) and not PC: + from openpilot.tools.lib.openpilotci import upload_file + + print("Uploading new refs") + + new_commit = get_commit() + log_fn = get_log_fn(new_commit, TEST_ROUTE) + save_log(log_fn, log_msgs) + try: + upload_file(log_fn, os.path.basename(log_fn)) + except Exception as e: + print("failed to upload", e) + + with open(ref_commit_fn, 'w') as f: + f.write(str(new_commit)) + + print("\n\nNew ref commit: ", new_commit) + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit new file mode 100644 index 0000000000..87f1adfada --- /dev/null +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -0,0 +1 @@ +73eb11111fb6407fa307c3f2bdd3331f2514c707 diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py new file mode 100755 index 0000000000..739efcb985 --- /dev/null +++ b/selfdrive/test/process_replay/process_replay.py @@ -0,0 +1,833 @@ +#!/usr/bin/env python3 +import os +import time +import copy +import json +import heapq +import signal +import platform +from collections import Counter, OrderedDict +from dataclasses import dataclass, field +from typing import Any +from collections.abc import Callable, Iterable +from tqdm import tqdm +import capnp + +import cereal.messaging as messaging +from cereal import car +from cereal.services import SERVICE_LIST +from msgq.visionipc import VisionIpcServer, get_endpoint_name as vipc_get_endpoint_name +from openpilot.common.params import Params +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.common.timeout import Timeout +from openpilot.common.realtime import DT_CTRL +from panda.python import ALTERNATIVE_EXPERIENCE +from openpilot.selfdrive.car.car_helpers import get_car, interfaces +from openpilot.system.manager.process_config import managed_processes +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams +from openpilot.selfdrive.test.process_replay.migration import migrate_all +from openpilot.selfdrive.test.process_replay.capture import ProcessOutputCapture +from openpilot.tools.lib.logreader import LogIterable +from openpilot.tools.lib.framereader import BaseFrameReader + +# Numpy gives different results based on CPU features after version 19 +NUMPY_TOLERANCE = 1e-7 +PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__)) +FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/") + +class DummySocket: + def __init__(self): + self.data: list[bytes] = [] + + def receive(self, non_blocking: bool = False) -> bytes | None: + if non_blocking: + return None + + return self.data.pop() + + def send(self, data: bytes): + self.data.append(data) + +class LauncherWithCapture: + def __init__(self, capture: ProcessOutputCapture, launcher: Callable): + self.capture = capture + self.launcher = launcher + + def __call__(self, *args, **kwargs): + self.capture.link_with_current_proc() + self.launcher(*args, **kwargs) + + +class ReplayContext: + def __init__(self, cfg): + self.proc_name = cfg.proc_name + self.pubs = cfg.pubs + self.main_pub = cfg.main_pub + self.main_pub_drained = cfg.main_pub_drained + self.unlocked_pubs = cfg.unlocked_pubs + assert(len(self.pubs) != 0 or self.main_pub is not None) + + def __enter__(self): + self.open_context() + + return self + + def __exit__(self, exc_type, exc_obj, exc_tb): + self.close_context() + + def open_context(self): + messaging.toggle_fake_events(True) + messaging.set_fake_prefix(self.proc_name) + + if self.main_pub is None: + self.events = OrderedDict() + pubs_with_events = [pub for pub in self.pubs if pub not in self.unlocked_pubs] + for pub in pubs_with_events: + self.events[pub] = messaging.fake_event_handle(pub, enable=True) + else: + self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)} + + def close_context(self): + del self.events + + messaging.toggle_fake_events(False) + messaging.delete_fake_prefix() + + @property + def all_recv_called_events(self): + return [man.recv_called_event for man in self.events.values()] + + @property + def all_recv_ready_events(self): + return [man.recv_ready_event for man in self.events.values()] + + def send_sync(self, pm, endpoint, dat): + self.events[endpoint].recv_called_event.wait() + self.events[endpoint].recv_called_event.clear() + pm.send(endpoint, dat) + self.events[endpoint].recv_ready_event.set() + + def unlock_sockets(self): + expected_sets = len(self.events) + while expected_sets > 0: + index = messaging.wait_for_one_event(self.all_recv_called_events) + self.all_recv_called_events[index].clear() + self.all_recv_ready_events[index].set() + expected_sets -= 1 + + def wait_for_recv_called(self): + messaging.wait_for_one_event(self.all_recv_called_events) + + def wait_for_next_recv(self, trigger_empty_recv): + index = messaging.wait_for_one_event(self.all_recv_called_events) + if self.main_pub is not None and self.main_pub_drained and trigger_empty_recv: + self.all_recv_called_events[index].clear() + self.all_recv_ready_events[index].set() + self.all_recv_called_events[index].wait() + + +@dataclass +class ProcessConfig: + proc_name: str + pubs: list[str] + subs: list[str] + ignore: list[str] + config_callback: Callable | None = None + init_callback: Callable | None = None + should_recv_callback: Callable | None = None + tolerance: float | None = None + processing_time: float = 0.001 + timeout: int = 30 + simulation: bool = True + main_pub: str | None = None + main_pub_drained: bool = True + vision_pubs: list[str] = field(default_factory=list) + ignore_alive_pubs: list[str] = field(default_factory=list) + unlocked_pubs: list[str] = field(default_factory=list) + + +class ProcessContainer: + def __init__(self, cfg: ProcessConfig): + self.prefix = OpenpilotPrefix(clean_dirs_on_exit=False) + self.cfg = copy.deepcopy(cfg) + self.process = copy.deepcopy(managed_processes[cfg.proc_name]) + self.msg_queue: list[capnp._DynamicStructReader] = [] + self.cnt = 0 + self.pm: messaging.PubMaster | None = None + self.sockets: list[messaging.SubSocket] | None = None + self.rc: ReplayContext | None = None + self.vipc_server: VisionIpcServer | None = None + self.environ_config: dict[str, Any] | None = None + self.capture: ProcessOutputCapture | None = None + + @property + def has_empty_queue(self) -> bool: + return len(self.msg_queue) == 0 + + @property + def pubs(self) -> list[str]: + return self.cfg.pubs + + @property + def subs(self) -> list[str]: + return self.cfg.subs + + def _clean_env(self): + for k in self.environ_config.keys(): + if k in os.environ: + del os.environ[k] + + for k in ["PROC_NAME", "SIMULATION"]: + if k in os.environ: + del os.environ[k] + + def _setup_env(self, params_config: dict[str, Any], environ_config: dict[str, Any]): + for k, v in environ_config.items(): + if len(v) != 0: + os.environ[k] = v + elif k in os.environ: + del os.environ[k] + + os.environ["PROC_NAME"] = self.cfg.proc_name + if self.cfg.simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] + + params = Params() + for k, v in params_config.items(): + if isinstance(v, bool): + params.put_bool(k, v) + else: + params.put(k, v) + + self.environ_config = environ_config + + def _setup_vision_ipc(self, all_msgs: LogIterable, frs: dict[str, Any]): + assert len(self.cfg.vision_pubs) != 0 + + vipc_server = VisionIpcServer("camerad") + streams_metas = available_streams(all_msgs) + for meta in streams_metas: + if meta.camera_state in self.cfg.vision_pubs: + frame_size = (frs[meta.camera_state].w, frs[meta.camera_state].h) + vipc_server.create_buffers(meta.stream, 2, False, *frame_size) + vipc_server.start_listener() + + self.vipc_server = vipc_server + self.cfg.vision_pubs = [meta.camera_state for meta in streams_metas if meta.camera_state in self.cfg.vision_pubs] + + def _start_process(self): + if self.capture is not None: + self.process.launcher = LauncherWithCapture(self.capture, self.process.launcher) + self.process.prepare() + self.process.start() + + def start( + self, params_config: dict[str, Any], environ_config: dict[str, Any], + all_msgs: LogIterable, frs: dict[str, BaseFrameReader] | None, + fingerprint: str | None, capture_output: bool + ): + with self.prefix as p: + self._setup_env(params_config, environ_config) + + if self.cfg.config_callback is not None: + params = Params() + self.cfg.config_callback(params, self.cfg, all_msgs) + + self.rc = ReplayContext(self.cfg) + self.rc.open_context() + + self.pm = messaging.PubMaster(self.cfg.pubs) + self.sockets = [messaging.sub_sock(s, timeout=100) for s in self.cfg.subs] + + if len(self.cfg.vision_pubs) != 0: + assert frs is not None + self._setup_vision_ipc(all_msgs, frs) + assert self.vipc_server is not None + + if capture_output: + self.capture = ProcessOutputCapture(self.cfg.proc_name, p.prefix) + + self._start_process() + + if self.cfg.init_callback is not None: + self.cfg.init_callback(self.rc, self.pm, all_msgs, fingerprint) + + # wait for process to startup + with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(self.cfg.proc_name)}"): + while not all(self.pm.all_readers_updated(s) for s in self.cfg.pubs if s not in self.cfg.ignore_alive_pubs): + time.sleep(0) + + def stop(self): + with self.prefix: + self.process.signal(signal.SIGKILL) + self.process.stop() + self.rc.close_context() + self.prefix.clean_dirs() + self._clean_env() + + def run_step(self, msg: capnp._DynamicStructReader, frs: dict[str, BaseFrameReader] | None) -> list[capnp._DynamicStructReader]: + assert self.rc and self.pm and self.sockets and self.process.proc + + output_msgs = [] + with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"): + end_of_cycle = True + if self.cfg.should_recv_callback is not None: + end_of_cycle = self.cfg.should_recv_callback(msg, self.cfg, self.cnt) + + self.msg_queue.append(msg) + if end_of_cycle: + self.rc.wait_for_recv_called() + + # call recv to let sub-sockets reconnect, after we know the process is ready + if self.cnt == 0: + for s in self.sockets: + messaging.recv_one_or_none(s) + + # empty recv on drained pub indicates the end of messages, only do that if there're any + trigger_empty_recv = False + if self.cfg.main_pub and self.cfg.main_pub_drained: + trigger_empty_recv = next((True for m in self.msg_queue if m.which() == self.cfg.main_pub), False) + + for m in self.msg_queue: + self.pm.send(m.which(), m.as_builder()) + # send frames if needed + if self.vipc_server is not None and m.which() in self.cfg.vision_pubs: + camera_state = getattr(m, m.which()) + camera_meta = meta_from_camera_state(m.which()) + assert frs is not None + img = frs[m.which()].get(camera_state.frameId, pix_fmt="nv12")[0] + self.vipc_server.send(camera_meta.stream, img.flatten().tobytes(), + camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) + self.msg_queue = [] + + self.rc.unlock_sockets() + self.rc.wait_for_next_recv(trigger_empty_recv) + + for socket in self.sockets: + ms = messaging.drain_sock(socket) + for m in ms: + m = m.as_builder() + m.logMonoTime = msg.logMonoTime + int(self.cfg.processing_time * 1e9) + output_msgs.append(m.as_reader()) + self.cnt += 1 + assert self.process.proc.is_alive() + + return output_msgs + + +def card_fingerprint_callback(rc, pm, msgs, fingerprint): + print("start fingerprinting") + params = Params() + canmsgs = [msg for msg in msgs if msg.which() == "can"][:300] + + # card expects one arbitrary can and pandaState + rc.send_sync(pm, "can", messaging.new_message("can", 1)) + pm.send("pandaStates", messaging.new_message("pandaStates", 1)) + rc.send_sync(pm, "can", messaging.new_message("can", 1)) + rc.wait_for_next_recv(True) + + # fingerprinting is done, when CarParams is set + while params.get("CarParams") is None: + if len(canmsgs) == 0: + raise ValueError("Fingerprinting failed. Run out of can msgs") + + m = canmsgs.pop(0) + rc.send_sync(pm, "can", m.as_builder().to_bytes()) + rc.wait_for_next_recv(False) + + +def get_car_params_callback(rc, pm, msgs, fingerprint): + params = Params() + if fingerprint: + CarInterface, _, _ = interfaces[fingerprint] + CP = CarInterface.get_non_essential_params(fingerprint) + else: + can = DummySocket() + sendcan = DummySocket() + + canmsgs = [msg for msg in msgs if msg.which() == "can"] + has_cached_cp = params.get("CarParamsCache") is not None + assert len(canmsgs) != 0, "CAN messages are required for fingerprinting" + assert os.environ.get("SKIP_FW_QUERY", False) or has_cached_cp, \ + "CarParamsCache is required for fingerprinting. Make sure to keep carParams msgs in the logs." + + for m in canmsgs[:300]: + can.send(m.as_builder().to_bytes()) + _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) + + if not params.get_bool("DisengageOnAccelerator"): + CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS + + params.put("CarParams", CP.to_bytes()) + return CP + + +def controlsd_rcv_callback(msg, cfg, frame): + return (frame - 1) == 0 or msg.which() == 'carState' + + +def card_rcv_callback(msg, cfg, frame): + # no sendcan until card is initialized + if msg.which() != "can": + return False + + socks = [ + s for s in cfg.subs if + frame % int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency) == 0 + ] + if "sendcan" in socks and (frame - 1) < 2000: + socks.remove("sendcan") + return len(socks) > 0 + + +def calibration_rcv_callback(msg, cfg, frame): + # calibrationd publishes 1 calibrationData every 5 cameraOdometry packets. + # should_recv always true to increment frame + return (frame - 1) == 0 or msg.which() == 'cameraOdometry' + + +def torqued_rcv_callback(msg, cfg, frame): + # should_recv always true to increment frame + return (frame - 1) == 0 or msg.which() == 'liveLocationKalman' + + +def dmonitoringmodeld_rcv_callback(msg, cfg, frame): + return msg.which() == "driverCameraState" + + +class ModeldCameraSyncRcvCallback: + def __init__(self): + self.road_present = False + self.wide_road_present = False + self.is_dual_camera = True + + def __call__(self, msg, cfg, frame): + self.is_dual_camera = len(cfg.vision_pubs) == 2 + if msg.which() == "roadCameraState": + self.road_present = True + elif msg.which() == "wideRoadCameraState": + self.wide_road_present = True + + if self.road_present and self.wide_road_present: + self.road_present, self.wide_road_present = False, False + return True + elif self.road_present and not self.is_dual_camera: + self.road_present = False + return True + else: + return False + + +class MessageBasedRcvCallback: + def __init__(self, trigger_msg_type): + self.trigger_msg_type = trigger_msg_type + + def __call__(self, msg, cfg, frame): + return msg.which() == self.trigger_msg_type + + +class FrequencyBasedRcvCallback: + def __init__(self, trigger_msg_type): + self.trigger_msg_type = trigger_msg_type + + def __call__(self, msg, cfg, frame): + if msg.which() != self.trigger_msg_type: + return False + + resp_sockets = [ + s for s in cfg.subs + if frame % max(1, int(SERVICE_LIST[msg.which()].frequency / SERVICE_LIST[s].frequency)) == 0 + ] + return bool(len(resp_sockets)) + + +def controlsd_config_callback(params, cfg, lr): + controlsState = None + initialized = False + for msg in lr: + if msg.which() == "controlsState": + controlsState = msg.controlsState + if initialized: + break + elif msg.which() == "onroadEvents": + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.onroadEvents] + + assert controlsState is not None and initialized, "controlsState never initialized" + params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) + + +def locationd_config_pubsub_callback(params, cfg, lr): + ublox = params.get_bool("UbloxAvailable") + sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) + + cfg.pubs = set(cfg.pubs) - sub_keys + + +CONFIGS = [ + ProcessConfig( + proc_name="controlsd", + pubs=[ + "carState", "deviceState", "pandaStates", "peripheralState", "liveCalibration", "driverMonitoringState", + "longitudinalPlan", "liveLocationKalman", "liveParameters", "radarState", + "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", + "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope", "carOutput" + ], + subs=["controlsState", "carControl", "onroadEvents"], + ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"], + config_callback=controlsd_config_callback, + init_callback=get_car_params_callback, + should_recv_callback=controlsd_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, + ), + ProcessConfig( + proc_name="card", + pubs=["pandaStates", "carControl", "onroadEvents", "can"], + subs=["sendcan", "carState", "carParams", "carOutput"], + ignore=["logMonoTime", "carState.cumLagMs"], + init_callback=card_fingerprint_callback, + should_recv_callback=card_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, + main_pub="can", + ), + ProcessConfig( + proc_name="radard", + pubs=["can", "carState", "modelV2"], + subs=["radarState", "liveTracks"], + ignore=["logMonoTime", "radarState.cumLagMs"], + init_callback=get_car_params_callback, + should_recv_callback=MessageBasedRcvCallback("can"), + main_pub="can", + ), + ProcessConfig( + proc_name="plannerd", + pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"], + subs=["longitudinalPlan", "uiPlan"], + ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime"], + init_callback=get_car_params_callback, + should_recv_callback=FrequencyBasedRcvCallback("modelV2"), + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="calibrationd", + pubs=["carState", "cameraOdometry", "carParams"], + subs=["liveCalibration"], + ignore=["logMonoTime"], + should_recv_callback=calibration_rcv_callback, + ), + ProcessConfig( + proc_name="dmonitoringd", + pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"], + subs=["driverMonitoringState"], + ignore=["logMonoTime"], + should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"), + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="locationd", + pubs=[ + "cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal", + "liveCalibration", "carState", "gpsLocation" + ], + subs=["liveLocationKalman"], + ignore=["logMonoTime"], + config_callback=locationd_config_pubsub_callback, + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="paramsd", + pubs=["liveLocationKalman", "carState"], + subs=["liveParameters"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=FrequencyBasedRcvCallback("liveLocationKalman"), + tolerance=NUMPY_TOLERANCE, + processing_time=0.004, + ), + ProcessConfig( + proc_name="ubloxd", + pubs=["ubloxRaw"], + subs=["ubloxGnss", "gpsLocationExternal"], + ignore=["logMonoTime"], + ), + ProcessConfig( + proc_name="torqued", + pubs=["liveLocationKalman", "carState", "carControl", "carOutput"], + subs=["liveTorqueParameters"], + ignore=["logMonoTime"], + init_callback=get_car_params_callback, + should_recv_callback=torqued_rcv_callback, + tolerance=NUMPY_TOLERANCE, + ), + ProcessConfig( + proc_name="modeld", + pubs=["deviceState", "roadCameraState", "wideRoadCameraState", "liveCalibration", "driverMonitoringState"], + subs=["modelV2", "cameraOdometry"], + ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"], + should_recv_callback=ModeldCameraSyncRcvCallback(), + tolerance=NUMPY_TOLERANCE, + processing_time=0.020, + main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("roadCameraState").stream), + main_pub_drained=False, + vision_pubs=["roadCameraState", "wideRoadCameraState"], + ignore_alive_pubs=["wideRoadCameraState"], + init_callback=get_car_params_callback, + ), + ProcessConfig( + proc_name="dmonitoringmodeld", + pubs=["liveCalibration", "driverCameraState"], + subs=["driverStateV2"], + ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.dspExecutionTime"], + should_recv_callback=dmonitoringmodeld_rcv_callback, + tolerance=NUMPY_TOLERANCE, + processing_time=0.020, + main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), + main_pub_drained=False, + vision_pubs=["driverCameraState"], + ignore_alive_pubs=["driverCameraState"], + ), +] + + +def get_process_config(name: str) -> ProcessConfig: + try: + return copy.deepcopy(next(c for c in CONFIGS if c.proc_name == name)) + except StopIteration as ex: + raise Exception(f"Cannot find process config with name: {name}") from ex + + +def get_custom_params_from_lr(lr: LogIterable, initial_state: str = "first") -> dict[str, Any]: + """ + Use this to get custom params dict based on provided logs. + Useful when replaying following processes: calibrationd, paramsd, torqued + The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. + """ + + car_params = [m for m in lr if m.which() == "carParams"] + live_calibration = [m for m in lr if m.which() == "liveCalibration"] + live_parameters = [m for m in lr if m.which() == "liveParameters"] + live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] + + assert initial_state in ["first", "last"] + msg_index = 0 if initial_state == "first" else -1 + + assert len(car_params) > 0, "carParams required for initial state of liveParameters and CarParamsPrevRoute" + CP = car_params[msg_index].carParams + + custom_params = { + "CarParamsPrevRoute": CP.as_builder().to_bytes() + } + + if len(live_calibration) > 0: + custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() + if len(live_parameters) > 0: + lp_dict = live_parameters[msg_index].to_dict() + lp_dict["carFingerprint"] = CP.carFingerprint + custom_params["LiveParameters"] = json.dumps(lp_dict) + if len(live_torque_parameters) > 0: + custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() + + return custom_params + + +def replay_process_with_name(name: str | Iterable[str], lr: LogIterable, *args, **kwargs) -> list[capnp._DynamicStructReader]: + if isinstance(name, str): + cfgs = [get_process_config(name)] + elif isinstance(name, Iterable): + cfgs = [get_process_config(n) for n in name] + else: + raise ValueError("name must be str or collections of strings") + + return replay_process(cfgs, lr, *args, **kwargs) + + +def replay_process( + cfg: ProcessConfig | Iterable[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] = None, + fingerprint: str = None, return_all_logs: bool = False, custom_params: dict[str, Any] = None, + captured_output_store: dict[str, dict[str, str]] = None, disable_progress: bool = False +) -> list[capnp._DynamicStructReader]: + if isinstance(cfg, Iterable): + cfgs = list(cfg) + else: + cfgs = [cfg] + + all_msgs = migrate_all(lr, old_logtime=True, + manager_states=True, + panda_states=any("pandaStates" in cfg.pubs for cfg in cfgs), + camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs)) + process_logs = _replay_multi_process(cfgs, all_msgs, frs, fingerprint, custom_params, captured_output_store, disable_progress) + + if return_all_logs: + keys = {m.which() for m in process_logs} + modified_logs = [m for m in all_msgs if m.which() not in keys] + modified_logs.extend(process_logs) + modified_logs.sort(key=lambda m: int(m.logMonoTime)) + log_msgs = modified_logs + else: + log_msgs = process_logs + + return log_msgs + + +def _replay_multi_process( + cfgs: list[ProcessConfig], lr: LogIterable, frs: dict[str, BaseFrameReader] | None, fingerprint: str | None, + custom_params: dict[str, Any] | None, captured_output_store: dict[str, dict[str, str]] | None, disable_progress: bool +) -> list[capnp._DynamicStructReader]: + if fingerprint is not None: + params_config = generate_params_config(lr=lr, fingerprint=fingerprint, custom_params=custom_params) + env_config = generate_environ_config(fingerprint=fingerprint) + else: + CP = next((m.carParams for m in lr if m.which() == "carParams"), None) + params_config = generate_params_config(lr=lr, CP=CP, custom_params=custom_params) + env_config = generate_environ_config(CP=CP) + + # validate frs and vision pubs + all_vision_pubs = [pub for cfg in cfgs for pub in cfg.vision_pubs] + if len(all_vision_pubs) != 0: + assert frs is not None, "frs must be provided when replaying process using vision streams" + assert all(meta_from_camera_state(st) is not None for st in all_vision_pubs), \ + f"undefined vision stream spotted, probably misconfigured process: (vision pubs: {all_vision_pubs})" + required_vision_pubs = {m.camera_state for m in available_streams(lr)} & set(all_vision_pubs) + assert all(st in frs for st in required_vision_pubs), f"frs for this process must contain following vision streams: {required_vision_pubs}" + + all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) + log_msgs = [] + try: + containers = [] + for cfg in cfgs: + container = ProcessContainer(cfg) + containers.append(container) + container.start(params_config, env_config, all_msgs, frs, 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} + lr_pubs = all_pubs - all_subs + pubs_to_containers = {pub: [container for container in containers if pub in container.pubs] for pub in all_pubs} + + pub_msgs = [msg for msg in all_msgs if msg.which() in lr_pubs] + # external queue for messages taken from logs; internal queue for messages generated by processes, which will be republished + external_pub_queue: list[capnp._DynamicStructReader] = pub_msgs.copy() + internal_pub_queue: list[capnp._DynamicStructReader] = [] + # heap for maintaining the order of messages generated by processes, where each element: (logMonoTime, index in internal_pub_queue) + internal_pub_index_heap: list[tuple[int, int]] = [] + + pbar = tqdm(total=len(external_pub_queue), disable=disable_progress) + while len(external_pub_queue) != 0 or (len(internal_pub_index_heap) != 0 and not all(c.has_empty_queue for c in containers)): + if len(internal_pub_index_heap) == 0 or (len(external_pub_queue) != 0 and external_pub_queue[0].logMonoTime < internal_pub_index_heap[0][0]): + msg = external_pub_queue.pop(0) + pbar.update(1) + else: + _, index = heapq.heappop(internal_pub_index_heap) + msg = internal_pub_queue[index] + + target_containers = pubs_to_containers[msg.which()] + for container in target_containers: + output_msgs = container.run_step(msg, frs) + for m in output_msgs: + if m.which() in all_pubs: + internal_pub_queue.append(m) + heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1)) + log_msgs.extend(output_msgs) + finally: + for container in containers: + container.stop() + if captured_output_store is not None: + assert container.capture is not None + out, err = container.capture.read_outerr() + captured_output_store[container.cfg.proc_name] = {"out": out, "err": err} + + return log_msgs + + +def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=None) -> dict[str, Any]: + params_dict = { + "OpenpilotEnabledToggle": True, + "DisengageOnAccelerator": True, + "DisableLogging": False, + } + + if custom_params is not None: + params_dict.update(custom_params) + if lr is not None: + has_ublox = any(msg.which() == "ubloxGnss" for msg in lr) + params_dict["UbloxAvailable"] = has_ublox + is_rhd = next((msg.driverMonitoringState.isRHD for msg in lr if msg.which() == "driverMonitoringState"), False) + params_dict["IsRhdDetected"] = is_rhd + + if CP is not None: + if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: + params_dict["DisengageOnAccelerator"] = False + + if fingerprint is None: + if CP.fingerprintSource == "fw": + params_dict["CarParamsCache"] = CP.as_builder().to_bytes() + + if CP.openpilotLongitudinalControl: + params_dict["ExperimentalLongitudinalEnabled"] = True + + if CP.notCar: + params_dict["JoystickDebugMode"] = True + + return params_dict + + +def generate_environ_config(CP=None, fingerprint=None, log_dir=None) -> dict[str, Any]: + environ_dict = {} + if platform.system() != "Darwin": + environ_dict["PARAMS_ROOT"] = "/dev/shm/params" + if log_dir is not None: + environ_dict["LOG_ROOT"] = log_dir + + environ_dict["REPLAY"] = "1" + + # Regen or python process + if CP is not None and fingerprint is None: + if CP.fingerprintSource == "fw": + environ_dict['SKIP_FW_QUERY'] = "" + environ_dict['FINGERPRINT'] = "" + else: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = CP.carFingerprint + elif fingerprint is not None: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = fingerprint + else: + environ_dict["SKIP_FW_QUERY"] = "" + environ_dict["FINGERPRINT"] = "" + + return environ_dict + + +def check_openpilot_enabled(msgs: LogIterable) -> bool: + cur_enabled_count = 0 + max_enabled_count = 0 + for msg in msgs: + if msg.which() == "carParams": + if msg.carParams.notCar: + return True + elif msg.which() == "controlsState": + if msg.controlsState.active: + cur_enabled_count += 1 + else: + cur_enabled_count = 0 + max_enabled_count = max(max_enabled_count, cur_enabled_count) + + return max_enabled_count > int(10. / DT_CTRL) + + +def check_most_messages_valid(msgs: LogIterable, threshold: float = 0.9) -> bool: + msgs_counts = Counter(msg.which() for msg in msgs) + msgs_valid_counts = Counter(msg.which() for msg in msgs if msg.valid) + + most_valid_for_service = {} + for msg_type in msgs_counts.keys(): + valid_share = msgs_valid_counts.get(msg_type, 0) / msgs_counts[msg_type] + ok = valid_share >= threshold + if not ok: + print(f"WARNING: Service {msg_type} has {valid_share * 100:.2f}% valid messages, which is below threshold of {threshold * 100:.2f}%") + most_valid_for_service[msg_type] = ok + + return all(most_valid_for_service.values()) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit new file mode 100644 index 0000000000..70178b8ea3 --- /dev/null +++ b/selfdrive/test/process_replay/ref_commit @@ -0,0 +1 @@ +e536df5586a71b22baa789dc584d7eab67f1fbbb diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py new file mode 100755 index 0000000000..bf7a4bfd97 --- /dev/null +++ b/selfdrive/test/process_replay/regen.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +import os +import argparse +import time +import capnp +import numpy as np + +from typing import Any +from collections.abc import Iterable + +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \ + check_openpilot_enabled, check_most_messages_valid, get_custom_params_from_lr +from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES +from openpilot.selfdrive.test.update_ci_routes import upload_route +from openpilot.tools.lib.route import Route +from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType +from openpilot.tools.lib.logreader import LogReader, LogIterable +from openpilot.tools.lib.helpers import save_log + + +class DummyFrameReader(BaseFrameReader): + def __init__(self, w: int, h: int, frame_count: int, pix_val: int): + self.pix_val = pix_val + self.w, self.h = w, h + self.frame_count = frame_count + self.frame_type = FrameType.raw + + def get(self, idx, count=1, pix_fmt="yuv420p"): + if pix_fmt == "rgb24": + shape = (self.h, self.w, 3) + elif pix_fmt == "nv12" or pix_fmt == "yuv420p": + shape = (int((self.h * self.w) * 3 / 2),) + else: + raise NotImplementedError + + return [np.full(shape, self.pix_val, dtype=np.uint8) for _ in range(count)] + + @staticmethod + def zero_dcamera(): + return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0) + + +def regen_segment( + lr: LogIterable, frs: dict[str, Any] = None, + processes: Iterable[ProcessConfig] = CONFIGS, disable_tqdm: bool = False +) -> list[capnp._DynamicStructReader]: + all_msgs = sorted(lr, key=lambda m: m.logMonoTime) + custom_params = get_custom_params_from_lr(all_msgs) + + print("Replayed processes:", [p.proc_name for p in processes]) + print("\n\n", "*"*30, "\n\n", sep="") + + output_logs = replay_process(processes, all_msgs, frs, return_all_logs=True, custom_params=custom_params, disable_progress=disable_tqdm) + + return output_logs + + +def setup_data_readers( + route: str, sidx: int, use_route_meta: bool, + needs_driver_cam: bool = True, needs_road_cam: bool = True, dummy_driver_cam: bool = False +) -> tuple[LogReader, dict[str, Any]]: + if use_route_meta: + r = Route(route) + lr = LogReader(r.log_paths()[sidx]) + frs = {} + if needs_road_cam and len(r.camera_paths()) > sidx and r.camera_paths()[sidx] is not None: + frs['roadCameraState'] = FrameReader(r.camera_paths()[sidx]) + if needs_road_cam and len(r.ecamera_paths()) > sidx and r.ecamera_paths()[sidx] is not None: + frs['wideRoadCameraState'] = FrameReader(r.ecamera_paths()[sidx]) + if needs_driver_cam: + if dummy_driver_cam: + frs['driverCameraState'] = DummyFrameReader.zero_dcamera() + elif len(r.dcamera_paths()) > sidx and r.dcamera_paths()[sidx] is not None: + device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") + assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." + frs['driverCameraState'] = FrameReader(r.dcamera_paths()[sidx]) + else: + lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") + frs = {} + if needs_road_cam: + frs['roadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs['wideRoadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/ecamera.hevc") + if needs_driver_cam: + if dummy_driver_cam: + frs['driverCameraState'] = DummyFrameReader.zero_dcamera() + else: + device_type = next(str(msg.initData.deviceType) for msg in lr if msg.which() == "initData") + assert device_type != "neo", "Driver camera not supported on neo segments. Use dummy dcamera." + frs['driverCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/dcamera.hevc") + + return lr, frs + + +def regen_and_save( + route: str, sidx: int, processes: str | Iterable[str] = "all", outdir: str = FAKEDATA, + upload: bool = False, use_route_meta: bool = False, disable_tqdm: bool = False, dummy_driver_cam: bool = False +) -> str: + if not isinstance(processes, str) and not hasattr(processes, "__iter__"): + raise ValueError("whitelist_proc must be a string or iterable") + + if processes != "all": + if isinstance(processes, str): + raise ValueError(f"Invalid value for processes: {processes}") + + replayed_processes = [] + for d in processes: + cfg = get_process_config(d) + replayed_processes.append(cfg) + else: + replayed_processes = CONFIGS + + all_vision_pubs = {pub for cfg in replayed_processes for pub in cfg.vision_pubs} + lr, frs = setup_data_readers(route, sidx, use_route_meta, + needs_driver_cam="driverCameraState" in all_vision_pubs, + needs_road_cam="roadCameraState" in all_vision_pubs or "wideRoadCameraState" in all_vision_pubs, + dummy_driver_cam=dummy_driver_cam) + output_logs = regen_segment(lr, frs, replayed_processes, disable_tqdm=disable_tqdm) + + log_dir = os.path.join(outdir, time.strftime("%Y-%m-%d--%H-%M-%S--0", time.gmtime())) + rel_log_dir = os.path.relpath(log_dir) + rpath = os.path.join(log_dir, "rlog.bz2") + + os.makedirs(log_dir) + save_log(rpath, output_logs, compress=True) + + print("\n\n", "*"*30, "\n\n", sep="") + print("New route:", rel_log_dir, "\n") + + if not check_openpilot_enabled(output_logs): + raise Exception("Route did not engage for long enough") + if not check_most_messages_valid(output_logs): + raise Exception("Route has too many invalid messages") + + if upload: + upload_route(rel_log_dir) + + return rel_log_dir + + +if __name__ == "__main__": + def comma_separated_list(string): + return string.split(",") + + all_procs = [p.proc_name for p in CONFIGS] + parser = argparse.ArgumentParser(description="Generate new segments from old ones") + parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") + parser.add_argument("--outdir", help="log output dir", default=FAKEDATA) + parser.add_argument("--dummy-dcamera", action='store_true', help="Use dummy blank driver camera") + parser.add_argument("--whitelist-procs", type=comma_separated_list, default=all_procs, + help="Comma-separated whitelist of processes to regen (e.g. controlsd,radard)") + parser.add_argument("--blacklist-procs", type=comma_separated_list, default=[], + help="Comma-separated blacklist of processes to regen (e.g. controlsd,radard)") + parser.add_argument("route", type=str, help="The source route") + parser.add_argument("seg", type=int, help="Segment in source route") + args = parser.parse_args() + + blacklist_set = set(args.blacklist_procs) + processes = [p for p in args.whitelist_procs if p not in blacklist_set] + regen_and_save(args.route, args.seg, processes=processes, upload=args.upload, outdir=args.outdir, dummy_driver_cam=args.dummy_dcamera) diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py new file mode 100755 index 0000000000..656a5b89e1 --- /dev/null +++ b/selfdrive/test/process_replay/regen_all.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +import argparse +import concurrent.futures +import os +import random +import traceback +from tqdm import tqdm + +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.selfdrive.test.process_replay.regen import regen_and_save +from openpilot.selfdrive.test.process_replay.test_processes import FAKEDATA, source_segments as segments +from openpilot.tools.lib.route import SegmentName + + +def regen_job(segment, upload, disable_tqdm): + with OpenpilotPrefix(): + sn = SegmentName(segment[1]) + fake_dongle_id = 'regen' + ''.join(random.choice('0123456789ABCDEF') for _ in range(11)) + try: + relr = regen_and_save(sn.route_name.canonical_name, sn.segment_num, upload=upload, use_route_meta=False, + outdir=os.path.join(FAKEDATA, fake_dongle_id), disable_tqdm=disable_tqdm, dummy_driver_cam=True) + relr = '|'.join(relr.split('/')[-2:]) + return f' ("{segment[0]}", "{relr}"), ' + except Exception as e: + err = f" {segment} failed: {str(e)}" + err += traceback.format_exc() + err += "\n\n" + return err + + +if __name__ == "__main__": + all_cars = {car for car, _ in segments} + + parser = argparse.ArgumentParser(description="Generate new segments from old ones") + parser.add_argument("-j", "--jobs", type=int, default=1) + parser.add_argument("--no-upload", action="store_true") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + args = parser.parse_args() + + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + tested_segments = [(car, segment) for car, segment in segments if car in tested_cars] + + with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: + p = pool.map(regen_job, tested_segments, [not args.no_upload] * len(tested_segments), [args.jobs > 1] * len(tested_segments)) + msg = "Copy these new segments into test_processes.py:" + for seg in tqdm(p, desc="Generating segments", total=len(tested_segments)): + msg += "\n" + str(seg) + print() + print() + print(msg) diff --git a/selfdrive/test/process_replay/test_fuzzy.py b/selfdrive/test/process_replay/test_fuzzy.py new file mode 100644 index 0000000000..c802d9c573 --- /dev/null +++ b/selfdrive/test/process_replay/test_fuzzy.py @@ -0,0 +1,28 @@ +import copy +from hypothesis import given, HealthCheck, Phase, settings +import hypothesis.strategies as st +from parameterized import parameterized + +from cereal import log +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.test.fuzzy_generation import FuzzyGenerator +import openpilot.selfdrive.test.process_replay.process_replay as pr + +# These processes currently fail because of unrealistic data breaking assumptions +# that openpilot makes causing error with NaN, inf, int size, array indexing ... +# TODO: Make each one testable +NOT_TESTED = ['controlsd', 'card', 'plannerd', 'calibrationd', 'dmonitoringd', 'paramsd', 'dmonitoringmodeld', 'modeld'] + +TEST_CASES = [(cfg.proc_name, copy.deepcopy(cfg)) for cfg in pr.CONFIGS if cfg.proc_name not in NOT_TESTED] + +class TestFuzzProcesses: + + # TODO: make this faster and increase examples + @parameterized.expand(TEST_CASES) + @given(st.data()) + @settings(phases=[Phase.generate, Phase.target], max_examples=10, deadline=1000, suppress_health_check=[HealthCheck.too_slow, HealthCheck.data_too_large]) + def test_fuzz_process(self, proc_name, cfg, data): + msgs = FuzzyGenerator.get_random_event_msg(data.draw, events=cfg.pubs, real_floats=True) + lr = [log.Event.new_message(**m).as_reader() for m in msgs] + cfg.timeout = 5 + pr.replay_process(cfg, lr, fingerprint=TOYOTA.TOYOTA_COROLLA_TSS2, disable_progress=True) diff --git a/selfdrive/test/process_replay/test_imgproc.py b/selfdrive/test/process_replay/test_imgproc.py new file mode 100644 index 0000000000..27d0541a50 --- /dev/null +++ b/selfdrive/test/process_replay/test_imgproc.py @@ -0,0 +1,98 @@ +import os +import numpy as np +import hashlib + +import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` + +from openpilot.system.hardware import PC, TICI +from openpilot.common.basedir import BASEDIR +from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.system.camerad.snapshot.snapshot import yuv_to_rgb +from openpilot.tools.lib.logreader import LogReader + +# TODO: check all sensors +TEST_ROUTE = "8345e3b82948d454|2022-05-04--13-45-33/0" + +cam = DEVICE_CAMERAS[("tici", "ar0231")] +FRAME_WIDTH, FRAME_HEIGHT = (cam.dcam.width, cam.dcam.height) +FRAME_STRIDE = FRAME_WIDTH * 12 // 8 + 4 + +UV_WIDTH = FRAME_WIDTH // 2 +UV_HEIGHT = FRAME_HEIGHT // 2 +UV_SIZE = UV_WIDTH * UV_HEIGHT + + +def init_kernels(frame_offset=0): + ctx = cl.create_some_context(interactive=False) + + with open(os.path.join(BASEDIR, 'system/camerad/cameras/process_raw.cl')) as f: + build_args = f' -cl-fast-relaxed-math -cl-denorms-are-zero -cl-single-precision-constant -I{BASEDIR}/system/camerad/sensors ' + \ + f' -DFRAME_WIDTH={FRAME_WIDTH} -DFRAME_HEIGHT={FRAME_WIDTH} -DFRAME_STRIDE={FRAME_STRIDE} -DFRAME_OFFSET={frame_offset} ' + \ + f' -DRGB_WIDTH={FRAME_WIDTH} -DRGB_HEIGHT={FRAME_HEIGHT} -DYUV_STRIDE={FRAME_WIDTH} -DUV_OFFSET={FRAME_WIDTH*FRAME_HEIGHT}' + \ + ' -DSENSOR_ID=1 -DVIGNETTING=0 ' + if PC: + build_args += ' -DHALF_AS_FLOAT=1 -cl-std=CL2.0' + imgproc_prg = cl.Program(ctx, f.read()).build(options=build_args) + + return ctx, imgproc_prg + +def proc_frame(ctx, imgproc_prg, data, rgb=False): + q = cl.CommandQueue(ctx) + + yuv_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2, dtype=np.uint8) + + cam_g = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=data) + yuv_g = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2) + + krn = imgproc_prg.process_raw + krn.set_scalar_arg_dtypes([None, None, np.int32]) + local_worksize = (20, 20) if TICI else (4, 4) + + ev1 = krn(q, (FRAME_WIDTH//2, FRAME_HEIGHT//2), local_worksize, cam_g, yuv_g, 1) + cl.enqueue_copy(q, yuv_buff, yuv_g, wait_for=[ev1]).wait() + cl.enqueue_barrier(q) + + y = yuv_buff[:FRAME_WIDTH*FRAME_HEIGHT].reshape((FRAME_HEIGHT, FRAME_WIDTH)) + u = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT::2].reshape((UV_HEIGHT, UV_WIDTH)) + v = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT+1::2].reshape((UV_HEIGHT, UV_WIDTH)) + + if rgb: + return yuv_to_rgb(y, u, v) + else: + return y, u, v + + +def imgproc_replay(lr): + ctx, imgproc_prg = init_kernels() + + frames = [] + for m in lr: + if m.which() == 'roadCameraState': + cs = m.roadCameraState + if cs.image: + data = np.frombuffer(cs.image, dtype=np.uint8) + img = proc_frame(ctx, imgproc_prg, data) + + frames.append(img) + + return frames + + +if __name__ == "__main__": + # load logs + lr = list(LogReader(TEST_ROUTE)) + # run replay + out_frames = imgproc_replay(lr) + + all_pix = np.concatenate([np.concatenate([d.flatten() for d in f]) for f in out_frames]) + pix_hash = hashlib.sha1(all_pix).hexdigest() + + with open('imgproc_replay_ref_hash') as f: + ref_hash = f.read() + + if pix_hash != ref_hash: + print("result changed! please check kernel") + print(f"ref: {ref_hash}") + print(f"new: {pix_hash}") + else: + print("test passed") diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py new file mode 100755 index 0000000000..533ab125f9 --- /dev/null +++ b/selfdrive/test/process_replay/test_processes.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +import argparse +import concurrent.futures +import os +import sys +from collections import defaultdict +from tqdm import tqdm +from typing import Any + +from openpilot.common.git import get_commit +from openpilot.selfdrive.car.car_helpers import interface_names +from openpilot.tools.lib.openpilotci import get_url, upload_file +from openpilot.selfdrive.test.process_replay.compare_logs import compare_logs, format_diff +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, FAKEDATA, replay_process, \ + check_openpilot_enabled, check_most_messages_valid +from openpilot.tools.lib.filereader import FileReader +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.helpers import save_log + +source_segments = [ + ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.COMMA_BODY + ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.HYUNDAI_SONATA + ("HYUNDAI2", "d545129f3ca90f28|2022-11-07--20-43-08--3"), # HYUNDAI.HYUNDAI_KIA_EV6 (+ QCOM GPS) + ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.TOYOTA_PRIUS + ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.TOYOTA_RAV4 + ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.TOYOTA_COROLLA_TSS2 + ("HONDA", "eb140f119469d9ab|2021-06-12--10-46-24--27"), # HONDA.HONDA_CIVIC (NIDEC) + ("HONDA2", "7d2244f34d1bbcda|2021-06-25--12-25-37--26"), # HONDA.HONDA_ACCORD (BOSCH) + ("CHRYSLER", "4deb27de11bee626|2021-02-20--11-28-55--8"), # CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID + ("RAM", "17fc16d840fe9d21|2023-04-26--13-28-44--5"), # CHRYSLER.RAM_1500_5TH_GEN + ("SUBARU", "341dccd5359e3c97|2022-09-12--10-35-33--3"), # SUBARU.SUBARU_OUTBACK + ("GM", "0c58b6a25109da2b|2021-02-23--16-35-50--11"), # GM.CHEVROLET_VOLT + ("GM2", "376bf99325883932|2022-10-27--13-41-22--1"), # GM.CHEVROLET_BOLT_EUV + ("NISSAN", "35336926920f3571|2021-02-12--18-38-48--46"), # NISSAN.NISSAN_XTRAIL + ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.VOLKSWAGEN_GOLF + ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.MAZDA_CX9_2021 + ("FORD", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.FORD_BRONCO_SPORT_MK1 + + # Enable when port is tested and dashcamOnly is no longer set + #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.TESLA_AP2_MODELS + #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.VOLKSWAGEN_PASSAT_NMS +] + +segments = [ + ("BODY", "regen29FD9FF7760|2024-05-21--06-58-51--0"), + ("HYUNDAI", "regen0B1B76A1C27|2024-05-21--06-57-53--0"), + ("HYUNDAI2", "regen3BB55FA5E20|2024-05-21--06-59-03--0"), + ("TOYOTA", "regenF6FB954C1E2|2024-05-21--06-57-53--0"), + ("TOYOTA2", "regen0AC637CE7BA|2024-05-21--06-57-54--0"), + ("TOYOTA3", "regenC7BE3FAE496|2024-05-21--06-59-01--0"), + ("HONDA", "regen58E9F8B695A|2024-05-21--06-57-55--0"), + ("HONDA2", "regen8695608EB15|2024-05-21--06-57-55--0"), + ("CHRYSLER", "regenB0F8C25C902|2024-05-21--06-59-47--0"), + ("RAM", "regenB3B2C7A105B|2024-05-21--07-00-47--0"), + ("SUBARU", "regen860FD736DCC|2024-05-21--07-00-50--0"), + ("GM", "regen8CB3048DEB9|2024-05-21--06-59-49--0"), + ("GM2", "regen379D446541D|2024-05-21--07-00-51--0"), + ("NISSAN", "regen24871108F80|2024-05-21--07-00-38--0"), + ("VOLKSWAGEN", "regenF390392F275|2024-05-21--07-00-52--0"), + ("MAZDA", "regenE5A36020581|2024-05-21--07-01-51--0"), + ("FORD", "regenDC288ED0D78|2024-05-21--07-02-18--0"), +] + +# dashcamOnly makes don't need to be tested until a full port is done +excluded_interfaces = ["mock", "tesla"] + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" +REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit") +EXCLUDED_PROCS = {"modeld", "dmonitoringmodeld"} + + +def run_test_process(data): + segment, cfg, args, cur_log_fn, ref_log_path, lr_dat = data + res = None + if not args.upload_only: + lr = LogReader.from_bytes(lr_dat) + res, log_msgs = test_process(cfg, lr, segment, ref_log_path, cur_log_fn, args.ignore_fields, args.ignore_msgs) + # save logs so we can upload when updating refs + save_log(cur_log_fn, log_msgs) + + if args.update_refs or args.upload_only: + print(f'Uploading: {os.path.basename(cur_log_fn)}') + assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}" + upload_file(cur_log_fn, os.path.basename(cur_log_fn)) + os.remove(cur_log_fn) + return (segment, cfg.proc_name, res) + + +def get_log_data(segment): + r, n = segment.rsplit("--", 1) + with FileReader(get_url(r, n)) as f: + return (segment, f.read()) + + +def test_process(cfg, lr, segment, ref_log_path, new_log_path, ignore_fields=None, ignore_msgs=None): + if ignore_fields is None: + ignore_fields = [] + if ignore_msgs is None: + ignore_msgs = [] + + ref_log_msgs = list(LogReader(ref_log_path)) + + try: + log_msgs = replay_process(cfg, lr, disable_progress=True) + except Exception as e: + raise Exception("failed on segment: " + segment) from e + + # check to make sure openpilot is engaged in the route + if cfg.proc_name == "controlsd": + if not check_openpilot_enabled(log_msgs): + return f"Route did not enable at all or for long enough: {new_log_path}", log_msgs + if not check_most_messages_valid(log_msgs): + return f"Route did not have enough valid messages: {new_log_path}", log_msgs + + if cfg.proc_name != 'ubloxd' or segment != 'regen3BB55FA5E20|2024-05-21--06-59-03--0': + seen_msgs = {m.which() for m in log_msgs} + expected_msgs = set(cfg.subs) + if seen_msgs != expected_msgs: + return f"Expected messages: {expected_msgs}, but got: {seen_msgs}", log_msgs + + try: + return compare_logs(ref_log_msgs, log_msgs, ignore_fields + cfg.ignore, ignore_msgs, cfg.tolerance), log_msgs + except Exception as e: + return str(e), log_msgs + + +if __name__ == "__main__": + all_cars = {car for car, _ in segments} + all_procs = {cfg.proc_name for cfg in CONFIGS if cfg.proc_name not in EXCLUDED_PROCS} + + cpu_count = os.cpu_count() or 1 + + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=all_procs, + help="Whitelist given processes from the test (e.g. controlsd)") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=all_cars, + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], + help="Blacklist given processes from the test (e.g. controlsd)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + parser.add_argument("--ignore-fields", type=str, nargs="*", default=[], + help="Extra fields or msgs to ignore (e.g. carState.events)") + parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[], + help="Msgs to ignore (e.g. carEvents)") + parser.add_argument("--update-refs", action="store_true", + help="Updates reference logs using current commit") + parser.add_argument("--upload-only", action="store_true", + help="Skips testing processes and uploads logs from previous test run") + parser.add_argument("-j", "--jobs", type=int, default=max(cpu_count - 2, 1), + help="Max amount of parallel jobs") + args = parser.parse_args() + + tested_procs = set(args.whitelist_procs) - set(args.blacklist_procs) + tested_cars = set(args.whitelist_cars) - set(args.blacklist_cars) + tested_cars = {c.upper() for c in tested_cars} + + full_test = (tested_procs == all_procs) and (tested_cars == all_cars) and all(len(x) == 0 for x in (args.ignore_fields, args.ignore_msgs)) + upload = args.update_refs or args.upload_only + os.makedirs(os.path.dirname(FAKEDATA), exist_ok=True) + + if upload: + assert full_test, "Need to run full test when updating refs" + + try: + with open(REF_COMMIT_FN) as f: + ref_commit = f.read().strip() + except FileNotFoundError: + print("Couldn't find reference commit") + sys.exit(1) + + cur_commit = get_commit() + if not cur_commit: + raise Exception("Couldn't get current commit") + + print(f"***** testing against commit {ref_commit} *****") + + # check to make sure all car brands are tested + if full_test: + untested = (set(interface_names) - set(excluded_interfaces)) - {c.lower() for c in tested_cars} + assert len(untested) == 0, f"Cars missing routes: {str(untested)}" + + log_paths: defaultdict[str, dict[str, dict[str, str]]] = defaultdict(lambda: defaultdict(dict)) + with concurrent.futures.ProcessPoolExecutor(max_workers=args.jobs) as pool: + if not args.upload_only: + download_segments = [seg for car, seg in segments if car in tested_cars] + log_data: dict[str, LogReader] = {} + p1 = pool.map(get_log_data, download_segments) + for segment, lr in tqdm(p1, desc="Getting Logs", total=len(download_segments)): + log_data[segment] = lr + + pool_args: Any = [] + for car_brand, segment in segments: + if car_brand not in tested_cars: + continue + + for cfg in CONFIGS: + if cfg.proc_name not in tested_procs: + continue + + cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2") + if args.update_refs: # reference logs will not exist if routes were just regenerated + ref_log_path = get_url(*segment.rsplit("--", 1)) + else: + ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") + ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) + + dat = None if args.upload_only else log_data[segment] + pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) + + log_paths[segment][cfg.proc_name]['ref'] = ref_log_path + log_paths[segment][cfg.proc_name]['new'] = cur_log_fn + + results: Any = defaultdict(dict) + p2 = pool.map(run_test_process, pool_args) + for (segment, proc, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): + if not args.upload_only: + results[segment][proc] = result + + diff_short, diff_long, failed = format_diff(results, log_paths, ref_commit) + if not upload: + with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f: + f.write(diff_long) + print(diff_short) + + if failed: + print("TEST FAILED") + print("\n\nTo push the new reference logs for this commit run:") + print("./test_processes.py --upload-only") + else: + print("TEST SUCCEEDED") + + else: + with open(REF_COMMIT_FN, "w") as f: + f.write(cur_commit) + print(f"\n\nUpdated reference logs for commit: {cur_commit}") + + sys.exit(int(failed)) diff --git a/selfdrive/test/process_replay/test_regen.py b/selfdrive/test/process_replay/test_regen.py new file mode 100644 index 0000000000..17fefcb497 --- /dev/null +++ b/selfdrive/test/process_replay/test_regen.py @@ -0,0 +1,37 @@ +from parameterized import parameterized + +from openpilot.selfdrive.test.process_replay.regen import regen_segment, DummyFrameReader +from openpilot.selfdrive.test.process_replay.process_replay import check_openpilot_enabled +from openpilot.tools.lib.openpilotci import get_url +from openpilot.tools.lib.logreader import LogReader +from openpilot.tools.lib.framereader import FrameReader + +TESTED_SEGMENTS = [ + ("PRIUS_C2", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.TOYOTA_PRIUS: NEO, pandaStateDEPRECATED, no peripheralState, sensorEventsDEPRECATED + # Enable these once regen on CI becomes faster or use them for different tests running controlsd in isolation + # ("MAZDA_C3", "bd6a637565e91581|2021-10-30--15-14-53--4"), # MAZDA.CX9_2021: TICI, incomplete managerState + # ("FORD_C3", "54827bf84c38b14f|2023-01-26--21-59-07--4"), # FORD.BRONCO_SPORT_MK1: TICI +] + + +def ci_setup_data_readers(route, sidx): + lr = LogReader(get_url(route, sidx, "rlog")) + frs = { + 'roadCameraState': FrameReader(get_url(route, sidx, "fcamera")), + 'driverCameraState': DummyFrameReader.zero_dcamera() + } + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs["wideRoadCameraState"] = FrameReader(get_url(route, sidx, "ecamera")) + + return lr, frs + + +class TestRegen: + @parameterized.expand(TESTED_SEGMENTS) + def test_engaged(self, case_name, segment): + route, sidx = segment.rsplit("--", 1) + lr, frs = ci_setup_data_readers(route, sidx) + output_logs = regen_segment(lr, frs, disable_tqdm=True) + + engaged = check_openpilot_enabled(output_logs) + assert engaged, f"openpilot not engaged in {case_name}" diff --git a/selfdrive/test/process_replay/vision_meta.py b/selfdrive/test/process_replay/vision_meta.py new file mode 100644 index 0000000000..12deb58724 --- /dev/null +++ b/selfdrive/test/process_replay/vision_meta.py @@ -0,0 +1,43 @@ +from collections import namedtuple +from msgq.visionipc import VisionStreamType +from openpilot.common.realtime import DT_MDL, DT_DMON +from openpilot.common.transformations.camera import DEVICE_CAMERAS + +VideoStreamMeta = namedtuple("VideoStreamMeta", ["camera_state", "encode_index", "stream", "dt", "frame_sizes"]) +ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} +WIDE_ROAD_CAMERA_FRAME_SIZES = {k: (v.ecam.width, v.ecam.height) for k, v in DEVICE_CAMERAS.items() if v.ecam is not None} +DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} +VIPC_STREAM_METADATA = [ + # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes) + ("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_FRAME_SIZES), + ("wideRoadCameraState", "wideRoadEncodeIdx", VisionStreamType.VISION_STREAM_WIDE_ROAD, DT_MDL, WIDE_ROAD_CAMERA_FRAME_SIZES), + ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_CAMERA_FRAME_SIZES), +] + + +def meta_from_camera_state(state): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[0] == state), None) + return meta + + +def meta_from_encode_index(encode_index): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[1] == encode_index), None) + return meta + + +def meta_from_stream_type(stream_type): + meta = next((VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA if meta[2] == stream_type), None) + return meta + + +def available_streams(lr=None): + if lr is None: + return [VideoStreamMeta(*meta) for meta in VIPC_STREAM_METADATA] + + result = [] + for meta in VIPC_STREAM_METADATA: + has_cam_state = next((True for m in lr if m.which() == meta[0]), False) + if has_cam_state: + result.append(VideoStreamMeta(*meta)) + + return result diff --git a/selfdrive/test/profiling/.gitignore b/selfdrive/test/profiling/.gitignore new file mode 100644 index 0000000000..76acac7f93 --- /dev/null +++ b/selfdrive/test/profiling/.gitignore @@ -0,0 +1,2 @@ +cachegrind.out.* +*.prof diff --git a/selfdrive/test/profiling/__init__.py b/selfdrive/test/profiling/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/test/profiling/lib.py b/selfdrive/test/profiling/lib.py new file mode 100644 index 0000000000..93ba215386 --- /dev/null +++ b/selfdrive/test/profiling/lib.py @@ -0,0 +1,91 @@ +from collections import defaultdict, deque +from cereal.services import SERVICE_LIST +import cereal.messaging as messaging +import capnp + + +class ReplayDone(Exception): + pass + + +class SubSocket: + def __init__(self, msgs, trigger): + self.i = 0 + self.trigger = trigger + self.msgs = [m.as_builder().to_bytes() for m in msgs if m.which() == trigger] + self.max_i = len(self.msgs) - 1 + + def receive(self, non_blocking=False): + if non_blocking: + return None + + if self.i == self.max_i: + raise ReplayDone + + while True: + msg = self.msgs[self.i] + self.i += 1 + return msg + + +class PubSocket: + def send(self, data): + pass + + +class SubMaster(messaging.SubMaster): + def __init__(self, msgs, trigger, services, check_averag_freq=False): + self.frame = 0 + self.data = {} + self.ignore_alive = [] + + self.alive = {s: True for s in services} + self.updated = {s: False for s in services} + self.rcv_time = {s: 0. for s in services} + self.rcv_frame = {s: 0 for s in services} + self.valid = {s: True for s in services} + self.freq_ok = {s: True for s in services} + self.recv_dts = {s: deque([0.0] * messaging.AVG_FREQ_HISTORY, maxlen=messaging.AVG_FREQ_HISTORY) for s in services} + self.logMonoTime = {} + self.sock = {} + self.freq = {} + self.check_average_freq = check_averag_freq + self.non_polled_services = [] + self.ignore_average_freq = [] + + # TODO: specify multiple triggers for service like plannerd that poll on more than one service + cur_msgs = [] + self.msgs = [] + msgs = [m for m in msgs if m.which() in services] + + for msg in msgs: + cur_msgs.append(msg) + if msg.which() == trigger: + self.msgs.append(cur_msgs) + cur_msgs = [] + + self.msgs = list(reversed(self.msgs)) + + for s in services: + self.freq[s] = SERVICE_LIST[s].frequency + try: + data = messaging.new_message(s) + except capnp.lib.capnp.KjException: + # lists + data = messaging.new_message(s, 0) + + self.data[s] = getattr(data, s) + self.logMonoTime[s] = 0 + self.sock[s] = SubSocket(msgs, s) + + def update(self, timeout=None): + if not len(self.msgs): + raise ReplayDone + + cur_msgs = self.msgs.pop() + self.update_msgs(cur_msgs[0].logMonoTime, self.msgs.pop()) + + +class PubMaster(messaging.PubMaster): + def __init__(self): + self.sock = defaultdict(PubSocket) diff --git a/selfdrive/test/profiling/profiler.py b/selfdrive/test/profiling/profiler.py new file mode 100755 index 0000000000..2cd547171a --- /dev/null +++ b/selfdrive/test/profiling/profiler.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +import os +import sys +import cProfile +import pprofile +import pyprof2calltree + +from openpilot.common.params import Params +from openpilot.tools.lib.logreader import LogReader +from openpilot.selfdrive.test.profiling.lib import SubMaster, PubMaster, SubSocket, ReplayDone +from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS +from openpilot.selfdrive.car.toyota.values import CAR as TOYOTA +from openpilot.selfdrive.car.honda.values import CAR as HONDA +from openpilot.selfdrive.car.volkswagen.values import CAR as VW + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" + +CARS = { + 'toyota': ("0982d79ebb0de295|2021-01-03--20-03-36/6", TOYOTA.TOYOTA_RAV4), + 'honda': ("0982d79ebb0de295|2021-01-08--10-13-10/6", HONDA.HONDA_CIVIC), + "vw": ("ef895f46af5fd73f|2021-05-22--14-06-35/6", VW.AUDI_A3_MK3), +} + + +def get_inputs(msgs, process, fingerprint): + for config in CONFIGS: + if config.proc_name == process: + sub_socks = list(config.pubs) + trigger = sub_socks[0] + break + + # some procs block on CarParams + for msg in msgs: + if msg.which() == 'carParams': + m = msg.as_builder() + m.carParams.carFingerprint = fingerprint + Params().put("CarParams", m.carParams.copy().to_bytes()) + break + + sm = SubMaster(msgs, trigger, sub_socks) + pm = PubMaster() + if 'can' in sub_socks: + can_sock = SubSocket(msgs, 'can') + else: + can_sock = None + return sm, pm, can_sock + + +def profile(proc, func, car='toyota'): + segment, fingerprint = CARS[car] + segment = segment.replace('|', '/') + rlog_url = f"{BASE_URL}{segment}/rlog.bz2" + msgs = list(LogReader(rlog_url)) * int(os.getenv("LOOP", "1")) + + os.environ['FINGERPRINT'] = fingerprint + os.environ['SKIP_FW_QUERY'] = "1" + os.environ['REPLAY'] = "1" + + def run(sm, pm, can_sock): + try: + if can_sock is not None: + func(sm, pm, can_sock) + else: + func(sm, pm) + except ReplayDone: + pass + + # Statistical + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with pprofile.StatisticalProfile()(period=0.00001) as pr: + run(sm, pm, can_sock) + pr.dump_stats(f'cachegrind.out.{proc}_statistical') + + # Deterministic + sm, pm, can_sock = get_inputs(msgs, proc, fingerprint) + with cProfile.Profile() as pr: + run(sm, pm, can_sock) + pyprof2calltree.convert(pr.getstats(), f'cachegrind.out.{proc}_deterministic') + + +if __name__ == '__main__': + from openpilot.selfdrive.controls.controlsd import main as controlsd_thread + from openpilot.selfdrive.locationd.paramsd import main as paramsd_thread + from openpilot.selfdrive.controls.plannerd import main as plannerd_thread + + procs = { + 'controlsd': controlsd_thread, + 'paramsd': paramsd_thread, + 'plannerd': plannerd_thread, + } + + proc = sys.argv[1] + if proc not in procs: + print(f"{proc} not available") + sys.exit(0) + else: + profile(proc, procs[proc]) diff --git a/selfdrive/test/scons_build_test.sh b/selfdrive/test/scons_build_test.sh new file mode 100755 index 0000000000..a3b33f797a --- /dev/null +++ b/selfdrive/test/scons_build_test.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +SCRIPT_DIR=$(dirname "$0") +BASEDIR=$(realpath "$SCRIPT_DIR/../../") +cd $BASEDIR + +# tests that our build system's dependencies are configured properly, +# needs a machine with lots of cores +scons --clean +scons --no-cache --random -j$(nproc) \ No newline at end of file diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index 5c85312f1a..84dae25821 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -70,6 +70,7 @@ safe_checkout() { git checkout $GIT_COMMIT git clean -xdff git submodule sync + git submodule foreach --recursive "git reset --hard && git clean -xdff" git submodule update --init --recursive git submodule foreach --recursive "git reset --hard && git clean -xdff" @@ -95,6 +96,7 @@ unsafe_checkout() { git reset --hard $GIT_COMMIT git clean -df git submodule sync + git submodule foreach --recursive "git reset --hard && git clean -df" git submodule update --init --recursive git submodule foreach --recursive "git reset --hard && git clean -df" diff --git a/selfdrive/test/setup_vsound.sh b/selfdrive/test/setup_vsound.sh new file mode 100755 index 0000000000..a6601d0a61 --- /dev/null +++ b/selfdrive/test/setup_vsound.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +{ + #start pulseaudio daemon + sudo pulseaudio -D + + # create a virtual null audio and set it to default device + sudo pactl load-module module-null-sink sink_name=virtual_audio + sudo pactl set-default-sink virtual_audio +} > /dev/null 2>&1 diff --git a/selfdrive/test/setup_xvfb.sh b/selfdrive/test/setup_xvfb.sh new file mode 100755 index 0000000000..692b84d65f --- /dev/null +++ b/selfdrive/test/setup_xvfb.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash + +# Sets up a virtual display for running map renderer and simulator without an X11 display + +DISP_ID=99 +export DISPLAY=:$DISP_ID + +sudo Xvfb $DISPLAY -screen 0 2160x1080x24 2>/dev/null & + +# check for x11 socket for the specified display ID +while [ ! -S /tmp/.X11-unix/X$DISP_ID ] +do + echo "Waiting for Xvfb..." + sleep 1 +done + +touch ~/.Xauthority +export XDG_SESSION_TYPE="x11" +xset -q \ No newline at end of file diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py old mode 100755 new mode 100644 index de8a4420b3..75585e2f14 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 import bz2 import math import json @@ -10,7 +9,6 @@ import shutil import subprocess import time import numpy as np -import unittest from collections import Counter, defaultdict from functools import cached_property from pathlib import Path @@ -27,14 +25,21 @@ from openpilot.selfdrive.test.helpers import set_params_enabled, release_only from openpilot.system.hardware.hw import Paths from openpilot.tools.lib.logreader import LogReader -# Baseline CPU usage by process +""" +CPU usage budget +* each process is entitled to at least 8% +* total CPU usage of openpilot (sum(PROCS.values()) + should not exceed MAX_TOTAL_CPU +""" +MAX_TOTAL_CPU = 250. # total for all 8 cores PROCS = { - "selfdrive.controls.controlsd": 41.0, + # Baseline CPU usage by process + "selfdrive.controls.controlsd": 32.0, + "selfdrive.car.card": 22.0, "./loggerd": 14.0, "./encoderd": 17.0, "./camerad": 14.5, "./locationd": 11.0, - "./mapsd": (0.5, 10.0), "selfdrive.controls.plannerd": 11.0, "./ui": 18.0, "selfdrive.locationd.paramsd": 9.0, @@ -42,20 +47,19 @@ PROCS = { "selfdrive.controls.radard": 7.0, "selfdrive.modeld.modeld": 13.0, "selfdrive.modeld.dmonitoringmodeld": 8.0, - "selfdrive.modeld.navmodeld": 1.0, - "selfdrive.thermald.thermald": 3.87, + "system.hardware.hardwared": 3.87, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, "selfdrive.ui.soundd": 3.5, "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 1.54, "system.logmessaged": 0.2, - "selfdrive.tombstoned": 0, + "system.tombstoned": 0, "./logcatd": 0, "system.micd": 6.0, "system.timed": 0, - "selfdrive.boardd.pandad": 0, - "selfdrive.statsd": 0.4, + "selfdrive.pandad.pandad": 0, + "system.statsd": 0.4, "selfdrive.navd.navd": 0.4, "system.loggerd.uploader": (0.5, 15.0), "system.loggerd.deleter": 0.1, @@ -63,12 +67,12 @@ PROCS = { PROCS.update({ "tici": { - "./boardd": 4.0, + "./pandad": 4.0, "./ubloxd": 0.02, - "system.sensord.pigeond": 6.0, + "system.ubloxd.pigeond": 6.0, }, "tizi": { - "./boardd": 19.0, + "./pandad": 19.0, "system.qcomgpsd.qcomgpsd": 1.0, } }.get(HARDWARE.get_device_type(), {})) @@ -87,8 +91,6 @@ TIMINGS = { "driverCameraState": [2.5, 0.35], "modelV2": [2.5, 0.35], "driverStateV2": [2.5, 0.40], - "navModel": [2.5, 0.35], - "mapRenderState": [2.5, 0.35], "liveLocationKalman": [2.5, 0.35], "wideRoadCameraState": [1.5, 0.35], } @@ -99,10 +101,10 @@ def cputime_total(ct): @pytest.mark.tici -class TestOnroad(unittest.TestCase): +class TestOnroad: @classmethod - def setUpClass(cls): + def setup_class(cls): if "DEBUG" in os.environ: segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog")), Path(Paths.log_root()).iterdir()) segs = sorted(segs, key=lambda x: x.stat().st_mtime) @@ -122,7 +124,7 @@ class TestOnroad(unittest.TestCase): # start manager and run openpilot for a minute proc = None try: - manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") + manager_path = os.path.join(BASEDIR, "system/manager/manager.py") proc = subprocess.Popen(["python", manager_path]) sm = messaging.SubMaster(['carState']) @@ -178,7 +180,7 @@ class TestOnroad(unittest.TestCase): msgs[m.which()].append(m) return msgs - def test_service_frequencies(self): + def test_service_frequencies(self, subtests): for s, msgs in self.service_msgs.items(): if s in ('initData', 'sentinel'): continue @@ -187,18 +189,18 @@ class TestOnroad(unittest.TestCase): if s in ('ubloxGnss', 'ubloxRaw', 'gnssMeasurements', 'gpsLocation', 'gpsLocationExternal', 'qcomGnss'): continue - with self.subTest(service=s): + with subtests.test(service=s): assert len(msgs) >= math.floor(SERVICE_LIST[s].frequency*55) def test_cloudlog_size(self): msgs = [m for m in self.lr if m.which() == 'logMessage'] total_size = sum(len(m.as_builder().to_bytes()) for m in msgs) - self.assertLess(total_size, 3.5e5) + assert total_size < 3.5e5 cnt = Counter(json.loads(m.logMessage)['filename'] for m in msgs) big_logs = [f for f, n in cnt.most_common(3) if n / sum(cnt.values()) > 30.] - self.assertEqual(len(big_logs), 0, f"Log spam: {big_logs}") + assert len(big_logs) == 0, f"Log spam: {big_logs}" def test_log_sizes(self): for f, sz in self.log_sizes.items(): @@ -227,15 +229,15 @@ class TestOnroad(unittest.TestCase): result += "------------------------------------------------\n" print(result) - self.assertLess(max(ts), 250.) - self.assertLess(np.mean(ts), 10.) + assert max(ts) < 250. + assert np.mean(ts) < 10. #self.assertLess(np.std(ts), 5.) # some slow frames are expected since camerad/modeld can preempt ui veryslow = [x for x in ts if x > 40.] assert len(veryslow) < 5, f"Too many slow frame draw times: {veryslow}" - def test_cpu_usage(self): + def test_cpu_usage(self, subtests): result = "\n" result += "------------------------------------------------\n" result += "------------------ CPU Usage -------------------\n" @@ -278,17 +280,25 @@ class TestOnroad(unittest.TestCase): result += f"{proc_name.ljust(35)} {cpu_usage:5.2f}% ({exp}%) {err}\n" if len(err) > 0: cpu_ok = False + result += "------------------------------------------------\n" # Ensure there's no missing procs all_procs = {p.name for p in self.service_msgs['managerState'][0].managerState.processes if p.shouldBeRunning} for p in all_procs: - with self.subTest(proc=p): + with subtests.test(proc=p): assert any(p in pp for pp in PROCS.keys()), f"Expected CPU usage missing for {p}" - result += "------------------------------------------------\n" + # total CPU check + procs_tot = sum([(max(x) if isinstance(x, tuple) else x) for x in PROCS.values()]) + with subtests.test(name="total CPU"): + assert procs_tot < MAX_TOTAL_CPU, "Total CPU budget exceeded" + result += "------------------------------------------------\n" + result += f"Total allocated CPU usage is {procs_tot}%, budget is {MAX_TOTAL_CPU}%, {MAX_TOTAL_CPU-procs_tot:.1f}% left\n" + result += "------------------------------------------------\n" + print(result) - self.assertTrue(cpu_ok) + assert cpu_ok def test_memory_usage(self): mems = [m.deviceState.memoryUsagePercent for m in self.service_msgs['deviceState']] @@ -296,26 +306,26 @@ class TestOnroad(unittest.TestCase): # check for big leaks. note that memory usage is # expected to go up while the MSGQ buffers fill up - self.assertLessEqual(max(mems) - min(mems), 3.0) + assert max(mems) - min(mems) <= 3.0 def test_gpu_usage(self): - self.assertEqual(self.gpu_procs, {"weston", "ui", "camerad", "selfdrive.modeld.modeld"}) + assert self.gpu_procs == {"weston", "ui", "camerad", "selfdrive.modeld.modeld"} def test_camera_processing_time(self): result = "\n" result += "------------------------------------------------\n" - result += "-------------- Debayer Timing ------------------\n" + result += "-------------- ImgProc Timing ------------------\n" result += "------------------------------------------------\n" ts = [getattr(m, m.which()).processingTime for m in self.lr if 'CameraState' in m.which()] - self.assertLess(min(ts), 0.025, f"high execution time: {min(ts)}") + assert min(ts) < 0.025, f"high execution time: {min(ts)}" result += f"execution time: min {min(ts):.5f}s\n" result += f"execution time: max {max(ts):.5f}s\n" result += f"execution time: mean {np.mean(ts):.5f}s\n" result += "------------------------------------------------\n" print(result) - @unittest.skip("TODO: enable once timings are fixed") + @pytest.mark.skip("TODO: enable once timings are fixed") def test_camera_frame_timings(self): result = "\n" result += "------------------------------------------------\n" @@ -325,7 +335,7 @@ class TestOnroad(unittest.TestCase): ts = [getattr(m, m.which()).timestampSof for m in self.lr if name in m.which()] d_ms = np.diff(ts) / 1e6 d50 = np.abs(d_ms-50) - self.assertLess(max(d50), 1.0, f"high sof delta vs 50ms: {max(d50)}") + assert max(d50) < 1.0, f"high sof delta vs 50ms: {max(d50)}" result += f"{name} sof delta vs 50ms: min {min(d50):.5f}s\n" result += f"{name} sof delta vs 50ms: max {max(d50):.5f}s\n" result += f"{name} sof delta vs 50ms: mean {d50.mean():.5f}s\n" @@ -341,8 +351,8 @@ class TestOnroad(unittest.TestCase): cfgs = [("longitudinalPlan", 0.05, 0.05),] for (s, instant_max, avg_max) in cfgs: ts = [getattr(m, s).solverExecutionTime for m in self.service_msgs[s]] - self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") - self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + assert max(ts) < instant_max, f"high '{s}' execution time: {max(ts)}" + assert np.mean(ts) < avg_max, f"high avg '{s}' execution time: {np.mean(ts)}" result += f"'{s}' execution time: min {min(ts):.5f}s\n" result += f"'{s}' execution time: max {max(ts):.5f}s\n" result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" @@ -361,8 +371,8 @@ class TestOnroad(unittest.TestCase): ] for (s, instant_max, avg_max) in cfgs: ts = [getattr(m, s).modelExecutionTime for m in self.service_msgs[s]] - self.assertLess(max(ts), instant_max, f"high '{s}' execution time: {max(ts)}") - self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + assert max(ts) < instant_max, f"high '{s}' execution time: {max(ts)}" + assert np.mean(ts) < avg_max, f"high avg '{s}' execution time: {np.mean(ts)}" result += f"'{s}' execution time: min {min(ts):.5f}s\n" result += f"'{s}' execution time: max {max(ts):.5f}s\n" result += f"'{s}' execution time: mean {np.mean(ts):.5f}s\n" @@ -398,7 +408,7 @@ class TestOnroad(unittest.TestCase): result += f"{''.ljust(40)} {np.max(np.absolute([np.max(ts)/dt, np.min(ts)/dt]))} {np.std(ts)/dt}\n" result += "="*67 print(result) - self.assertTrue(passed) + assert passed @release_only def test_startup(self): @@ -409,7 +419,7 @@ class TestOnroad(unittest.TestCase): startup_alert = msg.controlsState.alertText1 break expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1 - self.assertEqual(startup_alert, expected, "wrong startup alert") + assert startup_alert == expected, "wrong startup alert" def test_engagable(self): no_entries = Counter() @@ -421,7 +431,3 @@ class TestOnroad(unittest.TestCase): eng = [m.controlsState.engageable for m in self.service_msgs['controlsState']] assert all(eng), \ f"Not engageable for whole segment:\n- controlsState.engageable: {Counter(eng)}\n- No entry events: {no_entries}" - - -if __name__ == "__main__": - pytest.main() diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py old mode 100755 new mode 100644 index a3f803e221..e08d0e676c --- a/selfdrive/test/test_time_to_onroad.py +++ b/selfdrive/test/test_time_to_onroad.py @@ -1,24 +1,26 @@ -#!/usr/bin/env python3 import os import pytest import time import subprocess +from cereal import car import cereal.messaging as messaging from openpilot.common.basedir import BASEDIR from openpilot.common.timeout import Timeout from openpilot.selfdrive.test.helpers import set_params_enabled +EventName = car.CarEvent.EventName + @pytest.mark.tici def test_time_to_onroad(): # launch set_params_enabled() - manager_path = os.path.join(BASEDIR, "selfdrive/manager/manager.py") + manager_path = os.path.join(BASEDIR, "system/manager/manager.py") proc = subprocess.Popen(["python", manager_path]) start_time = time.monotonic() - sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents', 'sendcan']) + sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents']) try: # wait for onroad. timeout assumes panda is up to date with Timeout(10, "timed out waiting to go onroad"): @@ -28,15 +30,14 @@ def test_time_to_onroad(): # wait for engageability try: with Timeout(10, "timed out waiting for engageable"): - sendcan_frame = None + initialized = False while True: sm.update(100) - # sendcan is only sent once we're initialized - if sm.seen['controlsState'] and sendcan_frame is None: - sendcan_frame = sm.frame + if sm.seen['onroadEvents'] and not any(EventName.controlsInitializing == e.name for e in sm['onroadEvents']): + initialized = True - if sendcan_frame is not None and sm.recv_frame['sendcan'] > sendcan_frame: + if initialized: sm.update(100) assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}" break diff --git a/selfdrive/test/test_updated.py b/selfdrive/test/test_updated.py new file mode 100644 index 0000000000..f8eae94823 --- /dev/null +++ b/selfdrive/test/test_updated.py @@ -0,0 +1,296 @@ +import datetime +import os +import pytest +import time +import tempfile +import shutil +import signal +import subprocess +import random + +from openpilot.common.basedir import BASEDIR +from openpilot.common.params import Params + + +@pytest.mark.tici +class TestUpdated: + + def setup_method(self): + self.updated_proc = None + + self.tmp_dir = tempfile.TemporaryDirectory() + org_dir = os.path.join(self.tmp_dir.name, "commaai") + + self.basedir = os.path.join(org_dir, "openpilot") + self.git_remote_dir = os.path.join(org_dir, "openpilot_remote") + self.staging_dir = os.path.join(org_dir, "safe_staging") + for d in [org_dir, self.basedir, self.git_remote_dir, self.staging_dir]: + os.mkdir(d) + + self.neos_version = os.path.join(org_dir, "neos_version") + self.neosupdate_dir = os.path.join(org_dir, "neosupdate") + with open(self.neos_version, "w") as f: + v = subprocess.check_output(r"bash -c 'source launch_env.sh && echo $REQUIRED_NEOS_VERSION'", + cwd=BASEDIR, shell=True, encoding='utf8').strip() + f.write(v) + + self.upper_dir = os.path.join(self.staging_dir, "upper") + self.merged_dir = os.path.join(self.staging_dir, "merged") + self.finalized_dir = os.path.join(self.staging_dir, "finalized") + + # setup local submodule remotes + submodules = subprocess.check_output("git submodule --quiet foreach 'echo $name'", + shell=True, cwd=BASEDIR, encoding='utf8').split() + for s in submodules: + sub_path = os.path.join(org_dir, s.split("_repo")[0]) + self._run(f"git clone {s} {sub_path}.git", cwd=BASEDIR) + + # setup two git repos, a remote and one we'll run updated in + self._run([ + f"git clone {BASEDIR} {self.git_remote_dir}", + f"git clone {self.git_remote_dir} {self.basedir}", + f"cd {self.basedir} && git submodule init && git submodule update", + f"cd {self.basedir} && scons -j{os.cpu_count()} cereal/ common/" + ]) + + self.params = Params(os.path.join(self.basedir, "persist/params")) + self.params.clear_all() + os.sync() + + def teardown_method(self): + try: + if self.updated_proc is not None: + self.updated_proc.terminate() + self.updated_proc.wait(30) + except Exception as e: + print(e) + self.tmp_dir.cleanup() + + + # *** test helpers *** + + + def _run(self, cmd, cwd=None): + if not isinstance(cmd, list): + cmd = (cmd,) + + for c in cmd: + subprocess.check_output(c, cwd=cwd, shell=True) + + def _get_updated_proc(self): + os.environ["PYTHONPATH"] = self.basedir + os.environ["GIT_AUTHOR_NAME"] = "testy tester" + os.environ["GIT_COMMITTER_NAME"] = "testy tester" + os.environ["GIT_AUTHOR_EMAIL"] = "testy@tester.test" + os.environ["GIT_COMMITTER_EMAIL"] = "testy@tester.test" + os.environ["UPDATER_TEST_IP"] = "localhost" + os.environ["UPDATER_LOCK_FILE"] = os.path.join(self.tmp_dir.name, "updater.lock") + os.environ["UPDATER_STAGING_ROOT"] = self.staging_dir + os.environ["UPDATER_NEOS_VERSION"] = self.neos_version + os.environ["UPDATER_NEOSUPDATE_DIR"] = self.neosupdate_dir + updated_path = os.path.join(self.basedir, "system/updated.py") + return subprocess.Popen(updated_path, env=os.environ) + + def _start_updater(self, offroad=True, nosleep=False): + self.params.put_bool("IsOffroad", offroad) + self.updated_proc = self._get_updated_proc() + if not nosleep: + time.sleep(1) + + def _update_now(self): + self.updated_proc.send_signal(signal.SIGHUP) + + # TODO: this should be implemented in params + def _read_param(self, key, timeout=1): + ret = None + start_time = time.monotonic() + while ret is None: + ret = self.params.get(key, encoding='utf8') + if time.monotonic() - start_time > timeout: + break + time.sleep(0.01) + return ret + + def _wait_for_update(self, timeout=30, clear_param=False): + if clear_param: + self.params.remove("LastUpdateTime") + + self._update_now() + t = self._read_param("LastUpdateTime", timeout=timeout) + if t is None: + raise Exception("timed out waiting for update to complete") + + def _make_commit(self): + all_dirs, all_files = [], [] + for root, dirs, files in os.walk(self.git_remote_dir): + if ".git" in root: + continue + for d in dirs: + all_dirs.append(os.path.join(root, d)) + for f in files: + all_files.append(os.path.join(root, f)) + + # make a new dir and some new files + new_dir = os.path.join(self.git_remote_dir, "this_is_a_new_dir") + os.mkdir(new_dir) + for _ in range(random.randrange(5, 30)): + for d in (new_dir, random.choice(all_dirs)): + with tempfile.NamedTemporaryFile(dir=d, delete=False) as f: + f.write(os.urandom(random.randrange(1, 1000000))) + + # modify some files + for f in random.sample(all_files, random.randrange(5, 50)): + with open(f, "w+") as ff: + txt = ff.readlines() + ff.seek(0) + for line in txt: + ff.write(line[::-1]) + + # remove some files + for f in random.sample(all_files, random.randrange(5, 50)): + os.remove(f) + + # remove some dirs + for d in random.sample(all_dirs, random.randrange(1, 10)): + shutil.rmtree(d) + + # commit the changes + self._run([ + "git add -A", + "git commit -m 'an update'", + ], cwd=self.git_remote_dir) + + def _check_update_state(self, update_available): + # make sure LastUpdateTime is recent + t = self._read_param("LastUpdateTime") + last_update_time = datetime.datetime.fromisoformat(t) + td = datetime.datetime.utcnow() - last_update_time + assert td.total_seconds() < 10 + self.params.remove("LastUpdateTime") + + # wait a bit for the rest of the params to be written + time.sleep(0.1) + + # check params + update = self._read_param("UpdateAvailable") + assert update == "1" == update_available, f"UpdateAvailable: {repr(update)}" + assert self._read_param("UpdateFailedCount") == "0" + + # TODO: check that the finalized update actually matches remote + # check the .overlay_init and .overlay_consistent flags + assert os.path.isfile(os.path.join(self.basedir, ".overlay_init")) + assert os.path.isfile(os.path.join(self.finalized_dir, ".overlay_consistent")) == update_available + + + # *** test cases *** + + + # Run updated for 100 cycles with no update + def test_no_update(self): + self._start_updater() + for _ in range(100): + self._wait_for_update(clear_param=True) + self._check_update_state(False) + + # Let the updater run with no update for a cycle, then write an update + def test_update(self): + self._start_updater() + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + self._check_update_state(False) + + # write an update to our remote + self._make_commit() + + # run for a cycle to get the update + self._wait_for_update(timeout=60, clear_param=True) + self._check_update_state(True) + + # run another cycle with no update + self._wait_for_update(clear_param=True) + self._check_update_state(True) + + # Let the updater run for 10 cycles, and write an update every cycle + @pytest.mark.skip("need to make this faster") + def test_update_loop(self): + self._start_updater() + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + for _ in range(10): + time.sleep(0.5) + self._make_commit() + self._wait_for_update(timeout=90, clear_param=True) + self._check_update_state(True) + + # Test overlay re-creation after tracking a new file in basedir's git + def test_overlay_reinit(self): + self._start_updater() + + overlay_init_fn = os.path.join(self.basedir, ".overlay_init") + + # run for a cycle with no update + self._wait_for_update(clear_param=True) + self.params.remove("LastUpdateTime") + first_mtime = os.path.getmtime(overlay_init_fn) + + # touch a file in the basedir + self._run("touch new_file && git add new_file", cwd=self.basedir) + + # run another cycle, should have a new mtime + self._wait_for_update(clear_param=True) + second_mtime = os.path.getmtime(overlay_init_fn) + assert first_mtime != second_mtime + + # run another cycle, mtime should be same as last cycle + self._wait_for_update(clear_param=True) + new_mtime = os.path.getmtime(overlay_init_fn) + assert second_mtime == new_mtime + + # Make sure updated exits if another instance is running + def test_multiple_instances(self): + # start updated and let it run for a cycle + self._start_updater() + time.sleep(1) + self._wait_for_update(clear_param=True) + + # start another instance + second_updated = self._get_updated_proc() + ret_code = second_updated.wait(timeout=5) + assert ret_code is not None + + + # *** test cases with NEOS updates *** + + + # Run updated with no update, make sure it clears the old NEOS update + def test_clear_neos_cache(self): + # make the dir and some junk files + os.mkdir(self.neosupdate_dir) + for _ in range(15): + with tempfile.NamedTemporaryFile(dir=self.neosupdate_dir, delete=False) as f: + f.write(os.urandom(random.randrange(1, 1000000))) + + self._start_updater() + self._wait_for_update(clear_param=True) + self._check_update_state(False) + assert not os.path.isdir(self.neosupdate_dir) + + # Let the updater run with no update for a cycle, then write an update + @pytest.mark.skip("TODO: only runs on device") + def test_update_with_neos_update(self): + # bump the NEOS version and commit it + self._run([ + "echo 'export REQUIRED_NEOS_VERSION=3' >> launch_env.sh", + "git -c user.name='testy' -c user.email='testy@tester.test' \ + commit -am 'a neos update'", + ], cwd=self.git_remote_dir) + + # run for a cycle to get the update + self._start_updater() + self._wait_for_update(timeout=60, clear_param=True) + self._check_update_state(True) + + # TODO: more comprehensive check + assert os.path.isdir(self.neosupdate_dir) diff --git a/selfdrive/test/update_ci_routes.py b/selfdrive/test/update_ci_routes.py new file mode 100755 index 0000000000..a9f4494ffd --- /dev/null +++ b/selfdrive/test/update_ci_routes.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +import os +import re +import subprocess +import sys +from collections.abc import Iterable + +from tqdm import tqdm + +from openpilot.selfdrive.car.tests.routes import routes as test_car_models_routes +from openpilot.selfdrive.test.process_replay.test_processes import source_segments as replay_segments +from openpilot.tools.lib.azure_container import AzureContainer +from openpilot.tools.lib.openpilotcontainers import DataCIContainer, DataProdContainer, OpenpilotCIContainer + +SOURCES: list[AzureContainer] = [ + DataProdContainer, + DataCIContainer +] + +DEST = OpenpilotCIContainer + +def upload_route(path: str, exclude_patterns: Iterable[str] = None) -> None: + if exclude_patterns is None: + exclude_patterns = [r'dcamera\.hevc'] + + r, n = path.rsplit("--", 1) + r = '/'.join(r.split('/')[-2:]) # strip out anything extra in the path + destpath = f"{r}/{n}" + for file in os.listdir(path): + if any(re.search(pattern, file) for pattern in exclude_patterns): + continue + DEST.upload_file(os.path.join(path, file), f"{destpath}/{file}") + + +def sync_to_ci_public(route: str) -> bool: + dest_container, dest_key = DEST.get_client_and_key() + key_prefix = route.replace('|', '/') + dongle_id = key_prefix.split('/')[0] + + if next(dest_container.list_blob_names(name_starts_with=key_prefix), None) is not None: + return True + + print(f"Uploading {route}") + for source_container in SOURCES: + # assumes az login has been run + print(f"Trying {source_container.ACCOUNT}/{source_container.CONTAINER}") + _, source_key = source_container.get_client_and_key() + cmd = [ + "azcopy", + "copy", + f"{source_container.BASE_URL}{key_prefix}?{source_key}", + f"{DEST.BASE_URL}{dongle_id}?{dest_key}", + "--recursive=true", + "--overwrite=false", + "--exclude-pattern=*/dcamera.hevc", + ] + + try: + result = subprocess.call(cmd, stdout=subprocess.DEVNULL) + if result == 0: + print("Success") + return True + except subprocess.CalledProcessError: + print("Failed") + + return False + + +if __name__ == "__main__": + failed_routes = [] + + to_sync = sys.argv[1:] + + if not len(to_sync): + # sync routes from the car tests routes and process replay + to_sync.extend([rt.route for rt in test_car_models_routes]) + to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments]) + + for r in tqdm(to_sync): + if not sync_to_ci_public(r): + failed_routes.append(r) + + if len(failed_routes): + print("failed routes:", failed_routes) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ea5734fe6e..e181cb9abd 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,10 +1,9 @@ import os import json -Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', - 'cereal', 'transformations') +Import('qt_env', 'arch', 'common', 'messaging', 'visionipc', 'transformations') -base_libs = [common, messaging, cereal, visionipc, transformations, 'zmq', - 'capnp', 'kj', 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] +base_libs = [common, messaging, visionipc, transformations, + 'm', 'OpenCL', 'ssl', 'crypto', 'pthread'] + qt_env["LIBS"] if arch == 'larch64': base_libs.append('EGL') @@ -36,10 +35,12 @@ widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) Export('widgets') qt_libs = [widgets, qt_util] + base_libs -qt_src = ["main.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/body.cc", +qt_src = ["main.cc", "qt/sidebar.cc", "qt/body.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", "qt/offroad/software_settings.cc", "qt/offroad/onboarding.cc", - "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc"] + "qt/offroad/driverview.cc", "qt/offroad/experimental_mode.cc", + "qt/onroad/onroad_home.cc", "qt/onroad/annotated_camera.cc", + "qt/onroad/buttons.cc", "qt/onroad/alerts.cc"] # build translation files with open(File("translations/languages.json").abspath) as f: @@ -125,4 +126,4 @@ if GetOption('extras') and arch != "Darwin": # build watch3 if arch in ['x86_64', 'aarch64', 'Darwin'] or GetOption('extras'): - qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'zmq', 'visionipc', 'messaging']) + qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'msgq', 'visionipc']) diff --git a/selfdrive/ui/__init__.py b/selfdrive/ui/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/ui/installer/continue_openpilot.sh b/selfdrive/ui/installer/continue_openpilot.sh new file mode 100755 index 0000000000..3da67313eb --- /dev/null +++ b/selfdrive/ui/installer/continue_openpilot.sh @@ -0,0 +1,4 @@ +#!/usr/bin/bash + +cd /data/openpilot +exec ./launch_openpilot.sh diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc index 0e321d4e10..6889b40e51 100644 --- a/selfdrive/ui/qt/api.cc +++ b/selfdrive/ui/qt/api.cc @@ -1,6 +1,5 @@ #include "selfdrive/ui/qt/api.h" -#include #include #include @@ -8,38 +7,41 @@ #include #include #include -#include #include #include +#include #include -#include "common/params.h" #include "common/util.h" #include "system/hardware/hw.h" #include "selfdrive/ui/qt/util.h" namespace CommaApi { -QByteArray rsa_sign(const QByteArray &data) { - static std::string key = util::read_file(Path::rsa_file()); - if (key.empty()) { - qDebug() << "No RSA private key found, please run manager.py or registration.py"; - return {}; +RSA *get_rsa_private_key() { + static std::unique_ptr rsa_private(nullptr, RSA_free); + if (!rsa_private) { + FILE *fp = fopen(Path::rsa_file().c_str(), "rb"); + if (!fp) { + qDebug() << "No RSA private key found, please run manager.py or registration.py"; + return nullptr; + } + rsa_private.reset(PEM_read_RSAPrivateKey(fp, NULL, NULL, NULL)); + fclose(fp); } + return rsa_private.get(); +} + +QByteArray rsa_sign(const QByteArray &data) { + RSA *rsa_private = get_rsa_private_key(); + if (!rsa_private) return {}; - BIO* mem = BIO_new_mem_buf(key.data(), key.size()); - assert(mem); - RSA* rsa_private = PEM_read_bio_RSAPrivateKey(mem, NULL, NULL, NULL); - assert(rsa_private); - auto sig = QByteArray(); - sig.resize(RSA_size(rsa_private)); + QByteArray sig(RSA_size(rsa_private), Qt::Uninitialized); unsigned int sig_len; int ret = RSA_sign(NID_sha256, (unsigned char*)data.data(), data.size(), (unsigned char*)sig.data(), &sig_len, rsa_private); assert(ret == 1); - assert(sig_len == sig.size()); - BIO_free(mem); - RSA_free(rsa_private); + assert(sig.size() == sig_len); return sig; } @@ -57,9 +59,7 @@ QString create_jwt(const QJsonObject &payloads, int expiry) { QJsonDocument(payload).toJson(QJsonDocument::Compact).toBase64(b64_opts); auto hash = QCryptographicHash::hash(jwt.toUtf8(), QCryptographicHash::Sha256); - auto sig = rsa_sign(hash); - jwt += '.' + sig.toBase64(b64_opts); - return jwt; + return jwt + "." + rsa_sign(hash).toBase64(b64_opts); } } // namespace CommaApi diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index c6032852a1..f60b80b21a 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -10,7 +10,7 @@ #include "common/params.h" #include "selfdrive/ui/qt/offroad/driverview.h" #include "selfdrive/ui/qt/body.h" -#include "selfdrive/ui/qt/onroad.h" +#include "selfdrive/ui/qt/onroad/onroad_home.h" #include "selfdrive/ui/qt/sidebar.h" #include "selfdrive/ui/qt/widgets/controls.h" #include "selfdrive/ui/qt/widgets/offroad_alerts.h" diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index a5644bae4f..490eb118ca 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -5,6 +5,7 @@ #include +#include "common/swaglog.h" #include "selfdrive/ui/qt/maps/map_helpers.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/ui.h" @@ -74,7 +75,7 @@ void MapWindow::initLayers() { QVariantMap transition; transition["duration"] = 400; // ms - m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(uiState()->scene.navigate_on_openpilot)); + m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee")); m_map->setPaintProperty("navLayer", "line-color-transition", transition); m_map->setPaintProperty("navLayer", "line-width", 7.5); m_map->setLayoutProperty("navLayer", "line-cap", "round"); @@ -118,32 +119,28 @@ void MapWindow::updateState(const UIState &s) { const SubMaster &sm = *(s.sm); update(); - if (sm.updated("modelV2")) { - // set path color on change, and show map on rising edge of navigate on openpilot - bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() && - sm["controlsState"].getControlsState().getEnabled(); - if (nav_enabled != uiState()->scene.navigate_on_openpilot) { - if (loaded_once) { - m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); - } - if (nav_enabled) { - emit requestVisible(true); - } - } - uiState()->scene.navigate_on_openpilot = nav_enabled; + // on rising edge of a valid system time, reinitialize the map to set a new token + if (sm.valid("clocks") && !prev_time_valid) { + LOGW("Time is now valid, reinitializing map"); + m_settings.setApiKey(get_mapbox_token()); + initializeGL(); } + prev_time_valid = sm.valid("clocks"); if (sm.updated("liveLocationKalman")) { auto locationd_location = sm["liveLocationKalman"].getLiveLocationKalman(); auto locationd_pos = locationd_location.getPositionGeodetic(); auto locationd_orientation = locationd_location.getCalibratedOrientationNED(); auto locationd_velocity = locationd_location.getVelocityCalibrated(); + auto locationd_ecef = locationd_location.getPositionECEF(); - // Check std norm - auto pos_ecef_std = locationd_location.getPositionECEF().getStd(); - bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100; - - locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && pos_accurate_enough); + locationd_valid = (locationd_pos.getValid() && locationd_orientation.getValid() && locationd_velocity.getValid() && locationd_ecef.getValid()); + if (locationd_valid) { + // Check std norm + auto pos_ecef_std = locationd_ecef.getStd(); + bool pos_accurate_enough = sqrt(pow(pos_ecef_std[0], 2) + pow(pos_ecef_std[1], 2) + pow(pos_ecef_std[2], 2)) < 100; + locationd_valid = pos_accurate_enough; + } if (locationd_valid) { last_position = QMapLibre::Coordinate(locationd_pos.getValue()[0], locationd_pos.getValue()[1]); @@ -274,6 +271,10 @@ void MapWindow::initializeGL() { loaded_once = true; } }); + + QObject::connect(m_map.data(), &QMapLibre::Map::mapLoadingFailed, [=](QMapLibre::Map::MapLoadingFailure err_code, const QString &reason) { + LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str()); + }); } void MapWindow::paintGL() { @@ -365,7 +366,6 @@ void MapWindow::pinchTriggered(QPinchGesture *gesture) { void MapWindow::offroadTransition(bool offroad) { if (offroad) { clearRoute(); - uiState()->scene.navigate_on_openpilot = false; routing_problem = false; } else { auto dest = coordinate_from_param("NavDestination"); diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index 81ba50037a..31a44f27b1 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -51,6 +51,7 @@ private: void setError(const QString &err_str); bool loaded_once = false; + bool prev_time_valid = true; // Panning QPointF m_lastPos; @@ -69,11 +70,6 @@ private: MapInstructions* map_instructions; MapETA* map_eta; - // Blue with normal nav, green when nav is input into the model - QColor getNavPathColor(bool nav_enabled) { - return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee"); - } - void clearRoute(); void updateDestinationMarker(); uint64_t route_rcv_frame = 0; diff --git a/selfdrive/ui/qt/maps/map_eta.cc b/selfdrive/ui/qt/maps/map_eta.cc index 161844c618..0eb77e36ce 100644 --- a/selfdrive/ui/qt/maps/map_eta.cc +++ b/selfdrive/ui/qt/maps/map_eta.cc @@ -38,10 +38,13 @@ void MapETA::updateETA(float s, float s_typical, float d) { auto remaining = s < 3600 ? std::pair{QString::number(int(s / 60)), tr("min")} : std::pair{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")}; QString color = "#25DA6E"; - if (s / s_typical > 1.5) - color = "#DA3025"; - else if (s / s_typical > 1.2) - color = "#DAA725"; + if (std::abs(s_typical) > 1e-5) { + if (s / s_typical > 1.5) { + color = "#DA3025"; + } else if (s / s_typical > 1.2) { + color = "#DAA725"; + } + } // Distance auto distance = map_format_distance(d, uiState()->scene.is_metric); diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc index 3907ff7b05..16923f7a43 100644 --- a/selfdrive/ui/qt/maps/map_helpers.cc +++ b/selfdrive/ui/qt/maps/map_helpers.cc @@ -137,17 +137,17 @@ std::optional coordinate_from_param(const std::string &pa // return {distance, unit} std::pair map_format_distance(float d, bool is_metric) { - auto round_distance = [](float d) -> float { - return (d > 10) ? std::nearbyint(d) : std::nearbyint(d * 10) / 10.0; + auto round_distance = [](float d) -> QString { + return (d > 10) ? QString::number(std::nearbyint(d)) : QString::number(std::nearbyint(d * 10) / 10.0, 'f', 1); }; d = std::max(d, 0.0f); if (is_metric) { - return (d > 500) ? std::pair{QString::number(round_distance(d / 1000)), QObject::tr("km")} + return (d > 500) ? std::pair{round_distance(d / 1000), QObject::tr("km")} : std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("m")}; } else { float feet = d * METER_TO_FOOT; - return (feet > 500) ? std::pair{QString::number(round_distance(d * METER_TO_MILE)), QObject::tr("mi")} + return (feet > 500) ? std::pair{round_distance(d * METER_TO_MILE), QObject::tr("mi")} : std::pair{QString::number(50 * std::nearbyint(d / 50)), QObject::tr("ft")}; } } diff --git a/selfdrive/ui/qt/network/networking.cc b/selfdrive/ui/qt/network/networking.cc index 5354a01fa0..d7cdddff44 100644 --- a/selfdrive/ui/qt/network/networking.cc +++ b/selfdrive/ui/qt/network/networking.cc @@ -82,11 +82,11 @@ void Networking::connectToNetwork(const Network n) { if (wifi->isKnownConnection(n.ssid)) { wifi->activateWifiConnection(n.ssid); } else if (n.security_type == SecurityType::OPEN) { - wifi->connect(n); + wifi->connect(n, false); } else if (n.security_type == SecurityType::WPA) { QString pass = InputDialog::getText(tr("Enter password"), this, tr("for \"%1\"").arg(QString::fromUtf8(n.ssid)), true, 8); if (!pass.isEmpty()) { - wifi->connect(n, pass); + wifi->connect(n, false, pass); } } } @@ -96,7 +96,7 @@ void Networking::wrongPassword(const QString &ssid) { const Network &n = wifi->seenNetworks.value(ssid); QString pass = InputDialog::getText(tr("Wrong password"), this, tr("for \"%1\"").arg(QString::fromUtf8(n.ssid)), true, 8); if (!pass.isEmpty()) { - wifi->connect(n, pass); + wifi->connect(n, false, pass); } } } @@ -192,9 +192,9 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid hidden_network.ssid = ssid.toUtf8(); if (!pass.isEmpty()) { hidden_network.security_type = SecurityType::WPA; - wifi->connect(hidden_network, pass); + wifi->connect(hidden_network, true, pass); } else { - wifi->connect(hidden_network); + wifi->connect(hidden_network, true); } emit requestWifiScreen(); } diff --git a/selfdrive/ui/qt/network/networkmanager.h b/selfdrive/ui/qt/network/networkmanager.h index 2896b0fff7..8bdeaf3bbd 100644 --- a/selfdrive/ui/qt/network/networkmanager.h +++ b/selfdrive/ui/qt/network/networkmanager.h @@ -32,6 +32,7 @@ const QString NM_DBUS_INTERFACE_IP4_CONFIG = "org.freedesktop.NetworkMa const QString NM_DBUS_SERVICE = "org.freedesktop.NetworkManager"; +const int NM_DEVICE_STATE_UNKNOWN = 0; const int NM_DEVICE_STATE_ACTIVATED = 100; const int NM_DEVICE_STATE_NEED_AUTH = 60; const int NM_DEVICE_TYPE_WIFI = 2; diff --git a/selfdrive/ui/qt/network/wifi_manager.cc b/selfdrive/ui/qt/network/wifi_manager.cc index ebb5cb8736..717da47096 100644 --- a/selfdrive/ui/qt/network/wifi_manager.cc +++ b/selfdrive/ui/qt/network/wifi_manager.cc @@ -1,5 +1,7 @@ #include "selfdrive/ui/qt/network/wifi_manager.h" +#include + #include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/widgets/prime.h" @@ -14,9 +16,15 @@ bool compare_by_strength(const Network &a, const Network &b) { template T call(const QString &path, const QString &interface, const QString &method, Args &&...args) { - QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + QDBusInterface nm(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); nm.setTimeout(DBUS_TIMEOUT); - QDBusMessage response = nm.call(method, args...); + + QDBusMessage response = nm.call(method, std::forward(args)...); + if (response.type() == QDBusMessage::ErrorMessage) { + qCritical() << "DBus call error:" << response.errorMessage(); + return T(); + } + if constexpr (std::is_same_v) { return response; } else if (response.arguments().count() >= 1) { @@ -98,11 +106,21 @@ void WifiManager::refreshFinished(QDBusPendingCallWatcher *watcher) { ipv4_address = getIp4Address(); seenNetworks.clear(); - const QDBusReply> wather_reply = *watcher; - for (const QDBusObjectPath &path : wather_reply.value()) { - QDBusReply replay = call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "GetAll", NM_DBUS_INTERFACE_ACCESS_POINT); - auto properties = replay.value(); + const QDBusReply> watcher_reply = *watcher; + if (!watcher_reply.isValid()) { + qCritical() << "Failed to refresh"; + watcher->deleteLater(); + return; + } + + for (const QDBusObjectPath &path : watcher_reply.value()) { + QDBusReply reply = call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "GetAll", NM_DBUS_INTERFACE_ACCESS_POINT); + if (!reply.isValid()) { + qCritical() << "Failed to retrieve properties for path:" << path.path(); + continue; + } + auto properties = reply.value(); const QByteArray ssid = properties["Ssid"].toByteArray(); if (ssid.isEmpty()) continue; @@ -166,7 +184,7 @@ SecurityType WifiManager::getSecurityType(const QVariantMap &properties) { } } -void WifiManager::connect(const Network &n, const QString &password, const QString &username) { +void WifiManager::connect(const Network &n, const bool is_hidden, const QString &password, const QString &username) { setCurrentConnecting(n.ssid); forgetConnection(n.ssid); // Clear all connections that may already exist to the network we are connecting Connection connection; @@ -176,6 +194,7 @@ void WifiManager::connect(const Network &n, const QString &password, const QStri connection["connection"]["autoconnect-retries"] = 0; connection["802-11-wireless"]["ssid"] = n.ssid; + connection["802-11-wireless"]["hidden"] = is_hidden; connection["802-11-wireless"]["mode"] = "infrastructure"; if (n.security_type == SecurityType::WPA) { @@ -209,16 +228,8 @@ void WifiManager::deactivateConnection(const QDBusObjectPath &path) { } QVector WifiManager::getActiveConnections() { - QVector conns; - QDBusObjectPath path; - const QDBusArgument &arr = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "ActiveConnections"); - arr.beginArray(); - while (!arr.atEnd()) { - arr >> path; - conns.push_back(path); - } - arr.endArray(); - return conns; + auto result = call(NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE, "ActiveConnections"); + return qdbus_cast>(result); } bool WifiManager::isKnownConnection(const QString &ssid) { @@ -427,7 +438,10 @@ void WifiManager::addTetheringConnection() { connection["ipv4"]["route-metric"] = 1100; connection["ipv6"]["method"] = "ignore"; - call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); + auto path = call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); + if (!path.path().isEmpty()) { + knownConnections[path] = tethering_ssid; + } } void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { diff --git a/selfdrive/ui/qt/network/wifi_manager.h b/selfdrive/ui/qt/network/wifi_manager.h index 7debffa452..2f6a1829d7 100644 --- a/selfdrive/ui/qt/network/wifi_manager.h +++ b/selfdrive/ui/qt/network/wifi_manager.h @@ -52,7 +52,7 @@ public: std::optional activateWifiConnection(const QString &ssid); NetworkType currentNetworkType(); void updateGsmSettings(bool roaming, QString apn, bool metered); - void connect(const Network &ssid, const QString &password = {}, const QString &username = {}); + void connect(const Network &ssid, const bool is_hidden = false, const QString &password = {}, const QString &username = {}); // Tethering functions void setTetheringEnabled(bool enabled); @@ -63,7 +63,7 @@ public: private: QString adapter; // Path to network manager wifi-device QTimer timer; - unsigned int raw_adapter_state; // Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState + unsigned int raw_adapter_state = NM_DEVICE_STATE_UNKNOWN; // Connection status https://developer.gnome.org/NetworkManager/1.26/nm-dbus-types.html#NMDeviceState QString connecting_to_network; QString tethering_ssid; const QString defaultTetheringPassword = "swagswagcomma"; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 85393cabc0..5aa33974ac 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -1,5 +1,3 @@ -#include "selfdrive/ui/qt/offroad/settings.h" - #include #include #include @@ -8,20 +6,14 @@ #include -#include "selfdrive/ui/qt/network/networking.h" - -#include "common/params.h" #include "common/watchdog.h" #include "common/util.h" -#include "system/hardware/hw.h" -#include "selfdrive/ui/qt/widgets/controls.h" -#include "selfdrive/ui/qt/widgets/input.h" +#include "selfdrive/ui/qt/network/networking.h" +#include "selfdrive/ui/qt/offroad/settings.h" +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/widgets/prime.h" #include "selfdrive/ui/qt/widgets/scrollview.h" #include "selfdrive/ui/qt/widgets/ssh_keys.h" -#include "selfdrive/ui/qt/widgets/toggle.h" -#include "selfdrive/ui/ui.h" -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/qt/qt_window.h" TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { // param, title, desc, icon @@ -91,9 +83,14 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { std::vector longi_button_texts{tr("Aggressive"), tr("Standard"), tr("Relaxed")}; long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"), tr("Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. " - "In relaxed mode openpilot will stay further away from lead cars."), + "In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " + "your steering wheel distance button."), "../assets/offroad/icon_speed_limit.png", longi_button_texts); + + // set up uiState update for personality setting + QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState); + for (auto &[param, title, desc, icon] : toggle_defs) { auto toggle = new ParamControl(param, title, desc, icon, this); @@ -119,6 +116,18 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }); } +void TogglesPanel::updateState(const UIState &s) { + const SubMaster &sm = *(s.sm); + + if (sm.updated("controlsState")) { + auto personality = sm["controlsState"].getControlsState().getPersonality(); + if (personality != s.scene.personality && s.scene.started && isVisible()) { + long_personality_setting->setCheckedButton(static_cast(personality)); + } + uiState()->scene.personality = personality; + } +} + void TogglesPanel::expandToggleDescription(const QString ¶m) { toggles[param.toStdString()]->showDescription(); } @@ -134,21 +143,14 @@ void TogglesPanel::updateToggles() { "

    %2


    " "%3
    " "

    %4


    " - "%5
    " - "

    %6


    " - "%7") + "%5
    ") .arg(tr("openpilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. Experimental features are listed below:")) .arg(tr("End-to-End Longitudinal Control")) .arg(tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. " "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; " "mistakes should be expected.")) - .arg(tr("Navigate on openpilot")) - .arg(tr("When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right " - "appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around " - "exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc.")) .arg(tr("New Driving Visualization")) - .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. " - "When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green.")); + .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.")); const bool is_release = params.getBool("IsReleaseBranch"); auto cp_bytes = params.get("CarParamsPersistent"); @@ -198,6 +200,14 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { addItem(new LabelControl(tr("Dongle ID"), getDongleId().value_or(tr("N/A")))); addItem(new LabelControl(tr("Serial"), params.get("HardwareSerial").c_str())); + pair_device = new ButtonControl(tr("Pair Device"), tr("PAIR"), + tr("Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer.")); + connect(pair_device, &ButtonControl::clicked, [=]() { + PairingPopup popup(this); + popup.exec(); + }); + addItem(pair_device); + // offroad-only buttons auto dcamBtn = new ButtonControl(tr("Driver Camera"), tr("PREVIEW"), @@ -245,9 +255,14 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(translateBtn); + QObject::connect(uiState(), &UIState::primeTypeChanged, [this] (PrimeType type) { + pair_device->setVisible(type == PrimeType::UNPAIRED); + }); QObject::connect(uiState(), &UIState::offroadTransition, [=](bool offroad) { for (auto btn : findChildren()) { - btn->setEnabled(offroad); + if (btn != pair_device) { + btn->setEnabled(offroad); + } } }); @@ -328,6 +343,11 @@ void DevicePanel::poweroff() { } } +void DevicePanel::showEvent(QShowEvent *event) { + pair_device->setVisible(uiState()->primeType() == PrimeType::UNPAIRED); + ListWidget::showEvent(event); +} + void SettingsWindow::showEvent(QShowEvent *event) { setCurrentPanel(0); } diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index a5dd25b14f..35a044bdd2 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -10,7 +10,7 @@ #include #include - +#include "selfdrive/ui/ui.h" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/widgets/controls.h" @@ -42,6 +42,8 @@ class DevicePanel : public ListWidget { Q_OBJECT public: explicit DevicePanel(SettingsWindow *parent); + void showEvent(QShowEvent *event) override; + signals: void reviewTrainingGuide(); void showDriverView(); @@ -53,6 +55,7 @@ private slots: private: Params params; + ButtonControl *pair_device; }; class TogglesPanel : public ListWidget { @@ -64,6 +67,9 @@ public: public slots: void expandToggleDescription(const QString ¶m); +private slots: + void updateState(const UIState &s); + private: Params params; std::map toggles; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index 15c022db9a..c8245205ce 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -17,7 +17,7 @@ void SoftwarePanel::checkForUpdates() { - std::system("pkill -SIGUSR1 -f selfdrive.updated"); + std::system("pkill -SIGUSR1 -f system.updated.updated"); } SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { @@ -36,7 +36,7 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { if (downloadBtn->text() == tr("CHECK")) { checkForUpdates(); } else { - std::system("pkill -SIGHUP -f selfdrive.updated"); + std::system("pkill -SIGHUP -f system.updated.updated"); } }); addItem(downloadBtn); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h deleted file mode 100644 index b3ba411453..0000000000 --- a/selfdrive/ui/qt/onroad.h +++ /dev/null @@ -1,142 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include "common/util.h" -#include "selfdrive/ui/ui.h" -#include "selfdrive/ui/qt/widgets/cameraview.h" - - -const int btn_size = 192; -const int img_size = (btn_size / 4) * 3; - - -// ***** onroad widgets ***** -class OnroadAlerts : public QWidget { - Q_OBJECT - -public: - OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} - void updateAlert(const Alert &a); - -protected: - void paintEvent(QPaintEvent*) override; - -private: - QColor bg; - Alert alert = {}; -}; - -class ExperimentalButton : public QPushButton { - Q_OBJECT - -public: - explicit ExperimentalButton(QWidget *parent = 0); - void updateState(const UIState &s); - -private: - void paintEvent(QPaintEvent *event) override; - void changeMode(); - - Params params; - QPixmap engage_img; - QPixmap experimental_img; - bool experimental_mode; - bool engageable; -}; - - -class MapSettingsButton : public QPushButton { - Q_OBJECT - -public: - explicit MapSettingsButton(QWidget *parent = 0); - -private: - void paintEvent(QPaintEvent *event) override; - - QPixmap settings_img; -}; - -// container window for the NVG UI -class AnnotatedCameraWidget : public CameraWidget { - Q_OBJECT - -public: - explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); - void updateState(const UIState &s); - - MapSettingsButton *map_settings_btn; - -private: - void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); - - QVBoxLayout *main_layout; - ExperimentalButton *experimental_btn; - QPixmap dm_img; - float speed; - QString speedUnit; - float setSpeed; - float speedLimit; - bool is_cruise_set = false; - bool is_metric = false; - bool dmActive = false; - bool hideBottomIcons = false; - bool rightHandDM = false; - float dm_fade_state = 1.0; - bool has_us_speed_limit = false; - bool has_eu_speed_limit = false; - bool v_ego_cluster_seen = false; - int status = STATUS_DISENGAGED; - std::unique_ptr pm; - - int skip_frame_count = 0; - bool wide_cam_requested = false; - -protected: - void paintGL() override; - void initializeGL() override; - void showEvent(QShowEvent *event) override; - void updateFrameMat() override; - void drawLaneLines(QPainter &painter, const UIState *s); - void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); - void drawHud(QPainter &p); - void drawDriverState(QPainter &painter, const UIState *s); - inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } - inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } - inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } - - double prev_draw_t = 0; - FirstOrderFilter fps_filter; -}; - -// container for all onroad widgets -class OnroadWindow : public QWidget { - Q_OBJECT - -public: - OnroadWindow(QWidget* parent = 0); - bool isMapVisible() const { return map && map->isVisible(); } - void showMapPanel(bool show) { if (map) map->setVisible(show); } - -signals: - void mapPanelRequested(); - -private: - void paintEvent(QPaintEvent *event); - void mousePressEvent(QMouseEvent* e) override; - OnroadAlerts *alerts; - AnnotatedCameraWidget *nvg; - QColor bg = bg_colors[STATUS_DISENGAGED]; - QWidget *map = nullptr; - QHBoxLayout* split; - -private slots: - void offroadTransition(bool offroad); - void primeChanged(bool prime); - void updateState(const UIState &s); -}; diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc new file mode 100644 index 0000000000..0235c5ff42 --- /dev/null +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -0,0 +1,112 @@ +#include "selfdrive/ui/qt/onroad/alerts.h" + +#include +#include + +#include "selfdrive/ui/qt/util.h" + +void OnroadAlerts::updateState(const UIState &s) { + Alert a = getAlert(*(s.sm), s.scene.started_frame); + if (!alert.equal(a)) { + alert = a; + update(); + } +} + +void OnroadAlerts::clear() { + alert = {}; + update(); +} + +OnroadAlerts::Alert OnroadAlerts::getAlert(const SubMaster &sm, uint64_t started_frame) { + const cereal::ControlsState::Reader &cs = sm["controlsState"].getControlsState(); + const uint64_t controls_frame = sm.rcv_frame("controlsState"); + + Alert a = {}; + if (controls_frame >= started_frame) { // Don't get old alert. + a = {cs.getAlertText1().cStr(), cs.getAlertText2().cStr(), + cs.getAlertType().cStr(), cs.getAlertSize(), cs.getAlertStatus()}; + } + + if (!sm.updated("controlsState") && (sm.frame - started_frame) > 5 * UI_FREQ) { + const int CONTROLS_TIMEOUT = 5; + const int controls_missing = (nanos_since_boot() - sm.rcv_time("controlsState")) / 1e9; + + // Handle controls timeout + if (controls_frame < started_frame) { + // car is started, but controlsState hasn't been seen at all + a = {tr("openpilot Unavailable"), tr("Waiting for controls to start"), + "controlsWaiting", cereal::ControlsState::AlertSize::MID, + cereal::ControlsState::AlertStatus::NORMAL}; + } else if (controls_missing > CONTROLS_TIMEOUT && !Hardware::PC()) { + // car is started, but controls is lagging or died + if (cs.getEnabled() && (controls_missing - CONTROLS_TIMEOUT) < 10) { + a = {tr("TAKE CONTROL IMMEDIATELY"), tr("Controls Unresponsive"), + "controlsUnresponsive", cereal::ControlsState::AlertSize::FULL, + cereal::ControlsState::AlertStatus::CRITICAL}; + } else { + a = {tr("Controls Unresponsive"), tr("Reboot Device"), + "controlsUnresponsivePermanent", cereal::ControlsState::AlertSize::MID, + cereal::ControlsState::AlertStatus::NORMAL}; + } + } + } + return a; +} + +void OnroadAlerts::paintEvent(QPaintEvent *event) { + if (alert.size == cereal::ControlsState::AlertSize::NONE) { + return; + } + static std::map alert_heights = { + {cereal::ControlsState::AlertSize::SMALL, 271}, + {cereal::ControlsState::AlertSize::MID, 420}, + {cereal::ControlsState::AlertSize::FULL, height()}, + }; + int h = alert_heights[alert.size]; + + int margin = 40; + int radius = 30; + if (alert.size == cereal::ControlsState::AlertSize::FULL) { + margin = 0; + radius = 0; + } + QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); + + QPainter p(this); + + // draw background + gradient + p.setPen(Qt::NoPen); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + p.setBrush(QBrush(alert_colors[alert.status])); + p.drawRoundedRect(r, radius, radius); + + QLinearGradient g(0, r.y(), 0, r.bottom()); + g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05)); + g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35)); + + p.setCompositionMode(QPainter::CompositionMode_DestinationOver); + p.setBrush(QBrush(g)); + p.drawRoundedRect(r, radius, radius); + p.setCompositionMode(QPainter::CompositionMode_SourceOver); + + // text + const QPoint c = r.center(); + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setRenderHint(QPainter::TextAntialiasing); + if (alert.size == cereal::ControlsState::AlertSize::SMALL) { + p.setFont(InterFont(74, QFont::DemiBold)); + p.drawText(r, Qt::AlignCenter, alert.text1); + } else if (alert.size == cereal::ControlsState::AlertSize::MID) { + p.setFont(InterFont(88, QFont::Bold)); + p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1); + p.setFont(InterFont(66)); + p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2); + } else if (alert.size == cereal::ControlsState::AlertSize::FULL) { + bool l = alert.text1.length() > 15; + p.setFont(InterFont(l ? 132 : 177, QFont::Bold)); + p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1); + p.setFont(InterFont(88)); + p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2); + } +} diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h new file mode 100644 index 0000000000..1f76ba305b --- /dev/null +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +#include "selfdrive/ui/ui.h" + +class OnroadAlerts : public QWidget { + Q_OBJECT + +public: + OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} + void updateState(const UIState &s); + void clear(); + +protected: + struct Alert { + QString text1; + QString text2; + QString type; + cereal::ControlsState::AlertSize size; + cereal::ControlsState::AlertStatus status; + + bool equal(const Alert &other) const { + return text1 == other.text1 && text2 == other.text2 && type == other.type; + } + }; + + const QMap alert_colors = { + {cereal::ControlsState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, + {cereal::ControlsState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)}, + {cereal::ControlsState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, + }; + + void paintEvent(QPaintEvent*) override; + OnroadAlerts::Alert getAlert(const SubMaster &sm, uint64_t started_frame); + + QColor bg; + Alert alert = {}; +}; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc similarity index 67% rename from selfdrive/ui/qt/onroad.cc rename to selfdrive/ui/qt/onroad/annotated_camera.cc index 01750eaf2f..f7fb6b480f 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -1,265 +1,13 @@ -#include "selfdrive/ui/qt/onroad.h" +#include "selfdrive/ui/qt/onroad/annotated_camera.h" + +#include #include #include -#include -#include -#include - -#include -#include #include "common/swaglog.h" -#include "common/timing.h" +#include "selfdrive/ui/qt/onroad/buttons.h" #include "selfdrive/ui/qt/util.h" -#ifdef ENABLE_MAPS -#include "selfdrive/ui/qt/maps/map_helpers.h" -#include "selfdrive/ui/qt/maps/map_panel.h" -#endif - -static void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity) { - p.setRenderHint(QPainter::Antialiasing); - p.setOpacity(1.0); // bg dictates opacity of ellipse - p.setPen(Qt::NoPen); - p.setBrush(bg); - p.drawEllipse(center, btn_size / 2, btn_size / 2); - p.setOpacity(opacity); - p.drawPixmap(center - QPoint(img.width() / 2, img.height() / 2), img); - p.setOpacity(1.0); -} - -OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { - QVBoxLayout *main_layout = new QVBoxLayout(this); - main_layout->setMargin(UI_BORDER_SIZE); - QStackedLayout *stacked_layout = new QStackedLayout; - stacked_layout->setStackingMode(QStackedLayout::StackAll); - main_layout->addLayout(stacked_layout); - - nvg = new AnnotatedCameraWidget(VISION_STREAM_ROAD, this); - - QWidget * split_wrapper = new QWidget; - split = new QHBoxLayout(split_wrapper); - split->setContentsMargins(0, 0, 0, 0); - split->setSpacing(0); - split->addWidget(nvg); - - if (getenv("DUAL_CAMERA_VIEW")) { - CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this); - split->insertWidget(0, arCam); - } - - if (getenv("MAP_RENDER_VIEW")) { - CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this); - split->insertWidget(0, map_render); - } - - stacked_layout->addWidget(split_wrapper); - - alerts = new OnroadAlerts(this); - alerts->setAttribute(Qt::WA_TransparentForMouseEvents, true); - stacked_layout->addWidget(alerts); - - // setup stacking order - alerts->raise(); - - setAttribute(Qt::WA_OpaquePaintEvent); - QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState); - QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition); - QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged); -} - -void OnroadWindow::updateState(const UIState &s) { - if (!s.scene.started) { - return; - } - - QColor bgColor = bg_colors[s.status]; - Alert alert = Alert::get(*(s.sm), s.scene.started_frame); - alerts->updateAlert(alert); - - if (s.scene.map_on_left) { - split->setDirection(QBoxLayout::LeftToRight); - } else { - split->setDirection(QBoxLayout::RightToLeft); - } - - nvg->updateState(s); - - if (bg != bgColor) { - // repaint border - bg = bgColor; - update(); - } -} - -void OnroadWindow::mousePressEvent(QMouseEvent* e) { -#ifdef ENABLE_MAPS - if (map != nullptr) { - // Switch between map and sidebar when using navigate on openpilot - bool sidebarVisible = geometry().x() > 0; - bool show_map = uiState()->scene.navigate_on_openpilot ? sidebarVisible : !sidebarVisible; - map->setVisible(show_map && !map->isVisible()); - } -#endif - // propagation event to parent(HomeWindow) - QWidget::mousePressEvent(e); -} - -void OnroadWindow::offroadTransition(bool offroad) { -#ifdef ENABLE_MAPS - if (!offroad) { - if (map == nullptr && (uiState()->hasPrime() || !MAPBOX_TOKEN.isEmpty())) { - auto m = new MapPanel(get_mapbox_settings()); - map = m; - - QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested); - QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); - nvg->map_settings_btn->setEnabled(true); - - m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE); - split->insertWidget(0, m); - - // hidden by default, made visible when navRoute is published - m->setVisible(false); - } - } -#endif - - alerts->updateAlert({}); -} - -void OnroadWindow::primeChanged(bool prime) { -#ifdef ENABLE_MAPS - if (map && (!prime && MAPBOX_TOKEN.isEmpty())) { - nvg->map_settings_btn->setEnabled(false); - nvg->map_settings_btn->setVisible(false); - map->deleteLater(); - map = nullptr; - } -#endif -} - -void OnroadWindow::paintEvent(QPaintEvent *event) { - QPainter p(this); - p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); -} - -// ***** onroad widgets ***** - -// OnroadAlerts -void OnroadAlerts::updateAlert(const Alert &a) { - if (!alert.equal(a)) { - alert = a; - update(); - } -} - -void OnroadAlerts::paintEvent(QPaintEvent *event) { - if (alert.size == cereal::ControlsState::AlertSize::NONE) { - return; - } - static std::map alert_heights = { - {cereal::ControlsState::AlertSize::SMALL, 271}, - {cereal::ControlsState::AlertSize::MID, 420}, - {cereal::ControlsState::AlertSize::FULL, height()}, - }; - int h = alert_heights[alert.size]; - - int margin = 40; - int radius = 30; - if (alert.size == cereal::ControlsState::AlertSize::FULL) { - margin = 0; - radius = 0; - } - QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); - - QPainter p(this); - - // draw background + gradient - p.setPen(Qt::NoPen); - p.setCompositionMode(QPainter::CompositionMode_SourceOver); - p.setBrush(QBrush(alert_colors[alert.status])); - p.drawRoundedRect(r, radius, radius); - - QLinearGradient g(0, r.y(), 0, r.bottom()); - g.setColorAt(0, QColor::fromRgbF(0, 0, 0, 0.05)); - g.setColorAt(1, QColor::fromRgbF(0, 0, 0, 0.35)); - - p.setCompositionMode(QPainter::CompositionMode_DestinationOver); - p.setBrush(QBrush(g)); - p.drawRoundedRect(r, radius, radius); - p.setCompositionMode(QPainter::CompositionMode_SourceOver); - - // text - const QPoint c = r.center(); - p.setPen(QColor(0xff, 0xff, 0xff)); - p.setRenderHint(QPainter::TextAntialiasing); - if (alert.size == cereal::ControlsState::AlertSize::SMALL) { - p.setFont(InterFont(74, QFont::DemiBold)); - p.drawText(r, Qt::AlignCenter, alert.text1); - } else if (alert.size == cereal::ControlsState::AlertSize::MID) { - p.setFont(InterFont(88, QFont::Bold)); - p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1); - p.setFont(InterFont(66)); - p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2); - } else if (alert.size == cereal::ControlsState::AlertSize::FULL) { - bool l = alert.text1.length() > 15; - p.setFont(InterFont(l ? 132 : 177, QFont::Bold)); - p.drawText(QRect(0, r.y() + (l ? 240 : 270), width(), 600), Qt::AlignHCenter | Qt::TextWordWrap, alert.text1); - p.setFont(InterFont(88)); - p.drawText(QRect(0, r.height() - (l ? 361 : 420), width(), 300), Qt::AlignHCenter | Qt::TextWordWrap, alert.text2); - } -} - -// ExperimentalButton -ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { - setFixedSize(btn_size, btn_size); - - engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); - experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); - QObject::connect(this, &QPushButton::clicked, this, &ExperimentalButton::changeMode); -} - -void ExperimentalButton::changeMode() { - const auto cp = (*uiState()->sm)["carParams"].getCarParams(); - bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); - if (can_change) { - params.putBool("ExperimentalMode", !experimental_mode); - } -} - -void ExperimentalButton::updateState(const UIState &s) { - const auto cs = (*s.sm)["controlsState"].getControlsState(); - bool eng = cs.getEngageable() || cs.getEnabled(); - if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { - engageable = eng; - experimental_mode = cs.getExperimentalMode(); - update(); - } -} - -void ExperimentalButton::paintEvent(QPaintEvent *event) { - QPainter p(this); - QPixmap img = experimental_mode ? experimental_img : engage_img; - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); -} - - -// MapSettingsButton -MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) { - setFixedSize(btn_size, btn_size); - settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size}); - - // hidden by default, made visible if map is created (has prime or mapbox token) - setVisible(false); - setEnabled(false); -} - -void MapSettingsButton::paintEvent(QPaintEvent *event) { - QPainter p(this); - drawIcon(p, QPoint(btn_size / 2, btn_size / 2), settings_img, QColor(0, 0, 0, 166), isDown() ? 0.6 : 1.0); -} - // Window that shows camera view and variety of info drawn on top AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h new file mode 100644 index 0000000000..0be4adfffa --- /dev/null +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#include "selfdrive/ui/qt/onroad/buttons.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + +class AnnotatedCameraWidget : public CameraWidget { + Q_OBJECT + +public: + explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); + void updateState(const UIState &s); + + MapSettingsButton *map_settings_btn; + +private: + void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); + + QVBoxLayout *main_layout; + ExperimentalButton *experimental_btn; + QPixmap dm_img; + float speed; + QString speedUnit; + float setSpeed; + float speedLimit; + bool is_cruise_set = false; + bool is_metric = false; + bool dmActive = false; + bool hideBottomIcons = false; + bool rightHandDM = false; + float dm_fade_state = 1.0; + bool has_us_speed_limit = false; + bool has_eu_speed_limit = false; + bool v_ego_cluster_seen = false; + int status = STATUS_DISENGAGED; + std::unique_ptr pm; + + int skip_frame_count = 0; + bool wide_cam_requested = false; + +protected: + void paintGL() override; + void initializeGL() override; + void showEvent(QShowEvent *event) override; + void updateFrameMat() override; + void drawLaneLines(QPainter &painter, const UIState *s); + void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd); + void drawHud(QPainter &p); + void drawDriverState(QPainter &painter, const UIState *s); + inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + inline QColor whiteColor(int alpha = 255) { return QColor(255, 255, 255, alpha); } + inline QColor blackColor(int alpha = 255) { return QColor(0, 0, 0, alpha); } + + double prev_draw_t = 0; + FirstOrderFilter fps_filter; +}; diff --git a/selfdrive/ui/qt/onroad/buttons.cc b/selfdrive/ui/qt/onroad/buttons.cc new file mode 100644 index 0000000000..75ec316174 --- /dev/null +++ b/selfdrive/ui/qt/onroad/buttons.cc @@ -0,0 +1,64 @@ +#include "selfdrive/ui/qt/onroad/buttons.h" + +#include + +#include "selfdrive/ui/qt/util.h" + +void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity) { + p.setRenderHint(QPainter::Antialiasing); + p.setOpacity(1.0); // bg dictates opacity of ellipse + p.setPen(Qt::NoPen); + p.setBrush(bg); + p.drawEllipse(center, btn_size / 2, btn_size / 2); + p.setOpacity(opacity); + p.drawPixmap(center - QPoint(img.width() / 2, img.height() / 2), img); + p.setOpacity(1.0); +} + +// ExperimentalButton +ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { + setFixedSize(btn_size, btn_size); + + engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); + experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size}); + QObject::connect(this, &QPushButton::clicked, this, &ExperimentalButton::changeMode); +} + +void ExperimentalButton::changeMode() { + const auto cp = (*uiState()->sm)["carParams"].getCarParams(); + bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); + if (can_change) { + params.putBool("ExperimentalMode", !experimental_mode); + } +} + +void ExperimentalButton::updateState(const UIState &s) { + const auto cs = (*s.sm)["controlsState"].getControlsState(); + bool eng = cs.getEngageable() || cs.getEnabled(); + if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) { + engageable = eng; + experimental_mode = cs.getExperimentalMode(); + update(); + } +} + +void ExperimentalButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + QPixmap img = experimental_mode ? experimental_img : engage_img; + drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0); +} + +// MapSettingsButton +MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) { + setFixedSize(btn_size, btn_size); + settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size}); + + // hidden by default, made visible if map is created (has prime or mapbox token) + setVisible(false); + setEnabled(false); +} + +void MapSettingsButton::paintEvent(QPaintEvent *event) { + QPainter p(this); + drawIcon(p, QPoint(btn_size / 2, btn_size / 2), settings_img, QColor(0, 0, 0, 166), isDown() ? 0.6 : 1.0); +} diff --git a/selfdrive/ui/qt/onroad/buttons.h b/selfdrive/ui/qt/onroad/buttons.h new file mode 100644 index 0000000000..b0757795fb --- /dev/null +++ b/selfdrive/ui/qt/onroad/buttons.h @@ -0,0 +1,41 @@ +#pragma once + +#include + +#include "selfdrive/ui/ui.h" + +const int btn_size = 192; +const int img_size = (btn_size / 4) * 3; + +class ExperimentalButton : public QPushButton { + Q_OBJECT + +public: + explicit ExperimentalButton(QWidget *parent = 0); + void updateState(const UIState &s); + +private: + void paintEvent(QPaintEvent *event) override; + void changeMode(); + + Params params; + QPixmap engage_img; + QPixmap experimental_img; + bool experimental_mode; + bool engageable; +}; + + +class MapSettingsButton : public QPushButton { + Q_OBJECT + +public: + explicit MapSettingsButton(QWidget *parent = 0); + +private: + void paintEvent(QPaintEvent *event) override; + + QPixmap settings_img; +}; + +void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, const QBrush &bg, float opacity); diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc new file mode 100644 index 0000000000..66eb1812e6 --- /dev/null +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -0,0 +1,129 @@ +#include "selfdrive/ui/qt/onroad/onroad_home.h" + +#include +#include + +#ifdef ENABLE_MAPS +#include "selfdrive/ui/qt/maps/map_helpers.h" +#include "selfdrive/ui/qt/maps/map_panel.h" +#endif + +#include "selfdrive/ui/qt/util.h" + +OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { + QVBoxLayout *main_layout = new QVBoxLayout(this); + main_layout->setMargin(UI_BORDER_SIZE); + QStackedLayout *stacked_layout = new QStackedLayout; + stacked_layout->setStackingMode(QStackedLayout::StackAll); + main_layout->addLayout(stacked_layout); + + nvg = new AnnotatedCameraWidget(VISION_STREAM_ROAD, this); + + QWidget * split_wrapper = new QWidget; + split = new QHBoxLayout(split_wrapper); + split->setContentsMargins(0, 0, 0, 0); + split->setSpacing(0); + split->addWidget(nvg); + + if (getenv("DUAL_CAMERA_VIEW")) { + CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this); + split->insertWidget(0, arCam); + } + + if (getenv("MAP_RENDER_VIEW")) { + CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this); + split->insertWidget(0, map_render); + } + + stacked_layout->addWidget(split_wrapper); + + alerts = new OnroadAlerts(this); + alerts->setAttribute(Qt::WA_TransparentForMouseEvents, true); + stacked_layout->addWidget(alerts); + + // setup stacking order + alerts->raise(); + + setAttribute(Qt::WA_OpaquePaintEvent); + QObject::connect(uiState(), &UIState::uiUpdate, this, &OnroadWindow::updateState); + QObject::connect(uiState(), &UIState::offroadTransition, this, &OnroadWindow::offroadTransition); + QObject::connect(uiState(), &UIState::primeChanged, this, &OnroadWindow::primeChanged); +} + +void OnroadWindow::updateState(const UIState &s) { + if (!s.scene.started) { + return; + } + + if (s.scene.map_on_left) { + split->setDirection(QBoxLayout::LeftToRight); + } else { + split->setDirection(QBoxLayout::RightToLeft); + } + + alerts->updateState(s); + nvg->updateState(s); + + QColor bgColor = bg_colors[s.status]; + if (bg != bgColor) { + // repaint border + bg = bgColor; + update(); + } +} + +void OnroadWindow::mousePressEvent(QMouseEvent* e) { +#ifdef ENABLE_MAPS + if (map != nullptr) { + bool sidebarVisible = geometry().x() > 0; + bool show_map = !sidebarVisible; + map->setVisible(show_map && !map->isVisible()); + } +#endif + // propagation event to parent(HomeWindow) + QWidget::mousePressEvent(e); +} + +void OnroadWindow::createMapWidget() { +#ifdef ENABLE_MAPS + auto m = new MapPanel(get_mapbox_settings()); + map = m; + QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested); + QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); + nvg->map_settings_btn->setEnabled(true); + + m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE); + split->insertWidget(0, m); + // hidden by default, made visible when navRoute is published + m->setVisible(false); +#endif +} + +void OnroadWindow::offroadTransition(bool offroad) { +#ifdef ENABLE_MAPS + if (!offroad) { + if (map == nullptr && (uiState()->hasPrime() || !MAPBOX_TOKEN.isEmpty())) { + createMapWidget(); + } + } +#endif + alerts->clear(); +} + +void OnroadWindow::primeChanged(bool prime) { +#ifdef ENABLE_MAPS + if (map && (!prime && MAPBOX_TOKEN.isEmpty())) { + nvg->map_settings_btn->setEnabled(false); + nvg->map_settings_btn->setVisible(false); + map->deleteLater(); + map = nullptr; + } else if (!map && (prime || !MAPBOX_TOKEN.isEmpty())) { + createMapWidget(); + } +#endif +} + +void OnroadWindow::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255)); +} diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h new file mode 100644 index 0000000000..4976f56a67 --- /dev/null +++ b/selfdrive/ui/qt/onroad/onroad_home.h @@ -0,0 +1,31 @@ +#pragma once + +#include "selfdrive/ui/qt/onroad/alerts.h" +#include "selfdrive/ui/qt/onroad/annotated_camera.h" + +class OnroadWindow : public QWidget { + Q_OBJECT + +public: + OnroadWindow(QWidget* parent = 0); + bool isMapVisible() const { return map && map->isVisible(); } + void showMapPanel(bool show) { if (map) map->setVisible(show); } + +signals: + void mapPanelRequested(); + +private: + void createMapWidget(); + void paintEvent(QPaintEvent *event); + void mousePressEvent(QMouseEvent* e) override; + OnroadAlerts *alerts; + AnnotatedCameraWidget *nvg; + QColor bg = bg_colors[STATUS_DISENGAGED]; + QWidget *map = nullptr; + QHBoxLayout* split; + +private slots: + void offroadTransition(bool offroad); + void primeChanged(bool prime); + void updateState(const UIState &s); +}; diff --git a/selfdrive/ui/qt/python_helpers.py b/selfdrive/ui/qt/python_helpers.py new file mode 100644 index 0000000000..88c36290d9 --- /dev/null +++ b/selfdrive/ui/qt/python_helpers.py @@ -0,0 +1,20 @@ +import os +from cffi import FFI + +import sip + +from openpilot.common.ffi_wrapper import suffix +from openpilot.common.basedir import BASEDIR + + +def get_ffi(): + lib = os.path.join(BASEDIR, "selfdrive", "ui", "qt", "libpython_helpers" + suffix()) + + ffi = FFI() + ffi.cdef("void set_main_window(void *w);") + return ffi, ffi.dlopen(lib) + + +def set_main_window(widget): + ffi, lib = get_ffi() + lib.set_main_window(ffi.cast('void*', sip.unwrapinstance(widget))) diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index 4f2e4f48cb..aff9b015b3 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -20,7 +20,7 @@ #include "selfdrive/ui/qt/widgets/input.h" const std::string USER_AGENT = "AGNOSSetup-"; -const QString TEST_URL = "https://openpilot.comma.ai"; +const QString OPENPILOT_URL = "https://openpilot.comma.ai"; bool is_elf(char *fname) { FILE *fp = fopen(fname, "rb"); @@ -51,7 +51,7 @@ void Setup::download(QString url) { list = curl_slist_append(list, ("X-openpilot-serial: " + Hardware::get_serial()).c_str()); char tmpfile[] = "/tmp/installer_XXXXXX"; - FILE *fp = fdopen(mkstemp(tmpfile), "w"); + FILE *fp = fdopen(mkstemp(tmpfile), "wb"); curl_easy_setopt(curl, CURLOPT_URL, url.toStdString().c_str()); curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, NULL); @@ -201,20 +201,8 @@ QWidget * Setup::network_setup() { QPushButton *cont = new QPushButton(); cont->setObjectName("navBtn"); cont->setProperty("primary", true); - QObject::connect(cont, &QPushButton::clicked, [=]() { - auto w = currentWidget(); - QTimer::singleShot(0, [=]() { - setCurrentWidget(downloading_widget); - }); - QString url = InputDialog::getText(tr("Enter URL"), this, tr("for Custom Software")); - if (!url.isEmpty()) { - QTimer::singleShot(1000, this, [=]() { - download(url); - }); - } else { - setCurrentWidget(w); - } - }); + cont->setEnabled(false); + QObject::connect(cont, &QPushButton::clicked, this, &Setup::nextPage); blayout->addWidget(cont); // setup timer for testing internet connection @@ -229,11 +217,11 @@ QWidget * Setup::network_setup() { } repaint(); }); - request->sendRequest(TEST_URL); + request->sendRequest(OPENPILOT_URL); QTimer *timer = new QTimer(this); QObject::connect(timer, &QTimer::timeout, [=]() { if (!request->active() && cont->isVisible()) { - request->sendRequest(TEST_URL); + request->sendRequest(OPENPILOT_URL); } }); timer->start(1000); @@ -241,6 +229,106 @@ QWidget * Setup::network_setup() { return widget; } +QWidget * radio_button(QString title, QButtonGroup *group) { + QPushButton *btn = new QPushButton(title); + btn->setCheckable(true); + group->addButton(btn); + btn->setStyleSheet(R"( + QPushButton { + height: 230; + padding-left: 100px; + padding-right: 100px; + text-align: left; + font-size: 80px; + font-weight: 400; + border-radius: 10px; + background-color: #4F4F4F; + } + QPushButton:checked { + background-color: #465BEA; + } + )"); + + // checkmark icon + QPixmap pix(":/img_circled_check.svg"); + btn->setIcon(pix); + btn->setIconSize(QSize(0, 0)); + btn->setLayoutDirection(Qt::RightToLeft); + QObject::connect(btn, &QPushButton::toggled, [=](bool checked) { + btn->setIconSize(checked ? QSize(104, 104) : QSize(0, 0)); + }); + return btn; +} + +QWidget * Setup::software_selection() { + QWidget *widget = new QWidget(); + QVBoxLayout *main_layout = new QVBoxLayout(widget); + main_layout->setContentsMargins(55, 50, 55, 50); + main_layout->setSpacing(0); + + // title + QLabel *title = new QLabel(tr("Choose Software to Install")); + title->setStyleSheet("font-size: 90px; font-weight: 500;"); + main_layout->addWidget(title, 0, Qt::AlignLeft | Qt::AlignTop); + + main_layout->addSpacing(50); + + // openpilot + custom radio buttons + QButtonGroup *group = new QButtonGroup(widget); + group->setExclusive(true); + + QWidget *openpilot = radio_button(tr("openpilot"), group); + main_layout->addWidget(openpilot); + + main_layout->addSpacing(30); + + QWidget *custom = radio_button(tr("Custom Software"), group); + main_layout->addWidget(custom); + + main_layout->addStretch(); + + // back + continue buttons + QHBoxLayout *blayout = new QHBoxLayout; + main_layout->addLayout(blayout); + blayout->setSpacing(50); + + QPushButton *back = new QPushButton(tr("Back")); + back->setObjectName("navBtn"); + QObject::connect(back, &QPushButton::clicked, this, &Setup::prevPage); + blayout->addWidget(back); + + QPushButton *cont = new QPushButton(tr("Continue")); + cont->setObjectName("navBtn"); + cont->setEnabled(false); + cont->setProperty("primary", true); + blayout->addWidget(cont); + + QObject::connect(cont, &QPushButton::clicked, [=]() { + auto w = currentWidget(); + QTimer::singleShot(0, [=]() { + setCurrentWidget(downloading_widget); + }); + QString url = OPENPILOT_URL; + if (group->checkedButton() != openpilot) { + url = InputDialog::getText(tr("Enter URL"), this, tr("for Custom Software")); + } + if (!url.isEmpty()) { + QTimer::singleShot(1000, this, [=]() { + download(url); + }); + } else { + setCurrentWidget(w); + } + }); + + connect(group, QOverload::of(&QButtonGroup::buttonClicked), [=](QAbstractButton *btn) { + btn->setChecked(true); + cont->setEnabled(true); + }); + + return widget; +} + QWidget * Setup::downloading() { QWidget *widget = new QWidget(); QVBoxLayout *main_layout = new QVBoxLayout(widget); @@ -326,6 +414,7 @@ Setup::Setup(QWidget *parent) : QStackedWidget(parent) { addWidget(getting_started()); addWidget(network_setup()); + addWidget(software_selection()); downloading_widget = downloading(); addWidget(downloading_widget); diff --git a/selfdrive/ui/qt/setup/setup.h b/selfdrive/ui/qt/setup/setup.h index 8c33acc380..ee5cc85483 100644 --- a/selfdrive/ui/qt/setup/setup.h +++ b/selfdrive/ui/qt/setup/setup.h @@ -17,6 +17,7 @@ private: QWidget *low_voltage(); QWidget *getting_started(); QWidget *network_setup(); + QWidget *software_selection(); QWidget *downloading(); QWidget *download_failed(QLabel *url, QLabel *body); diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index e952940056..75b966c9aa 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -55,7 +55,7 @@ void Sidebar::mouseReleaseEvent(QMouseEvent *event) { flag_pressed = settings_pressed = false; update(); } - if (home_btn.contains(event->pos())) { + if (onroad && home_btn.contains(event->pos())) { MessageBuilder msg; msg.initEvent().initUserFlag(); pm->send("userFlag", msg); diff --git a/selfdrive/ui/qt/util.cc b/selfdrive/ui/qt/util.cc index bc3c494fa7..0bf5f2865e 100644 --- a/selfdrive/ui/qt/util.cc +++ b/selfdrive/ui/qt/util.cc @@ -61,7 +61,7 @@ QString timeAgo(const QDateTime &date) { QString s; if (diff < 60) { - s = "now"; + s = QObject::tr("now"); } else if (diff < 60 * 60) { int minutes = diff / 60; s = QObject::tr("%n minute(s) ago", "", minutes); @@ -105,20 +105,21 @@ void initApp(int argc, char *argv[], bool disable_hidpi) { std::signal(SIGINT, sigTermHandler); std::signal(SIGTERM, sigTermHandler); - if (disable_hidpi) { + QString app_dir; #ifdef __APPLE__ - // Get the devicePixelRatio, and scale accordingly to maintain 1:1 rendering - QApplication tmp(argc, argv); - qputenv("QT_SCALE_FACTOR", QString::number(1.0 / tmp.devicePixelRatio() ).toLocal8Bit()); -#endif + // Get the devicePixelRatio, and scale accordingly to maintain 1:1 rendering + QApplication tmp(argc, argv); + app_dir = QCoreApplication::applicationDirPath(); + if (disable_hidpi) { + qputenv("QT_SCALE_FACTOR", QString::number(1.0 / tmp.devicePixelRatio()).toLocal8Bit()); } +#else + app_dir = QFileInfo(util::readlink("/proc/self/exe").c_str()).path(); +#endif qputenv("QT_DBL_CLICK_DIST", QByteArray::number(150)); - // ensure the current dir matches the exectuable's directory - QApplication tmp(argc, argv); - QString appDir = QCoreApplication::applicationDirPath(); - QDir::setCurrent(appDir); + QDir::setCurrent(app_dir); setQtSurfaceFormat(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 7b1f2f1d24..914b04b654 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -11,6 +11,7 @@ #include #include +#include #include #include @@ -41,6 +42,8 @@ const char frame_fragment_shader[] = "out vec4 colorOut;\n" "void main() {\n" " colorOut = texture(uTexture, vTexCoord);\n" + // gamma to improve worst case visibility when dark + " colorOut.rgb = pow(colorOut.rgb, vec3(1.0/1.28));\n" "}\n"; #else #ifdef __APPLE__ @@ -96,12 +99,13 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) } // namespace CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : - stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { + stream_name(stream_name), active_stream_type(type), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { setAttribute(Qt::WA_OpaquePaintEvent); qRegisterMetaType>("availableStreams"); QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); QObject::connect(this, &CameraWidget::vipcAvailableStreamsUpdated, this, &CameraWidget::availableStreamsUpdated, Qt::QueuedConnection); + QObject::connect(QApplication::instance(), &QCoreApplication::aboutToQuit, this, &CameraWidget::stopVipcThread); } CameraWidget::~CameraWidget() { diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index c97038cf43..85ec498735 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -22,7 +22,7 @@ #include #endif -#include "cereal/visionipc/visionipc_client.h" +#include "msgq/visionipc/visionipc_client.h" #include "system/camerad/cameras/camera_common.h" #include "selfdrive/ui/ui.h" diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index e4630c6590..aa304e0df6 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -223,10 +223,8 @@ public: button_group->addButton(button, i); } - QObject::connect(button_group, QOverload::of(&QButtonGroup::buttonToggled), [=](int id, bool checked) { - if (checked) { - params.put(key, std::to_string(id)); - } + QObject::connect(button_group, QOverload::of(&QButtonGroup::buttonClicked), [=](int id) { + params.put(key, std::to_string(id)); }); } @@ -236,6 +234,19 @@ public: } } + void setCheckedButton(int id) { + button_group->button(id)->setChecked(true); + } + + void refresh() { + int value = atoi(params.get(key).c_str()); + button_group->button(value)->setChecked(true); + } + + void showEvent(QShowEvent *event) override { + refresh(); + } + private: std::string key; Params params; diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 324d6cf6ae..2621612f67 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -235,7 +235,7 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) { mainLayout->addWidget(content); - primeUser->setVisible(uiState()->primeType()); + primeUser->setVisible(uiState()->hasPrime()); mainLayout->setCurrentIndex(1); setStyleSheet(R"( @@ -269,15 +269,16 @@ void SetupWidget::replyFinished(const QString &response, bool success) { } QJsonObject json = doc.object(); + bool is_paired = json["is_paired"].toBool(); PrimeType prime_type = static_cast(json["prime_type"].toInt()); - uiState()->setPrimeType(prime_type); + uiState()->setPrimeType(is_paired ? prime_type : PrimeType::UNPAIRED); - if (!json["is_paired"].toBool()) { + if (!is_paired) { mainLayout->setCurrentIndex(0); } else { popup->reject(); - primeUser->setVisible(prime_type); + primeUser->setVisible(uiState()->hasPrime()); mainLayout->setCurrentIndex(1); } } diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 74fd05ed7b..6b579fcc5d 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -75,10 +75,6 @@ void MainWindow::closeSettings() { if (uiState()->scene.started) { homeWindow->showSidebar(false); - // Map is always shown when using navigate on openpilot - if (uiState()->scene.navigate_on_openpilot) { - homeWindow->showMapPanel(true); - } } } diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 11caf809d1..0550a7db9e 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -3,7 +3,6 @@ import numpy as np import time import wave -from typing import Dict, Optional, Tuple from cereal import car, messaging from openpilot.common.basedir import BASEDIR @@ -27,7 +26,7 @@ DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied AudibleAlert = car.CarControl.HUDControl.AudibleAlert -sound_list: Dict[int, Tuple[str, Optional[int], float]] = { +sound_list: dict[int, tuple[str, int | None, float]] = { # AudibleAlert, file name, play count (none for infinite) AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), @@ -64,7 +63,7 @@ class Soundd: self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) def load_sounds(self): - self.loaded_sounds: Dict[int, np.ndarray] = {} + self.loaded_sounds: dict[int, np.ndarray] = {} # Load all sounds for sound in sound_list: diff --git a/selfdrive/ui/tests/.gitignore b/selfdrive/ui/tests/.gitignore new file mode 100644 index 0000000000..6c624b66d3 --- /dev/null +++ b/selfdrive/ui/tests/.gitignore @@ -0,0 +1,6 @@ +test +playsound +test_sound +test_translations +ui_snapshot +test_ui/report \ No newline at end of file diff --git a/selfdrive/ui/tests/__init__.py b/selfdrive/ui/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/ui/tests/body.py b/selfdrive/ui/tests/body.py new file mode 100755 index 0000000000..7e24c2beb7 --- /dev/null +++ b/selfdrive/ui/tests/body.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +import time +import cereal.messaging as messaging + +if __name__ == "__main__": + while True: + pm = messaging.PubMaster(['carParams', 'carState']) + batt = 1. + while True: + msg = messaging.new_message('carParams') + msg.carParams.carName = "BODY" + msg.carParams.notCar = True + pm.send('carParams', msg) + + for b in range(100, 0, -1): + msg = messaging.new_message('carState') + msg.carState.charging = True + msg.carState.fuelGauge = b / 100. + pm.send('carState', msg) + time.sleep(0.1) + + time.sleep(1) diff --git a/selfdrive/ui/tests/create_test_translations.sh b/selfdrive/ui/tests/create_test_translations.sh new file mode 100755 index 0000000000..451a3cbfb0 --- /dev/null +++ b/selfdrive/ui/tests/create_test_translations.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +set -e + +UI_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"/.. +TEST_TEXT="(WRAPPED_SOURCE_TEXT)" +TEST_TS_FILE=$UI_DIR/translations/main_test_en.ts +TEST_QM_FILE=$UI_DIR/translations/main_test_en.qm + +# translation strings +UNFINISHED="<\/translation>" +TRANSLATED="$TEST_TEXT<\/translation>" + +mkdir -p $UI_DIR/translations +rm -f $TEST_TS_FILE $TEST_QM_FILE +lupdate -recursive "$UI_DIR" -ts $TEST_TS_FILE +sed -i "s/$UNFINISHED/$TRANSLATED/" $TEST_TS_FILE +lrelease $TEST_TS_FILE diff --git a/selfdrive/ui/tests/cycle_offroad_alerts.py b/selfdrive/ui/tests/cycle_offroad_alerts.py new file mode 100755 index 0000000000..75b19ceb90 --- /dev/null +++ b/selfdrive/ui/tests/cycle_offroad_alerts.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +import os +import sys +import time +import json + +from openpilot.common.basedir import BASEDIR +from openpilot.common.params import Params +from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert + +if __name__ == "__main__": + params = Params() + + with open(os.path.join(BASEDIR, "selfdrive/controls/lib/alerts_offroad.json")) as f: + offroad_alerts = json.load(f) + + t = 10 if len(sys.argv) < 2 else int(sys.argv[1]) + while True: + print("setting alert update") + params.put_bool("UpdateAvailable", True) + r = open(os.path.join(BASEDIR, "RELEASES.md")).read() + r = r[:r.find('\n\n')] # Slice latest release notes + params.put("UpdaterNewReleaseNotes", r + "\n") + + time.sleep(t) + params.put_bool("UpdateAvailable", False) + + # cycle through normal alerts + for a in offroad_alerts: + print("setting alert:", a) + set_offroad_alert(a, True) + time.sleep(t) + set_offroad_alert(a, False) + + print("no alert") + time.sleep(t) diff --git a/selfdrive/ui/tests/playsound.cc b/selfdrive/ui/tests/playsound.cc new file mode 100644 index 0000000000..6487d04790 --- /dev/null +++ b/selfdrive/ui/tests/playsound.cc @@ -0,0 +1,30 @@ +#include +#include +#include +#include + +int main(int argc, char **argv) { + + QApplication a(argc, argv); + + QTimer::singleShot(0, [=]{ + QSoundEffect s; + const char *vol = getenv("VOLUME"); + s.setVolume(vol ? atof(vol) : 1.0); + for (int i = 1; i < argc; i++) { + QString fn = argv[i]; + qDebug() << "playing" << fn; + + QEventLoop loop; + s.setSource(QUrl::fromLocalFile(fn)); + QEventLoop::connect(&s, &QSoundEffect::loadedChanged, &loop, &QEventLoop::quit); + loop.exec(); + s.play(); + QEventLoop::connect(&s, &QSoundEffect::playingChanged, &loop, &QEventLoop::quit); + loop.exec(); + } + QCoreApplication::exit(); + }); + + return a.exec(); +} diff --git a/selfdrive/ui/tests/test_runner.cc b/selfdrive/ui/tests/test_runner.cc new file mode 100644 index 0000000000..ac63139d17 --- /dev/null +++ b/selfdrive/ui/tests/test_runner.cc @@ -0,0 +1,25 @@ +#define CATCH_CONFIG_RUNNER +#include "catch2/catch.hpp" + +#include +#include +#include +#include + +int main(int argc, char **argv) { + // unit tests for Qt + QApplication app(argc, argv); + + QString language_file = "main_test_en"; + qDebug() << "Loading language:" << language_file; + + QTranslator translator; + QString translationsPath = QDir::cleanPath(qApp->applicationDirPath() + "/../translations"); + if (!translator.load(language_file, translationsPath)) { + qDebug() << "Failed to load translation file!"; + } + app.installTranslator(&translator); + + const int res = Catch::Session().run(argc, argv); + return (res < 0xff ? res : 0xff); +} diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py new file mode 100644 index 0000000000..468bc92cca --- /dev/null +++ b/selfdrive/ui/tests/test_soundd.py @@ -0,0 +1,35 @@ +from cereal import car +from cereal import messaging +from cereal.messaging import SubMaster, PubMaster +from openpilot.selfdrive.ui.soundd import CONTROLS_TIMEOUT, check_controls_timeout_alert + +import time + +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + + +class TestSoundd: + def test_check_controls_timeout_alert(self): + sm = SubMaster(['controlsState']) + pm = PubMaster(['controlsState']) + + for _ in range(100): + cs = messaging.new_message('controlsState') + cs.controlsState.enabled = True + + pm.send("controlsState", cs) + + time.sleep(0.01) + + sm.update(0) + + assert not check_controls_timeout_alert(sm) + + for _ in range(CONTROLS_TIMEOUT * 110): + sm.update(0) + time.sleep(0.01) + + assert check_controls_timeout_alert(sm) + + # TODO: add test with micd for checking that soundd actually outputs sounds + diff --git a/selfdrive/ui/tests/test_translations.cc b/selfdrive/ui/tests/test_translations.cc new file mode 100644 index 0000000000..fcefc5784f --- /dev/null +++ b/selfdrive/ui/tests/test_translations.cc @@ -0,0 +1,48 @@ +#include "catch2/catch.hpp" + +#include "common/params.h" +#include "selfdrive/ui/qt/window.h" + +const QString TEST_TEXT = "(WRAPPED_SOURCE_TEXT)"; // what each string should be translated to +QRegExp RE_NUM("\\d*"); + +QStringList getParentWidgets(QWidget* widget){ + QStringList parentWidgets; + while (widget->parentWidget() != Q_NULLPTR) { + widget = widget->parentWidget(); + parentWidgets.append(widget->metaObject()->className()); + } + return parentWidgets; +} + +template +void checkWidgetTrWrap(MainWindow &w) { + for (auto widget : w.findChildren()) { + const QString text = widget->text(); + bool isNumber = RE_NUM.exactMatch(text); + bool wrapped = text.contains(TEST_TEXT); + QString parentWidgets = getParentWidgets(widget).join("->"); + + if (!text.isEmpty() && !isNumber && !wrapped) { + FAIL(("\"" + text + "\" must be wrapped. Parent widgets: " + parentWidgets).toStdString()); + } + + // warn if source string wrapped, but UI adds text + // TODO: add way to ignore this + if (wrapped && text != TEST_TEXT) { + WARN(("\"" + text + "\" is dynamic and needs a custom retranslate function. Parent widgets: " + parentWidgets).toStdString()); + } + } +} + +// Tests all strings in the UI are wrapped with tr() +TEST_CASE("UI: test all strings wrapped") { + Params().remove("LanguageSetting"); + Params().remove("HardwareSerial"); + Params().remove("DongleId"); + qputenv("TICI", "1"); + + MainWindow w; + checkWidgetTrWrap(w); + checkWidgetTrWrap(w); +} diff --git a/selfdrive/ui/tests/test_translations.py b/selfdrive/ui/tests/test_translations.py old mode 100755 new mode 100644 index 9ba9054ea1..0967152fa4 --- a/selfdrive/ui/tests/test_translations.py +++ b/selfdrive/ui/tests/test_translations.py @@ -1,8 +1,7 @@ -#!/usr/bin/env python3 +import pytest import json import os import re -import unittest import shutil import tempfile import xml.etree.ElementTree as ET @@ -12,7 +11,7 @@ from parameterized import parameterized_class from openpilot.selfdrive.ui.update_translations import TRANSLATIONS_DIR, LANGUAGES_FILE, update_translations -with open(LANGUAGES_FILE, "r") as f: +with open(LANGUAGES_FILE) as f: translation_files = json.load(f) UNFINISHED_TRANSLATION_TAG = "" not in cur_translations, - f"{self.file} ({self.name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them") + assert "" not in cur_translations, \ + f"{self.file} ({self.name}) translation file has obsolete translations. Run selfdrive/ui/update_translations.py --vanish to remove them" def test_finished_translations(self): """ @@ -81,27 +80,27 @@ class TestTranslations(unittest.TestCase): numerusform = [t.text for t in translation.findall("numerusform")] for nf in numerusform: - self.assertIsNotNone(nf, f"Ensure all plural translation forms are completed: {source_text}") - self.assertIn("%n", nf, "Ensure numerus argument (%n) exists in translation.") - self.assertIsNone(FORMAT_ARG.search(nf), "Plural translations must use %n, not %1, %2, etc.: {}".format(numerusform)) + assert nf is not None, f"Ensure all plural translation forms are completed: {source_text}" + assert "%n" in nf, "Ensure numerus argument (%n) exists in translation." + assert FORMAT_ARG.search(nf) is None, f"Plural translations must use %n, not %1, %2, etc.: {numerusform}" else: - self.assertIsNotNone(translation.text, f"Ensure translation is completed: {source_text}") + assert translation.text is not None, f"Ensure translation is completed: {source_text}" source_args = FORMAT_ARG.findall(source_text) translation_args = FORMAT_ARG.findall(translation.text) - self.assertEqual(sorted(source_args), sorted(translation_args), - f"Ensure format arguments are consistent: `{source_text}` vs. `{translation.text}`") + assert sorted(source_args) == sorted(translation_args), \ + f"Ensure format arguments are consistent: `{source_text}` vs. `{translation.text}`" def test_no_locations(self): for line in self._read_translation_file(TRANSLATIONS_DIR, self.file).splitlines(): - self.assertFalse(line.strip().startswith(LOCATION_TAG), - f"Line contains location tag: {line.strip()}, remove all line numbers.") + assert not line.strip().startswith(LOCATION_TAG), \ + f"Line contains location tag: {line.strip()}, remove all line numbers." def test_entities_error(self): cur_translations = self._read_translation_file(TRANSLATIONS_DIR, self.file) matches = re.findall(r'@(\w+);', cur_translations) - self.assertEqual(len(matches), 0, f"The string(s) {matches} were found with '@' instead of '&'") + assert len(matches) == 0, f"The string(s) {matches} were found with '@' instead of '&'" def test_bad_language(self): IGNORED_WORDS = {'pédale'} @@ -128,7 +127,3 @@ class TestTranslations(unittest.TestCase): words = set(translation_text.translate(str.maketrans('', '', string.punctuation + '%n')).lower().split()) bad_words_found = words & (banned_words - IGNORED_WORDS) assert not bad_words_found, f"Bad language found in {self.name}: '{translation_text}'. Bad word(s): {', '.join(bad_words_found)}" - - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py new file mode 100644 index 0000000000..1d33c103a4 --- /dev/null +++ b/selfdrive/ui/tests/test_ui/run.py @@ -0,0 +1,193 @@ +from collections import namedtuple +import pathlib +import shutil +import sys +import jinja2 +import matplotlib.pyplot as plt +import numpy as np +import os +import pywinctl +import time + +from cereal import messaging, car, log +from msgq.visionipc import VisionIpcServer, VisionStreamType + +from cereal.messaging import SubMaster, PubMaster +from openpilot.common.mock import mock_messages +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.selfdrive.test.helpers import with_processes +from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state +from openpilot.tools.webcam.camera import Camera + +UI_DELAY = 0.5 # may be slower on CI? + +NetworkType = log.DeviceState.NetworkType +NetworkStrength = log.DeviceState.NetworkStrength + +EventName = car.CarEvent.EventName +EVENTS_BY_NAME = {v: k for k, v in EventName.schema.enumerants.items()} + + +def setup_common(click, pm: PubMaster): + Params().put("DongleId", "123456789012345") + dat = messaging.new_message('deviceState') + dat.deviceState.started = True + dat.deviceState.networkType = NetworkType.cell4G + dat.deviceState.networkStrength = NetworkStrength.moderate + dat.deviceState.freeSpacePercent = 80 + dat.deviceState.memoryUsagePercent = 2 + dat.deviceState.cpuTempC = [2,]*3 + dat.deviceState.gpuTempC = [2,]*3 + dat.deviceState.cpuUsagePercent = [2,]*8 + + pm.send("deviceState", dat) + +def setup_homescreen(click, pm: PubMaster): + setup_common(click, pm) + +def setup_settings_device(click, pm: PubMaster): + setup_common(click, pm) + + click(100, 100) + +def setup_settings_network(click, pm: PubMaster): + setup_common(click, pm) + + setup_settings_device(click, pm) + click(300, 600) + +def setup_onroad(click, pm: PubMaster): + setup_common(click, pm) + + dat = messaging.new_message('pandaStates', 1) + dat.pandaStates[0].ignitionLine = True + dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno + + pm.send("pandaStates", dat) + + d = DEVICE_CAMERAS[("tici", "ar0231")] + server = VisionIpcServer("camerad") + server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, d.fcam.width, d.fcam.height) + server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, d.dcam.width, d.dcam.height) + server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, d.fcam.width, d.fcam.height) + server.start_listener() + + time.sleep(0.5) # give time for vipc server to start + + IMG = Camera.bgr2nv12(np.random.randint(0, 255, (d.fcam.width, d.fcam.height, 3), dtype=np.uint8)) + IMG_BYTES = IMG.flatten().tobytes() + + cams = ('roadCameraState', 'wideRoadCameraState') + + frame_id = 0 + for cam in cams: + msg = messaging.new_message(cam) + cs = getattr(msg, cam) + cs.frameId = frame_id + cs.timestampSof = int((frame_id * DT_MDL) * 1e9) + cs.timestampEof = int((frame_id * DT_MDL) * 1e9) + cam_meta = meta_from_camera_state(cam) + + pm.send(msg.which(), msg) + server.send(cam_meta.stream, IMG_BYTES, cs.frameId, cs.timestampSof, cs.timestampEof) + +@mock_messages(['liveLocationKalman']) +def setup_onroad_map(click, pm: PubMaster): + setup_onroad(click, pm) + + click(500, 500) + + time.sleep(UI_DELAY) # give time for the map to render + +def setup_onroad_sidebar(click, pm: PubMaster): + setup_onroad_map(click, pm) + click(500, 500) + +CASES = { + "homescreen": setup_homescreen, + "settings_device": setup_settings_device, + "settings_network": setup_settings_network, + "onroad": setup_onroad, + "onroad_map": setup_onroad_map, + "onroad_sidebar": setup_onroad_sidebar +} + +TEST_DIR = pathlib.Path(__file__).parent + +TEST_OUTPUT_DIR = TEST_DIR / "report" +SCREENSHOTS_DIR = TEST_OUTPUT_DIR / "screenshots" + + +class TestUI: + def __init__(self): + os.environ["SCALE"] = "1" + sys.modules["mouseinfo"] = False + + def setup(self): + self.sm = SubMaster(["uiDebug"]) + self.pm = PubMaster(["deviceState", "pandaStates", "controlsState", 'roadCameraState', 'wideRoadCameraState', 'liveLocationKalman']) + while not self.sm.valid["uiDebug"]: + self.sm.update(1) + time.sleep(UI_DELAY) # wait a bit more for the UI to start rendering + try: + self.ui = pywinctl.getWindowsWithTitle("ui")[0] + except Exception as e: + print(f"failed to find ui window, assuming that it's in the top left (for Xvfb) {e}") + self.ui = namedtuple("bb", ["left", "top", "width", "height"])(0,0,2160,1080) + + def screenshot(self): + import pyautogui + im = pyautogui.screenshot(region=(self.ui.left, self.ui.top, self.ui.width, self.ui.height)) + assert im.width == 2160 + assert im.height == 1080 + img = np.array(im) + im.close() + return img + + def click(self, x, y, *args, **kwargs): + import pyautogui + pyautogui.click(self.ui.left + x, self.ui.top + y, *args, **kwargs) + time.sleep(UI_DELAY) # give enough time for the UI to react + + @with_processes(["ui"]) + def test_ui(self, name, setup_case): + self.setup() + + setup_case(self.click, self.pm) + + time.sleep(UI_DELAY) # wait a bit more for the UI to finish rendering + + im = self.screenshot() + plt.imsave(SCREENSHOTS_DIR / f"{name}.png", im) + + +def create_html_report(): + OUTPUT_FILE = TEST_OUTPUT_DIR / "index.html" + + with open(TEST_DIR / "template.html") as f: + template = jinja2.Template(f.read()) + + cases = {f.stem: (str(f.relative_to(TEST_OUTPUT_DIR)), "reference.png") for f in SCREENSHOTS_DIR.glob("*.png")} + cases = dict(sorted(cases.items())) + + with open(OUTPUT_FILE, "w") as f: + f.write(template.render(cases=cases)) + +def create_screenshots(): + if TEST_OUTPUT_DIR.exists(): + shutil.rmtree(TEST_OUTPUT_DIR) + + SCREENSHOTS_DIR.mkdir(parents=True) + + t = TestUI() + for name, setup in CASES.items(): + t.test_ui(name, setup) + +if __name__ == "__main__": + print("creating test screenshots") + create_screenshots() + + print("creating html report") + create_html_report() diff --git a/selfdrive/ui/tests/test_ui/template.html b/selfdrive/ui/tests/test_ui/template.html new file mode 100644 index 0000000000..68df5879e6 --- /dev/null +++ b/selfdrive/ui/tests/test_ui/template.html @@ -0,0 +1,34 @@ + + + + +{% for name, (image, ref_image) in cases.items() %} + +

    {{name}}

    +
    +
    + +
    +
    + +
    + +{% endfor %} + \ No newline at end of file diff --git a/selfdrive/ui/tests/ui_snapshot.cc b/selfdrive/ui/tests/ui_snapshot.cc new file mode 100644 index 0000000000..14e0fab835 --- /dev/null +++ b/selfdrive/ui/tests/ui_snapshot.cc @@ -0,0 +1,66 @@ +#include "selfdrive/ui/tests/ui_snapshot.h" + +#include +#include +#include +#include +#include + +#include "selfdrive/ui/qt/home.h" +#include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/qt/window.h" +#include "selfdrive/ui/ui.h" + +void saveWidgetAsImage(QWidget *widget, const QString &fileName) { + QImage image(widget->size(), QImage::Format_ARGB32); + QPainter painter(&image); + widget->render(&painter); + image.save(fileName); +} + +int main(int argc, char *argv[]) { + initApp(argc, argv); + + QApplication app(argc, argv); + + QCommandLineParser parser; + parser.setApplicationDescription("Take a snapshot of the UI."); + parser.addHelpOption(); + parser.addOption(QCommandLineOption(QStringList() << "o" + << "output", + "Output image file path. The file's suffix is used to " + "determine the format. Supports PNG and JPEG formats. " + "Defaults to \"snapshot.png\".", + "file", "snapshot.png")); + parser.process(app); + + const QString output = parser.value("output"); + if (output.isEmpty()) { + qCritical() << "No output file specified"; + return 1; + } + + auto current = QDir::current(); + + // change working directory to find assets + if (!QDir::setCurrent(QCoreApplication::applicationDirPath() + QDir::separator() + "..")) { + qCritical() << "Failed to set current directory"; + return 1; + } + + MainWindow w; + w.setFixedSize(2160, 1080); + w.show(); + app.installEventFilter(&w); + + // restore working directory + QDir::setCurrent(current.absolutePath()); + + // wait for the UI to update + QObject::connect(uiState(), &UIState::uiUpdate, [&](const UIState &s) { + saveWidgetAsImage(&w, output); + app.quit(); + }); + + return app.exec(); +} diff --git a/selfdrive/ui/tests/ui_snapshot.h b/selfdrive/ui/tests/ui_snapshot.h new file mode 100644 index 0000000000..b4699f6af5 --- /dev/null +++ b/selfdrive/ui/tests/ui_snapshot.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +void saveWidgetAsImage(QWidget *widget, const QString &fileName); diff --git a/selfdrive/ui/translations/README.md b/selfdrive/ui/translations/README.md new file mode 100644 index 0000000000..4f11fe3388 --- /dev/null +++ b/selfdrive/ui/translations/README.md @@ -0,0 +1,71 @@ +# Multilanguage + +[![languages](https://raw.githubusercontent.com/commaai/openpilot/badges/translation_badge.svg)](#) + +## Contributing + +Before getting started, make sure you have set up the openpilot Ubuntu development environment by reading the [tools README.md](/tools/README.md). + +### Policy + +Most of the languages supported by openpilot come from and are maintained by the community via pull requests. A pull request likely to be merged is one that [fixes a translation or adds missing translations.](https://github.com/commaai/openpilot/blob/master/selfdrive/ui/translations/README.md#improving-an-existing-language) + +We also generally merge pull requests adding support for a new language if there are community members willing to maintain it. Maintaining a language is ensuring quality and completion of translations before each openpilot release. + +comma may remove or hide language support from releases depending on translation quality and completeness. + +### Adding a New Language + +openpilot provides a few tools to help contributors manage their translations and to ensure quality. To get started: + +1. Add your new language to [languages.json](/selfdrive/ui/translations/languages.json) with the appropriate [language code](https://en.wikipedia.org/wiki/List_of_ISO_639-1_codes) and the localized language name (Traditional Chinese is `中文(繁體)`). +2. Generate the XML translation file (`*.ts`): + ```shell + selfdrive/ui/update_translations.py + ``` +3. Edit the translation file, marking each translation as completed: + ```shell + linguist selfdrive/ui/translations/your_language_file.ts + ``` +4. View your finished translations by compiling and starting the UI, then find it in the language selector: + ```shell + scons -j$(nproc) selfdrive/ui && selfdrive/ui/ui + ``` +5. Read [Checking the UI](#checking-the-ui) to double-check your translations fit in the UI. + +### Improving an Existing Language + +Follow step 3. above, you can review existing translations and add missing ones. Once you're done, just open a pull request to openpilot. + +### Checking the UI +Different languages use varying space to convey the same message, so it's a good idea to double-check that your translations do not overlap and fit into each widget. Start the UI (step 4. above) and view each page, making adjustments to translations as needed. + +#### To view offroad alerts: + +With the UI started, you can view the offroad alerts with: +```shell +selfdrive/ui/tests/cycle_offroad_alerts.py +``` + +### Updating the UI + +Any time you edit source code in the UI, you need to update the translations to ensure the line numbers and contexts are up to date (first step above). + +### Testing + +openpilot has a few unit tests to make sure all translations are up-to-date and that all strings are wrapped in a translation marker. They are run in CI, but you can also run them locally. + +Tests translation files up to date: + +```shell +selfdrive/ui/tests/test_translations.py +``` + +Tests all static source strings are wrapped: + +```shell +selfdrive/ui/tests/create_test_translations.sh && selfdrive/ui/tests/test_translations +``` + +--- +![multilanguage_onroad](https://user-images.githubusercontent.com/25857203/178912800-2c798af8-78e3-498e-9e19-35906e0bafff.png) diff --git a/selfdrive/ui/translations/auto_translate.py b/selfdrive/ui/translations/auto_translate.py new file mode 100755 index 0000000000..c2e4bbc552 --- /dev/null +++ b/selfdrive/ui/translations/auto_translate.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 + +import argparse +import json +import os +import pathlib +import xml.etree.ElementTree as ET +from typing import cast + +import requests + +TRANSLATIONS_DIR = pathlib.Path(__file__).resolve().parent +TRANSLATIONS_LANGUAGES = TRANSLATIONS_DIR / "languages.json" + +OPENAI_MODEL = "gpt-4" +OPENAI_API_KEY = os.environ.get("OPENAI_API_KEY") +OPENAI_PROMPT = "You are a professional translator from English to {language} (ISO 639 language code). " + \ + "The following sentence or word is in the GUI of a software called openpilot, translate it accordingly." + + +def get_language_files(languages: list[str] = None) -> dict[str, pathlib.Path]: + files = {} + + with open(TRANSLATIONS_LANGUAGES) as fp: + language_dict = json.load(fp) + + for filename in language_dict.values(): + path = TRANSLATIONS_DIR / f"{filename}.ts" + language = path.stem.split("main_")[1] + + if languages is None or language in languages: + files[language] = path + + return files + + +def translate_phrase(text: str, language: str) -> str: + response = requests.post( + "https://api.openai.com/v1/chat/completions", + json={ + "model": OPENAI_MODEL, + "messages": [ + { + "role": "system", + "content": OPENAI_PROMPT.format(language=language), + }, + { + "role": "user", + "content": text, + }, + ], + "temperature": 0.8, + "max_tokens": 1024, + "top_p": 1, + }, + headers={ + "Authorization": f"Bearer {OPENAI_API_KEY}", + "Content-Type": "application/json", + }, + ) + + if 400 <= response.status_code < 600: + raise requests.HTTPError(f'Error {response.status_code}: {response.json()}', response=response) + + data = response.json() + + return cast(str, data["choices"][0]["message"]["content"]) + + +def translate_file(path: pathlib.Path, language: str, all_: bool) -> None: + tree = ET.parse(path) + + root = tree.getroot() + + for context in root.findall("./context"): + name = context.find("name") + if name is None: + raise ValueError("name not found") + + print(f"Context: {name.text}") + + for message in context.findall("./message"): + source = message.find("source") + translation = message.find("translation") + + if source is None or translation is None: + raise ValueError("source or translation not found") + + if not all_ and translation.attrib.get("type") != "unfinished": + continue + + llm_translation = translate_phrase(cast(str, source.text), language) + + print(f"Source: {source.text}\n" + + f"Current translation: {translation.text}\n" + + f"LLM translation: {llm_translation}") + + translation.text = llm_translation + + with path.open("w", encoding="utf-8") as fp: + fp.write('\n' + + '\n' + + ET.tostring(root, encoding="utf-8").decode()) + + +def main(): + arg_parser = argparse.ArgumentParser("Auto translate") + + group = arg_parser.add_mutually_exclusive_group(required=True) + group.add_argument("-a", "--all-files", action="store_true", help="Translate all files") + group.add_argument("-f", "--file", nargs="+", help="Translate the selected files. (Example: -f fr de)") + + arg_parser.add_argument("-t", "--all-translations", action="store_true", default=False, help="Translate all sections. (Default: only unfinished)") + + args = arg_parser.parse_args() + + if OPENAI_API_KEY is None: + print("OpenAI API key is missing. (Hint: use `export OPENAI_API_KEY=YOUR-KEY` before you run the script).\n" + + "If you don't have one go to: https://beta.openai.com/account/api-keys.") + exit(1) + + files = get_language_files(None if args.all_files else args.file) + + if args.file: + missing_files = set(args.file) - set(files) + if len(missing_files): + print(f"No language files found: {missing_files}") + exit(1) + + print(f"Translation mode: {'all' if args.all_translations else 'only unfinished'}. Files: {list(files)}") + + for lang, path in files.items(): + print(f"Translate {lang} ({path})") + translate_file(path, lang, args.all_translations) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/ui/translations/create_badges.py b/selfdrive/ui/translations/create_badges.py new file mode 100755 index 0000000000..f56f894111 --- /dev/null +++ b/selfdrive/ui/translations/create_badges.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 +import json +import os +import requests +import xml.etree.ElementTree as ET + +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.ui.tests.test_translations import UNFINISHED_TRANSLATION_TAG +from openpilot.selfdrive.ui.update_translations import LANGUAGES_FILE, TRANSLATIONS_DIR + +TRANSLATION_TAG = " 90 else "red" + + # Download badge + badge_label = f"LANGUAGE {name}" + badge_message = f"{percent_finished}% complete" + if unfinished_translations != 0: + badge_message += f" ({unfinished_translations} unfinished)" + + r = requests.get(f"{SHIELDS_URL}/{badge_label}-{badge_message}-{color}", timeout=10) + assert r.status_code == 200, "Error downloading badge" + content_svg = r.content.decode("utf-8") + + xml = ET.fromstring(content_svg) + assert "width" in xml.attrib + max_badge_width = max(max_badge_width, int(xml.attrib["width"])) + + # Make tag ids in each badge unique to combine them into one svg + for tag in ("r", "s"): + content_svg = content_svg.replace(f'id="{tag}"', f'id="{tag}{idx}"') + content_svg = content_svg.replace(f'"url(#{tag})"', f'"url(#{tag}{idx})"') + + badge_svg.extend([f'', content_svg, ""]) + + badge_svg.insert(0, '') + badge_svg.append("") + + with open(os.path.join(BASEDIR, "translation_badge.svg"), "w") as badge_f: + badge_f.write("\n".join(badge_svg)) diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index eb6a580c01..d8146723a4 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -293,6 +293,18 @@ Review مراجعة + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + اقرن جهازك بجهاز (connect.comma.ai) واحصل على عرضك من comma prime. + + + Pair Device + إقران الجهاز + + + PAIR + إقران +
    DriverViewWindow @@ -428,10 +440,6 @@ غير قادر على تحميل التحديثات %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - إعدادات التاريخ والتوقيت غير صحيحة، لن يبدأ النظام. اتصل بالإنترنت من أجل ضبط الوقت. - Taking camera snapshots. System won't start until finished. التقاط لقطات كاميرا. لن يبدأ النظام حتى تنتهي هذه العملية. @@ -480,6 +488,29 @@ تنبيه + + OnroadAlerts + + openpilot Unavailable + openpilot غير متوفر + + + Waiting for controls to start + في انتظار بدء عناصر التحكم + + + TAKE CONTROL IMMEDIATELY + تحكم على الفور + + + Controls Unresponsive + الضوابط غير مستجيبة + + + Reboot Device + إعادة التشغيل + + PairingPopup @@ -615,6 +646,10 @@ ft قدم + + now + الآن + Reset @@ -654,7 +689,7 @@ This may take up to a minute. System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - + تم تفعيل إعادة ضبط النظام. اضغط على تأكيد لمسح جميع المحتويات والإعدادات. اضغط على إلغاء لاستئناف التمهيد. @@ -762,6 +797,18 @@ This may take up to a minute. Select a language اختر لغة + + Choose Software to Install + اختر البرنامج للتثبيت + + + openpilot + openpilot + + + Custom Software + البرمجيات المخصصة + SetupWidget @@ -1005,7 +1052,7 @@ This may take up to a minute. TogglesPanel Enable openpilot - تمكين + تمكين openpilot Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off. @@ -1091,10 +1138,6 @@ This may take up to a minute. Driving Personality شخصية القيادة - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - يوصى بالوضع القياسي. في الوضع الهجومي، سيتبع openpilot السيارات الرائدة بشكل أقرب، ويصبح أكثر هجومية في دواسات الوقود والمكابح. في وضعية الراحة يبقى openplot بعيداً لمسافة جيدة عن السيارة الرائدة. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: يتم وضع openpilot بشكل قياسي في <b>وضعية الراحة</b>. يمكن الوضع التجريبي <b>ميزات المستوى ألفا</b> التي لا تكون جاهزة في وضع الراحة: @@ -1107,22 +1150,10 @@ This may take up to a minute. Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected. دع نظام القيادة يتحكم بالوقود والمكابح. سيقوم openpilot بالقيادة كما لو أنه كائن بشري، بما في ذلك التوقف عند الإشارة الحمراء، وإشارات التوقف. وبما أن نمط القيادة يحدد سرعة القيادة، فإن السرعة المضبوطة تشكل الحد الأقصى فقط. هذه خاصية الجودة ألفا، فيجب توقع حدوث الأخطاء. - - Navigate on openpilot - التنقل على openpilot - - - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - عندما يكون هناك وجهة للتنقل، فإن openpilot سيقوم بإدخال معلومات الخريطة في هذا النموذج. وهذا يقدم سياقاً مفيداً ويسمح لـopenpilot بالبقاء يساراً أو يميناً بالشكل المناسب عند المنعطفات/المخارج. يبقى سلوك تغيير المسار مفعلاً عند السائق،. هذه هي خاصية الجودة ألفا، ولذلك يجب توقع الأخطاء لا سيما عند المخارج والمنعطفات هذه الأخطاء قد تشمل العبور غير المقصود لخطوط المسارات، والتأخر في الخروج، والقيادة نحو الحواجز الفاصلة في المناطق المثلثة بين الطريق الرئيسي والمخارج، وغير ذلك من الأخطاء المشابهة. - New Driving Visualization تصور القيادة الديد - - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - تصور القيادة سينتقل إلى الكاميرا واسعة الزاوية المواجهة للطريق في السرعات المنخفضة من أجل إظهار بعض المنعطفات بشكل أفضل. سيتم أيضاً إظهار شعار الوضع التجريبي في الزاوية العلوية اليمنى. عند تحديد وجهة التنقل، واستخدام نظام القيادة لها كوضع مدخل، سيتحول مسار القيادة على الخريطة إلى اللون الأخضر. - Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. الوضع التجريبي غير متوفر حالياً في هذه السيارة نظراً لاستخدام رصيد التحكم التكيفي بالسرعة من أجل التحكم الطولي. @@ -1139,6 +1170,22 @@ This may take up to a minute. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. تمكين التحكم الطولي من openpilot (ألفا) للسماح بالوضع التجريبي. + + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + يوصى بالمعيار. في الوضع العدواني، سيتبع الطيار المفتوح السيارات الرائدة بشكل أقرب ويكون أكثر عدوانية مع البنزين والفرامل. في الوضع المريح، سيبقى openpilot بعيدًا عن السيارات الرائدة. في السيارات المدعومة، يمكنك التنقل بين هذه الشخصيات باستخدام زر مسافة عجلة القيادة. + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Always-On Driver Monitoring + + + + Enable driver monitoring even when openpilot is not engaged. + + Updater diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 6d814b9625..010aa4d304 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -293,6 +293,18 @@ Review Überprüfen + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Koppele dein Gerät mit Comma Connect (connect.comma.ai) und sichere dir dein Comma Prime Angebot. + + + Pair Device + + + + PAIR + + DriverViewWindow @@ -419,10 +431,6 @@ %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - - Taking camera snapshots. System won't start until finished. @@ -475,6 +483,29 @@ HINWEIS + + OnroadAlerts + + openpilot Unavailable + + + + Waiting for controls to start + + + + TAKE CONTROL IMMEDIATELY + + + + Controls Unresponsive + + + + Reboot Device + + + PairingPopup @@ -598,6 +629,10 @@ ft fuß + + now + + Reset @@ -744,6 +779,18 @@ This may take up to a minute. Select a language Sprache wählen + + Choose Software to Install + + + + openpilot + openpilot + + + Custom Software + + SetupWidget @@ -1094,35 +1141,35 @@ This may take up to a minute. - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. + End-to-End Longitudinal Control - End-to-End Longitudinal Control + openpilot longitudinal control may come in a future update. - Navigate on openpilot + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. - openpilot longitudinal control may come in a future update. + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Always-On Driver Monitoring - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Enable driver monitoring even when openpilot is not engaged. diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index cd96c466e9..dde6adadd3 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -293,6 +293,18 @@ Disengage to Power Off Désengager pour éteindre + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Associez votre appareil avec comma connect (connect.comma.ai) et profitez de l'offre comma prime. + + + Pair Device + + + + PAIR + + DriverViewWindow @@ -424,10 +436,6 @@ Impossible de télécharger les mises à jour %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - Paramètres de date et d'heure invalides, le système ne démarrera pas. Connectez l'appareil à Internet pour régler l'heure. - Taking camera snapshots. System won't start until finished. Capture de clichés photo. Le système ne démarrera pas tant qu'il n'est pas terminé. @@ -476,6 +484,29 @@ ALERTE + + OnroadAlerts + + openpilot Unavailable + + + + Waiting for controls to start + + + + TAKE CONTROL IMMEDIATELY + + + + Controls Unresponsive + + + + Reboot Device + + + PairingPopup @@ -599,6 +630,10 @@ ft ft + + now + + Reset @@ -746,6 +781,18 @@ Cela peut prendre jusqu'à une minute. Select a language Choisir une langue + + Choose Software to Install + + + + openpilot + openpilot + + + Custom Software + + SetupWidget @@ -1075,10 +1122,6 @@ Cela peut prendre jusqu'à une minute. Driving Personality Personnalité de conduite - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - Le mode standard est recommandé. En mode agressif, openpilot suivra de plus près les voitures de tête et sera plus agressif avec l'accélérateur et le frein. En mode détendu, openpilot restera plus éloigné des voitures de tête. - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: Par défaut, openpilot conduit en <b>mode détente</b>. Le mode expérimental permet d'activer des <b>fonctionnalités alpha</b> qui ne sont pas prêtes pour le mode détente. Les fonctionnalités expérimentales sont listées ci-dessous : @@ -1108,20 +1151,24 @@ Cela peut prendre jusqu'à une minute. Contrôle longitudinal de bout en bout - Navigate on openpilot - Navigation avec openpilot + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Activer le contrôle longitudinal d'openpilot (en alpha) pour autoriser le mode expérimental. - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - Lorsque la navigation dispose d'une destination, openpilot entrera les informations de la carte dans le modèle. Cela fournit un contexte utile pour le modèle et permet à openpilot de se diriger à gauche ou à droite de manière appropriée aux bifurcations/sorties. Le comportement relatif au changement de voie reste inchangé et doit toujours être activé par le conducteur. Il s'agit d'une fonctionnalité alpha ; il faut s'attendre à des erreurs, en particulier aux abords des sorties et des bifurcations. Ces erreurs peuvent inclure des franchissements involontaires de passages piétons, des prises de sortie tardives, la conduite vers des zones de séparation de type zebras, etc. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - La visualisation de la conduite passera sur la caméra grand angle dirigée vers la route à faible vitesse afin de mieux montrer certains virages. Le logo du mode expérimental s'affichera également dans le coin supérieur droit. Lorsqu'une destination de navigation est définie et que le modèle de conduite l'utilise comme entrée, la trajectoire de conduite sur la carte deviendra verte. + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + La visualisation de la conduite passera sur la caméra grand angle dirigée vers la route à faible vitesse afin de mieux montrer certains virages. Le logo du mode expérimental s'affichera également dans le coin supérieur droit. - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - Activer le contrôle longitudinal d'openpilot (en alpha) pour autoriser le mode expérimental. + Always-On Driver Monitoring + + + + Enable driver monitoring even when openpilot is not engaged. + diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index d07c7d525a..e0fb60620b 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -293,6 +293,18 @@ Review 確認 + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + デバイスを comma connect (connect.comma.ai)でペアリングし、comma primeの特典を申請してください。 + + + Pair Device + + + + PAIR + + DriverViewWindow @@ -418,10 +430,6 @@ %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - - Taking camera snapshots. System won't start until finished. @@ -474,6 +482,29 @@ 警告 + + OnroadAlerts + + openpilot Unavailable + + + + Waiting for controls to start + + + + TAKE CONTROL IMMEDIATELY + + + + Controls Unresponsive + + + + Reboot Device + + + PairingPopup @@ -594,6 +625,10 @@ ft フィート + + now + + Reset @@ -740,6 +775,18 @@ This may take up to a minute. Select a language 言語を選択 + + Choose Software to Install + + + + openpilot + openpilot + + + Custom Software + + SetupWidget @@ -1086,35 +1133,35 @@ This may take up to a minute. - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. + End-to-End Longitudinal Control - End-to-End Longitudinal Control + openpilot longitudinal control may come in a future update. - Navigate on openpilot + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. - openpilot longitudinal control may come in a future update. + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Always-On Driver Monitoring - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Enable driver monitoring even when openpilot is not engaged. diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index a903978329..56fc5014ee 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -293,6 +293,18 @@ Review 다시보기 + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 장치를 comma connect (connect.comma.ai)에서 페어링하고 comma prime 무료 이용권을 사용하세요. + + + Pair Device + 장치 동기화 + + + PAIR + 동기화 + DriverViewWindow @@ -419,10 +431,6 @@ 업데이트를 다운로드할 수 없습니다 %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - 날짜 및 시간 설정이 잘못되어 시스템이 시작되지 않습니다. 날짜와 시간을 동기화하려면 인터넷에 연결하세요. - Taking camera snapshots. System won't start until finished. 카메라 스냅샷 찍기가 완료될 때까지 시스템이 시작되지 않습니다. @@ -475,11 +483,34 @@ 알림 + + OnroadAlerts + + openpilot Unavailable + 오픈파일럿을 사용할수없습니다 + + + Waiting for controls to start + 프로세스가 준비중입니다 + + + TAKE CONTROL IMMEDIATELY + 핸들을 잡아주세요 + + + Controls Unresponsive + 프로세스가 응답하지않습니다 + + + Reboot Device + 장치를 재부팅하세요 + + PairingPopup Pair your device to your comma account - 장치를 comma 계정에 페어링합니다 + 장치를 comma 계정에 동기화합니다 Go to https://connect.comma.ai on your phone @@ -595,6 +626,10 @@ ft ft + + now + now + Reset @@ -634,7 +669,7 @@ This may take up to a minute. System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - + 시스템 재설정이 시작되었습니다. 모든 콘텐츠와 설정을 지우려면 확인을 누르시고 부팅을 재개하려면 취소를 누르세요. @@ -742,6 +777,18 @@ This may take up to a minute. Select a language 언어를 선택하세요 + + Choose Software to Install + 설치할 소프트웨어 선택 + + + openpilot + openpilot + + + Custom Software + 커스텀 소프트웨어 + SetupWidget @@ -762,7 +809,7 @@ This may take up to a minute. Sidebar CONNECT - 연결됨 + 커넥트 OFFLINE @@ -1091,18 +1138,10 @@ This may take up to a minute. Driving Personality 주행 모드 - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - 표준 모드를 권장합니다. 공격적 모드에서 openpilot은 앞 차량을 더 가까이 따라가며 적극적으로 가감속합니다. 편안한 모드에서 openpilot은 앞 차량을 더 멀리서 따라갑니다. - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. openpilot 가감속 제어 알파 버전은 비 릴리즈 브랜치에서 실험 모드와 함께 테스트할 수 있습니다. - - Navigate on openpilot - openpilot 내비게이트 - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. 실험 모드를 사용하려면 openpilot E2E 가감속 제어 (알파) 토글을 활성화하세요. @@ -1112,12 +1151,20 @@ This may take up to a minute. E2E 가감속 제어 - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - 내비게이션에 목적지가 설정되어 있으면 openpilot이 지도 정보를 주행 모델에 입력합니다. 이는 모델에 유용한 정보를 제공하고 openpilot이 진출입로 및 램프에서 적절하게 왼쪽 또는 오른쪽을 유지할 수 있도록 해 줍니다. 차선 변경 기능은 여전히 운전자의 조작에 의해 활성화됩니다. 이 기능은 알파 버전입니다. 특히 진출입로 및 분기점 주변에서 실수가 발생할 수 있으며 이러한 실수에는 의도하지 않은 차선 이탈, 늦은 진출, 도로 가장자리의 분리대 또는 경계석을 향해 운전하는 행동 등이 포함됩니다. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + 표준 모드를 권장합니다. 공격적 모드의 openpilot은 선두 차량을 더 가까이 따라가고 가감속제어를 사용하여 더욱 공격적으로 움직입니다. 편안한 모드의 openpilot은 선두 차량으로부터 더 멀리 떨어져 있습니다. 지원되는 차량에서는 스티어링 휠 거리 버튼을 사용하여 이러한 특성을 순환할 수 있습니다. - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - 주행 시각화는 저속으로 주행 시 도로를 향한 광각 카메라로 자동 전환되어 일부 곡선 경로를 더 잘 보여줍니다. 실험 모드 로고는 우측 상단에 표시됩니다. 내비게이션 목적지가 설정되고 주행 모델에 입력되면 지도의 주행 경로가 녹색으로 바뀝니다. + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + + + + Always-On Driver Monitoring + + + + Enable driver monitoring even when openpilot is not engaged. + diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index fce5a8a8ff..feaa6e86a1 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -293,6 +293,18 @@ Review Revisar + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Pareie seu dispositivo com comma connect (connect.comma.ai) e reivindique sua oferta de comma prime. + + + Pair Device + Parear Dispositivo + + + PAIR + PAREAR + DriverViewWindow @@ -420,10 +432,6 @@ Não é possível baixar atualizações %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - Configurações de data e hora inválidas, o sistema não será iniciado. Conecte-se à internet para definir o horário. - Taking camera snapshots. System won't start until finished. Tirando fotos da câmera. O sistema não será iniciado até terminar. @@ -476,6 +484,29 @@ ALERTA + + OnroadAlerts + + openpilot Unavailable + openpilot Indisponível + + + Waiting for controls to start + Aguardando controles para iniciar + + + TAKE CONTROL IMMEDIATELY + ASSUMA IMEDIATAMENTE + + + Controls Unresponsive + Controles Não Respondem + + + Reboot Device + Reinicie o Dispositivo + + PairingPopup @@ -599,6 +630,10 @@ ft pés + + now + agora + Reset @@ -638,7 +673,7 @@ Isso pode levar até um minuto. System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - Reinicialização do sistema acionada. Pressione confirmar para apagar todo o conteúdo e configurações. Pressione cancel para retomar a inicialização. + Reinicialização do sistema acionada. Pressione confirmar para apagar todo o conteúdo e configurações. Pressione cancel para retomar a inicialização. @@ -746,6 +781,18 @@ Isso pode levar até um minuto. Select a language Selecione o Idioma + + Choose Software to Install + Escolha o Software a ser Instalado + + + openpilot + openpilot + + + Custom Software + Software Customizado + SetupWidget @@ -1095,18 +1142,10 @@ Isso pode levar até um minuto. Driving Personality Temperamento de Direção - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - Neutro é o recomendado. No modo disputa o openpilot seguirá o carro da frente mais de perto e será mais agressivo com a aceleração e frenagem. No modo calmo o openpilot se manterá mais longe do carro da frente. - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. Uma versão embrionária do controle longitudinal openpilot pode ser testada em conjunto com o modo Experimental, em branches que não sejam de produção. - - Navigate on openpilot - Navegação no openpilot - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. Habilite o controle longitudinal (embrionário) openpilot para permitir o modo Experimental. @@ -1116,12 +1155,20 @@ Isso pode levar até um minuto. Controle Longitudinal de Ponta a Ponta - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - Quando a navegação tem um destino, o openpilot insere as informações do mapa no modelo. Isso fornece contexto útil para o modelo e permite que o openpilot mantenha a esquerda ou a direita apropriadamente em bifurcações/saídas. O comportamento de mudança de faixa permanece inalterado e ainda é ativado somente pelo motorista. Este é um recurso de qualidade embrionária; erros devem ser esperados, principalmente em torno de saídas e bifurcações. Esses erros podem incluir travessias não intencionais na faixa de rodagem, saída tardia, condução em direção a barreiras divisórias nas áreas de marcas de canalização, etc. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + Neutro é o recomendado. No modo disputa o openpilot seguirá o carro da frente mais de perto e será mais agressivo com a aceleração e frenagem. No modo calmo o openpilot se manterá mais longe do carro da frente. Em carros compatíveis, você pode alternar esses temperamentos com o botão de distância do volante. + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + A visualização de condução fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será mostrado no canto superior direito. + + + Always-On Driver Monitoring + Monitoramento do Motorista Sempre Ativo - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - A visualização de condução fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será mostrado no canto superior direito. Quando um destino de navegação é definido e o modelo de condução o utiliza como entrada o caminho de condução no mapa fica verde. + Enable driver monitoring even when openpilot is not engaged. + Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado. diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index f4d097b2a2..e594a6975f 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -68,23 +68,23 @@ Hidden Network - + เครือข่ายที่ซ่อนอยู่ CONNECT - เชื่อมต่อ + เชื่อมต่อ Enter SSID - ป้อนค่า SSID + ป้อนค่า SSID Enter password - ใส่รหัสผ่าน + ใส่รหัสผ่าน for "%1" - สำหรับ "%1" + สำหรับ "%1" @@ -293,6 +293,18 @@ Review ทบทวน + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + จับคู่อุปกรณ์ของคุณกับ comma connect (connect.comma.ai) และรับข้อเสนอ comma prime ของคุณ + + + Pair Device + จับคู่อุปกรณ์ + + + PAIR + จับคู่ + DriverViewWindow @@ -423,10 +435,6 @@ ไม่สามารถดาวน์โหลดอัพเดทได้ %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - วันที่และเวลาไม่ถูกต้อง ระบบจะไม่เริ่มทำงาน เชื่อต่ออินเตอร์เน็ตเพื่อตั้งเวลา - Taking camera snapshots. System won't start until finished. กล้องกำลังถ่ายภาพ ระบบจะไม่เริ่มทำงานจนกว่าจะเสร็จ @@ -475,6 +483,29 @@ การแจ้งเตือน + + OnroadAlerts + + openpilot Unavailable + openpilot ไม่สามารถใช้งานได้ + + + Waiting for controls to start + กำลังรอให้ controls เริ่มทำงาน + + + TAKE CONTROL IMMEDIATELY + เข้าควบคุมรถเดี๋ยวนี้ + + + Controls Unresponsive + Controls ไม่ตอบสนอง + + + Reboot Device + รีบูตอุปกรณ์ + + PairingPopup @@ -595,6 +626,10 @@ ft ฟุต + + now + ตอนนี้ + Reset @@ -634,7 +669,7 @@ This may take up to a minute. System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - + ระบบถูกรีเซ็ต กดยืนยันเพื่อลบข้อมูลและการตั้งค่าทั้งหมด กดยกเลิกเพื่อบูตต่อ @@ -742,6 +777,18 @@ This may take up to a minute. Select a language เลือกภาษา + + Choose Software to Install + เลือกซอฟต์แวร์ที่จะติดตั้ง + + + openpilot + openpilot + + + Custom Software + ซอฟต์แวร์ที่กำหนดเอง + SetupWidget @@ -1091,10 +1138,6 @@ This may take up to a minute. Driving Personality บุคลิกการขับขี่ - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - แนะนำให้ใช้แบบมาตรฐาน ในโหมดดุดัน openpilot จะตามรถคันหน้าใกล้ขึ้นและเร่งและเบรคแบบดุดันมากขึ้น ในโหมดผ่อนคลาย openpilot จะอยู่ห่างจากรถคันหน้ามากขึ้น - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. ระบบควบคุมการเร่ง/เบรคโดย openpilot เวอร์ชัน alpha สามารถทดสอบได้พร้อมกับโหมดการทดลอง บน branch ที่กำลังพัฒนา @@ -1104,20 +1147,24 @@ This may take up to a minute. ควบคุมเร่ง/เบรคแบบ End-to-End - Navigate on openpilot - การนำทางบน openpilot + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + เปิดระบบควบคุมการเร่ง/เบรคโดย openpilot (alpha) เพื่อเปิดใช้งานโหมดทดลอง + + + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + แนะนำให้ใช้แบบมาตรฐาน ในโหมดดุดัน openpilot จะตามรถคันหน้าใกล้ขึ้นและเร่งและเบรคแบบดุดันมากขึ้น ในโหมดผ่อนคลาย openpilot จะอยู่ห่างจากรถคันหน้ามากขึ้น ในรถรุ่นที่รองรับคุณสามารถเปลี่ยนบุคลิกไปแบบต่าง ๆ โดยใช้ปุ่มปรับระยะห่างบนพวงมาลัย - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - เมื่อการนำทางมีจุดหมายปลายทาง openpilot จะป้อนข้อมูลแผนที่เข้าไปยังโมเดล ซึ่งจะเป็นบริบทที่มีประโยชน์สำหรับโมเดลและจะทำให้ openpilot สามารถรักษาเลนซ้ายหรือขวาได้อย่างเหมาะสมบริเวณทางแยกหรือทางออก พฤติกรรมการเปลี่ยนเลนยังคงเหมือนเดิมและยังคงต้องถูกเริ่มโดยคนขับ ความสามารถนี้ยังอยู่ในระดับ alpha ซึ่งอาจะเกิดความผิดพลาดได้โดยเฉพาะบริเวณทางแยกหรือทางออก ความผิดพลาดที่อาจเกิดขึ้นได้อาจรวมถึงการข้ามเส้นแบ่งเลนโดยไม่ตั้งใจ, การเข้าช่องทางออกช้ากว่าปกติ, การขับเข้าหาแบริเออร์ในเขตปลอดภัย, ฯลฯ + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - การแสดงภาพการขับขี่จะเปลี่ยนไปใช้กล้องมุมกว้างที่หันหน้าไปทางถนนเมื่ออยู่ในความเร็วต่ำ เพื่อแสดงภาพการเลี้ยวที่ดีขึ้น โลโก้โหมดการทดลองจะแสดงที่มุมบนขวาด้วย เมื่อเป้าหมายการนำทางถูกเลือกและโมเดลการขับขี่กำลังใช้เป็นอินพุต เส้นทางการขับขี่บนแผนที่จะเปลี่ยนเป็นสีเขียว + Always-On Driver Monitoring + - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - เปิดระบบควบคุมการเร่ง/เบรคโดย openpilot (alpha) เพื่อเปิดใช้งานโหมดทดลอง + Enable driver monitoring even when openpilot is not engaged. + diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index ec6f71f5b0..48615f1699 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -293,6 +293,18 @@ Review + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + Cihazınızı comma connect (connect.comma.ai) ile eşleştirin ve comma prime aboneliğine göz atın. + + + Pair Device + + + + PAIR + + DriverViewWindow @@ -422,10 +434,6 @@ %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - - Taking camera snapshots. System won't start until finished. @@ -474,6 +482,29 @@ UYARI + + OnroadAlerts + + openpilot Unavailable + + + + Waiting for controls to start + + + + TAKE CONTROL IMMEDIATELY + + + + Controls Unresponsive + + + + Reboot Device + + + PairingPopup @@ -594,6 +625,10 @@ ft ft + + now + + Reset @@ -740,6 +775,18 @@ This may take up to a minute. Select a language Dil seçin + + Choose Software to Install + + + + openpilot + openpilot + + + Custom Software + + SetupWidget @@ -1069,10 +1116,6 @@ This may take up to a minute. On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - - openpilot defaults to driving in <b>chill mode</b>. Experimental mode enables <b>alpha-level features</b> that aren't ready for chill mode. Experimental features are listed below: @@ -1086,35 +1129,39 @@ This may take up to a minute. - Navigate on openpilot + New Driving Visualization - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. - New Driving Visualization + openpilot longitudinal control may come in a future update. - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. - openpilot longitudinal control may come in a future update. + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Always-On Driver Monitoring + + + + Enable driver monitoring even when openpilot is not engaged. diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 91f55c6e94..32119ee10f 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -68,23 +68,23 @@ Hidden Network - + 隐藏的网络 CONNECT - CONNECT + 连线 Enter SSID - 输入SSID + 输入 SSID Enter password - 输入密码 + 输入密码 for "%1" - 网络名称:"%1" + 网络名称:"%1" @@ -293,6 +293,18 @@ Review 预览 + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 将您的设备与comma connect (connect.comma.ai)配对并领取您的comma prime优惠。 + + + Pair Device + 配对设备 + + + PAIR + 配对 + DriverViewWindow @@ -419,10 +431,6 @@ 无法下载更新 %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - 日期和时间设置无效,系统无法启动。请连接至互联网以设置时间。 - Taking camera snapshots. System won't start until finished. 正在使用相机拍摄中。在完成之前,系统将无法启动。 @@ -475,6 +483,29 @@ 警报 + + OnroadAlerts + + openpilot Unavailable + 无法使用 openpilot + + + Waiting for controls to start + 等待控制服务啟動 + + + TAKE CONTROL IMMEDIATELY + 立即接管 + + + Controls Unresponsive + 控制服务无响应 + + + Reboot Device + 重启设备 + + PairingPopup @@ -595,6 +626,10 @@ ft ft + + now + 现在 + Reset @@ -634,7 +669,7 @@ This may take up to a minute. System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - + 系统重置已触发。按下“确认”以清除所有内容和设置,按下“取消”以继续启动。 @@ -742,6 +777,18 @@ This may take up to a minute. Select a language 选择语言 + + Choose Software to Install + 选择要安装的软件 + + + openpilot + openpilot + + + Custom Software + 定制软件 + SetupWidget @@ -1091,18 +1138,10 @@ This may take up to a minute. Driving Personality 驾驶风格 - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - 推荐使用标准模式。在积极模式中,openpilot 会更靠近前车并在加速和刹车方面更积极。在舒适模式中,openpilot 会与前车保持较远的距离。 - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. 在正式(release)版本以外的分支上,可以测试 openpilot 纵向控制的 Alpha 版本以及实验模式。 - - Navigate on openpilot - Navigate on openpilot - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. 启用 openpilot 纵向控制(alpha)开关以允许实验模式。 @@ -1112,12 +1151,20 @@ This may take up to a minute. 端到端纵向控制 - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - 当导航有目的地时,openpilot 将输入地图信息到模型中。这为模型提供了有用的背景信息,使 openpilot 能够在叉路/出口时适当地保持左侧或右侧行驶。车道变换行为保持不变,仍由驾驶员激活。这是一个 Alpha 版的功能;可能会出现错误,特别是在出口和分叉处。这些错误可能包括意外的车道越界、晚出口、朝着分隔栏驶向安全地带等。 + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + 推荐使用标准模式。在积极模式下,openpilot 会更靠近前方车辆,并在油门和刹车方面更加激进。在放松模式下,openpilot 会与前方车辆保持更远距离。在支持的车型上,你可以使用方向盘上的距离按钮来循环切换这些驾驶风格。 + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + 在低速时,驾驶可视化将转换为道路朝向的广角摄像头,以更好地展示某些转弯。测试模式标志也将显示在右上角。 + + + Always-On Driver Monitoring + 驾驶员监控常开 - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - 行驶画面将在低速时切换到道路朝向的广角摄像头,以更好地显示一些转弯。实验模式标志也将显示在右上角。当设置了导航目的地并且驾驶模型正在使用它作为输入时,地图上的驾驶路径将变为绿色。 + Enable driver monitoring even when openpilot is not engaged. + 即使在openpilot未激活时也启用驾驶员监控。 diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index e8c8854d0e..9d1c16db9f 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -68,23 +68,23 @@ Hidden Network - + 隱藏的網路 CONNECT - 雲端服務 + 連線 Enter SSID - 輸入 SSID + 輸入 SSID Enter password - 輸入密碼 + 輸入密碼 for "%1" - 給 "%1" + 給 "%1" @@ -247,11 +247,11 @@ openpilot requires the device to be mounted within 4° left or right and within 5° up or 9° down. openpilot is continuously calibrating, resetting is rarely required. - openpilot 需要將設備固定在左右偏差 4° 以內,朝上偏差 5° 以內或朝下偏差 9° 以內。鏡頭在後台會持續自動校準,很少有需要重設的情況。 + openpilot 需要將裝置固定在左右偏差 4° 以內,朝上偏差 5° 以內或朝下偏差 9° 以內。鏡頭在後台會持續自動校準,很少有需要重置的情況。 Your device is pointed %1° %2 and %3° %4. - 你的設備目前朝%2 %1° 以及朝%4 %3° 。 + 你的裝置目前朝%2 %1° 以及朝%4 %3° 。 down @@ -293,6 +293,18 @@ Review 回顧 + + Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. + 將您的裝置與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 + + + Pair Device + 配對裝置 + + + PAIR + 配對 + DriverViewWindow @@ -419,21 +431,17 @@ 無法下載更新 %1 - - Invalid date and time settings, system won't start. Connect to internet to set time. - 日期和時間設定無效,系統無法啟動。請連接至網際網路以設定時間。 - Taking camera snapshots. System won't start until finished. 正在使用相機拍攝中。在完成之前,系統將無法啟動。 An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. - 一個給您設備的操作系統的更新正在後台下載中。當更新準備好安裝時,您將收到提示進行更新。 + 一個有關操作系統的更新正在後台下載中。當更新準備好安裝時,您將收到提示進行更新。 Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. - 設備註冊失敗。它將無法連接或上傳至 comma.ai 伺服器,並且無法獲得 comma.ai 的支援。如果這是一個官方設備,請訪問 https://comma.ai/support 。 + 裝置註冊失敗。它將無法連接或上傳至 comma.ai 伺服器,並且無法獲得 comma.ai 的支援。如果這是一個官方裝置,請訪問 https://comma.ai/support 。 NVMe drive not mounted. @@ -441,7 +449,7 @@ Unsupported NVMe drive detected. Device may draw significantly more power and overheat due to the unsupported NVMe. - 檢測到不支援的 NVMe 固態硬碟。您的設備因為使用了不支援的 NVMe 固態硬碟可能會消耗更多電力並更易過熱。 + 檢測到不支援的 NVMe 固態硬碟。您的裝置因為使用了不支援的 NVMe 固態硬碟可能會消耗更多電力並更易過熱。 openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. @@ -453,11 +461,11 @@ openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. - openpilot 偵測到設備的安裝位置發生變化。請確保設備完全安裝在支架上,並確保支架牢固地固定在擋風玻璃上。 + openpilot 偵測到裝置的安裝位置發生變化。請確保裝置完全安裝在支架上,並確保支架牢固地固定在擋風玻璃上。 Device temperature too high. System cooling down before starting. Current internal component temperature: %1 - 設備溫度過高。系統正在冷卻中,等冷卻完畢後才會啟動。目前內部組件溫度:%1 + 裝置溫度過高。系統正在冷卻中,等冷卻完畢後才會啟動。目前內部組件溫度:%1 @@ -475,11 +483,34 @@ 提醒 + + OnroadAlerts + + openpilot Unavailable + 無法使用 openpilot + + + Waiting for controls to start + 等待操控服務開始 + + + TAKE CONTROL IMMEDIATELY + 立即接管 + + + Controls Unresponsive + 操控服務沒有反應 + + + Reboot Device + 請重新啟裝置 + + PairingPopup Pair your device to your comma account - 將設備與您的 comma 帳號配對 + 將裝置與您的 comma 帳號配對 Go to https://connect.comma.ai on your phone @@ -595,6 +626,10 @@ ft ft + + now + 現在 + Reset @@ -604,7 +639,7 @@ Are you sure you want to reset your device? - 您確定要重設你的設備嗎? + 您確定要重設你的裝置嗎? System Reset @@ -624,17 +659,17 @@ Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. - 無法掛載資料分割區。分割區可能已經毀損。請確認是否要刪除並重新設定。 + 無法掛載資料分割區。分割區可能已經毀損。請確認是否要刪除並重置。 Resetting device... This may take up to a minute. - 設備重設中… + 重置中… 這可能需要一分鐘的時間。 System reset triggered. Press confirm to erase all content and settings. Press cancel to resume boot. - + 系統重設已啟動。按下「確認」以清除所有內容和設定,或按下「取消」以繼續開機。 @@ -645,7 +680,7 @@ This may take up to a minute. Device - 設備 + 裝置 Network @@ -720,7 +755,7 @@ This may take up to a minute. Ensure the entered URL is valid, and the device’s internet connection is good. - 請確定您輸入的是有效的安裝網址,並且確定設備的網路連線狀態良好。 + 請確定您輸入的是有效的安裝網址,並且確定裝置的網路連線狀態良好。 Reboot device @@ -736,12 +771,24 @@ This may take up to a minute. Something went wrong. Reboot the device. - 發生了一些錯誤。請重新啟動您的設備。 + 發生了一些錯誤。請重新啟動您的裝置。 Select a language 選擇語言 + + Choose Software to Install + 選擇要安裝的軟體 + + + openpilot + openpilot + + + Custom Software + 自訂軟體 + SetupWidget @@ -751,11 +798,11 @@ This may take up to a minute. Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. - 將您的設備與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 + 將您的裝置與 comma connect (connect.comma.ai) 配對並領取您的 comma 高級會員優惠。 Pair device - 配對設備 + 配對裝置 @@ -1091,18 +1138,10 @@ This may take up to a minute. Driving Personality 駕駛風格 - - Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. - 推薦使用標準模式。在積極模式中,openpilot 會更靠近前車並在加速和剎車方面更積極。在舒適模式中,openpilot 會與前車保持較遠的距離。 - An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. 在正式 (release) 版以外的分支上可以測試 openpilot 縱向控制的 Alpha 版本以及實驗模式。 - - Navigate on openpilot - Navigate on openpilot - Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. 啟用 openpilot 縱向控制(alpha)切換以允許實驗模式。 @@ -1112,12 +1151,20 @@ This may take up to a minute. 端到端縱向控制 - When navigation has a destination, openpilot will input the map information into the model. This provides useful context for the model and allows openpilot to keep left or right appropriately at forks/exits. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected, particularly around exits and forks. These mistakes can include unintended laneline crossings, late exit taking, driving towards dividing barriers in the gore areas, etc. - 當導航有目的地時,openpilot 將把地圖資訊輸入模型中。這為模型提供了有用的背景資訊,使 openpilot 能夠在叉路/出口時適當地保持左側或右側行駛。車道變換行為保持不變,仍由駕駛員啟用。這是一個 Alpha 版的功能;可能會出現錯誤,特別是在出口和分叉處。這些錯誤可能包括意外的車道越界、晚出口、朝著分隔欄駛向分隔帶區域等。 + Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with your steering wheel distance button. + 建議使用標準模式。在積極模式下,openpilot 會更接近前車並更積極地使用油門和剎車。在輕鬆模式下,openpilot 會與前車保持較遠距離。對於支援的汽車,您可以使用方向盤上的距離按鈕來切換這些駕駛風格。 + + + The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. + 在低速時,駕駛可視化將切換至道路朝向的廣角攝影機,以更好地顯示某些彎道。在右上角還會顯示「實驗模式」的標誌。 + + + Always-On Driver Monitoring + 駕駛監控常開 - The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. When a navigation destination is set and the driving model is using it as input, the driving path on the map will turn green. - 行駛畫面將在低速時切換至道路朝向的廣角鏡頭,以更好地顯示一些轉彎。實驗模式圖示也將顯示在右上角。當設定了導航目的地並且行駛模型正在將其作為輸入時,地圖上的行駛路徑將變為綠色。 + Enable driver monitoring even when openpilot is not engaged. + 即使在openpilot未激活時也啟用駕駛監控。 @@ -1128,7 +1175,7 @@ This may take up to a minute. An operating system update is required. Connect your device to Wi-Fi for the fastest update experience. The download size is approximately 1GB. - 設備的操作系統需要更新。請將您的設備連接到 Wi-Fi 以獲得最快的更新體驗。下載大小約為 1GB。 + 需要進行作業系統更新。建議將您的裝置連接上 Wi-Fi 獲得更快的更新下載。下載大小約為 1GB。 Connect to Wi-Fi diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 9afd22f13a..c70b7594c9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -57,26 +57,23 @@ void update_leads(UIState *s, const cereal::RadarState::Reader &radar_state, con void update_line_data(const UIState *s, const cereal::XYZTData::Reader &line, float y_off, float z_off, QPolygonF *pvd, int max_idx, bool allow_invert=true) { const auto line_x = line.getX(), line_y = line.getY(), line_z = line.getZ(); - QPolygonF left_points, right_points; - left_points.reserve(max_idx + 1); - right_points.reserve(max_idx + 1); - + QPointF left, right; + pvd->clear(); for (int i = 0; i <= max_idx; i++) { // highly negative x positions are drawn above the frame and cause flickering, clip to zy plane of camera if (line_x[i] < 0) continue; - QPointF left, right; + bool l = calib_frame_to_full_frame(s, line_x[i], line_y[i] - y_off, line_z[i] + z_off, &left); bool r = calib_frame_to_full_frame(s, line_x[i], line_y[i] + y_off, line_z[i] + z_off, &right); if (l && r) { // For wider lines the drawn polygon will "invert" when going over a hill and cause artifacts - if (!allow_invert && left_points.size() && left.y() > left_points.back().y()) { + if (!allow_invert && pvd->size() && left.y() > pvd->back().y()) { continue; } - left_points.push_back(left); - right_points.push_front(right); + pvd->push_back(left); + pvd->push_front(right); } } - *pvd = left_points + right_points; } void update_model(UIState *s, @@ -128,25 +125,27 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers scene.driver_pose_coss[i] = cosf(scene.driver_pose_vals[i]*(1.0-dm_fade_state)); } + auto [sin_y, sin_x, sin_z] = scene.driver_pose_sins; + auto [cos_y, cos_x, cos_z] = scene.driver_pose_coss; + const mat3 r_xyz = (mat3){{ - scene.driver_pose_coss[1]*scene.driver_pose_coss[2], - scene.driver_pose_coss[1]*scene.driver_pose_sins[2], - -scene.driver_pose_sins[1], + cos_x * cos_z, + cos_x * sin_z, + -sin_x, - -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_coss[0]*scene.driver_pose_sins[2], - -scene.driver_pose_sins[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_coss[0]*scene.driver_pose_coss[2], - -scene.driver_pose_sins[0]*scene.driver_pose_coss[1], + -sin_y * sin_x * cos_z - cos_y * sin_z, + -sin_y * sin_x * sin_z + cos_y * cos_z, + -sin_y * cos_x, - scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_coss[2] - scene.driver_pose_sins[0]*scene.driver_pose_sins[2], - scene.driver_pose_coss[0]*scene.driver_pose_sins[1]*scene.driver_pose_sins[2] + scene.driver_pose_sins[0]*scene.driver_pose_coss[2], - scene.driver_pose_coss[0]*scene.driver_pose_coss[1], + cos_y * sin_x * cos_z - sin_y * sin_z, + cos_y * sin_x * sin_z + sin_y * cos_z, + cos_y * cos_x, }}; // transform vertices for (int kpi = 0; kpi < std::size(default_face_kpts_3d); kpi++) { - vec3 kpt_this = default_face_kpts_3d[kpi]; - kpt_this = matvecmul3(r_xyz, kpt_this); - scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; + vec3 kpt_this = matvecmul3(r_xyz, default_face_kpts_3d[kpi]); + scene.face_kpts_draw[kpi] = (vec3){{kpt_this.v[0], kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; } } @@ -205,6 +204,8 @@ static void update_state(UIState *s) { auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState(); float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f; scene.light_sensor = std::max(100.0f - scale * cam_state.getExposureValPercent(), 0.0f); + } else if (!sm.allAliveAndValid({"wideRoadCameraState"})) { + scene.light_sensor = -1; } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; @@ -248,7 +249,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", - "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", + "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "clocks", }); Params params; @@ -320,7 +321,7 @@ void Device::resetInteractiveTimeout(int timeout) { void Device::updateBrightness(const UIState &s) { float clipped_brightness = offroad_brightness; - if (s.scene.started) { + if (s.scene.started && s.scene.light_sensor > 0) { clipped_brightness = s.scene.light_sensor; // CIE 1931 - https://www.photonstophotos.net/GeneralTopics/Exposure/Psychometric_Lightness_and_Gamma.htm diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 86cd70f028..7238159dda 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -1,6 +1,5 @@ #pragma once -#include #include #include @@ -22,7 +21,6 @@ const int UI_HEADER_HEIGHT = 420; const int UI_FREQ = 20; // Hz const int BACKLIGHT_OFFROAD = 50; -typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert; const float MIN_DRAW_DISTANCE = 10.0; const float MAX_DRAW_DISTANCE = 100.0; @@ -47,59 +45,6 @@ constexpr vec3 default_face_kpts_3d[] = { {18.02, -49.14, 8.00}, {6.36, -51.20, 8.00}, {-5.98, -51.20, 8.00}, }; -struct Alert { - QString text1; - QString text2; - QString type; - cereal::ControlsState::AlertSize size; - cereal::ControlsState::AlertStatus status; - AudibleAlert sound; - - bool equal(const Alert &a2) { - return text1 == a2.text1 && text2 == a2.text2 && type == a2.type && sound == a2.sound; - } - - static Alert get(const SubMaster &sm, uint64_t started_frame) { - const cereal::ControlsState::Reader &cs = sm["controlsState"].getControlsState(); - const uint64_t controls_frame = sm.rcv_frame("controlsState"); - - Alert alert = {}; - if (controls_frame >= started_frame) { // Don't get old alert. - alert = {cs.getAlertText1().cStr(), cs.getAlertText2().cStr(), - cs.getAlertType().cStr(), cs.getAlertSize(), - cs.getAlertStatus(), - cs.getAlertSound()}; - } - - if (!sm.updated("controlsState") && (sm.frame - started_frame) > 5 * UI_FREQ) { - const int CONTROLS_TIMEOUT = 5; - const int controls_missing = (nanos_since_boot() - sm.rcv_time("controlsState")) / 1e9; - - // Handle controls timeout - if (controls_frame < started_frame) { - // car is started, but controlsState hasn't been seen at all - alert = {"openpilot Unavailable", "Waiting for controls to start", - "controlsWaiting", cereal::ControlsState::AlertSize::MID, - cereal::ControlsState::AlertStatus::NORMAL, - AudibleAlert::NONE}; - } else if (controls_missing > CONTROLS_TIMEOUT && !Hardware::PC()) { - // car is started, but controls is lagging or died - if (cs.getEnabled() && (controls_missing - CONTROLS_TIMEOUT) < 10) { - alert = {"TAKE CONTROL IMMEDIATELY", "Controls Unresponsive", - "controlsUnresponsive", cereal::ControlsState::AlertSize::FULL, - cereal::ControlsState::AlertStatus::CRITICAL, - AudibleAlert::WARNING_IMMEDIATE}; - } else { - alert = {"Controls Unresponsive", "Reboot Device", - "controlsUnresponsivePermanent", cereal::ControlsState::AlertSize::MID, - cereal::ControlsState::AlertStatus::NORMAL, - AudibleAlert::NONE}; - } - } - } - return alert; - } -}; typedef enum UIStatus { STATUS_DISENGAGED, @@ -108,7 +53,8 @@ typedef enum UIStatus { } UIStatus; enum PrimeType { - UNKNOWN = -1, + UNKNOWN = -2, + UNPAIRED = -1, NONE = 0, MAGENTA = 1, LITE = 2, @@ -123,11 +69,6 @@ const QColor bg_colors [] = { [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), }; -static std::map alert_colors = { - {cereal::ControlsState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, - {cereal::ControlsState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)}, - {cereal::ControlsState::AlertStatus::CRITICAL, QColor(0xC9, 0x22, 0x31, 0xf1)}, -}; typedef struct UIScene { bool calibration_valid = false; @@ -154,9 +95,9 @@ typedef struct UIScene { float driver_pose_coss[3]; vec3 face_kpts_draw[std::size(default_face_kpts_3d)]; - bool navigate_on_openpilot = false; + cereal::LongitudinalPersonality personality; - float light_sensor; + float light_sensor = -1; bool started, ignition, is_metric, map_on_left, longitudinal_control; bool world_objects_visible = false; uint64_t started_frame; @@ -174,7 +115,7 @@ public: void setPrimeType(PrimeType type); inline PrimeType primeType() const { return prime_type; } - inline bool hasPrime() const { return prime_type != PrimeType::UNKNOWN && prime_type != PrimeType::NONE; } + inline bool hasPrime() const { return prime_type > PrimeType::NONE; } int fb_w = 0, fb_h = 0; diff --git a/selfdrive/ui/ui.py b/selfdrive/ui/ui.py new file mode 100755 index 0000000000..660495b1de --- /dev/null +++ b/selfdrive/ui/ui.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python3 +import os +import signal + +signal.signal(signal.SIGINT, signal.SIG_DFL) + +import cereal.messaging as messaging +from openpilot.system.hardware import HARDWARE + +from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtWidgets import QLabel, QWidget, QVBoxLayout, QStackedLayout, QApplication +from openpilot.selfdrive.ui.qt.python_helpers import set_main_window + + +if __name__ == "__main__": + app = QApplication([]) + win = QWidget() + set_main_window(win) + + bg = QLabel("", alignment=Qt.AlignCenter) + + alert1 = QLabel() + alert2 = QLabel() + vlayout = QVBoxLayout() + vlayout.addWidget(alert1, alignment=Qt.AlignCenter) + vlayout.addWidget(alert2, alignment=Qt.AlignCenter) + + tmp = QWidget() + tmp.setLayout(vlayout) + + stack = QStackedLayout(win) + stack.addWidget(tmp) + stack.addWidget(bg) + stack.setStackingMode(QStackedLayout.StackAll) + + win.setObjectName("win") + win.setStyleSheet(""" + #win { + background-color: black; + } + QLabel { + color: white; + font-size: 40px; + } + """) + + sm = messaging.SubMaster(['deviceState', 'controlsState']) + + def update(): + sm.update(0) + + onroad = sm.all_checks(['deviceState']) and sm['deviceState'].started + if onroad: + cs = sm['controlsState'] + color = ("grey" if str(cs.state) in ("overriding", "preEnabled") else "green") if cs.enabled else "blue" + bg.setText("\U0001F44D" if cs.engageable else "\U0001F6D1") + bg.setStyleSheet(f"font-size: 100px; background-color: {color};") + bg.show() + + alert1.setText(cs.alertText1) + alert2.setText(cs.alertText2) + + if not sm.alive['controlsState']: + alert1.setText("waiting for controls...") + else: + bg.hide() + alert1.setText("") + alert2.setText("offroad") + + HARDWARE.set_screen_brightness(100 if onroad else 40) + os.system("echo 0 > /sys/class/backlight/panel0-backlight/bl_power") + + timer = QTimer() + timer.timeout.connect(update) + timer.start(50) + + app.exec_() diff --git a/selfdrive/ui/update_translations.py b/selfdrive/ui/update_translations.py index e988a13b7f..0fe0f05ac4 100755 --- a/selfdrive/ui/update_translations.py +++ b/selfdrive/ui/update_translations.py @@ -28,7 +28,7 @@ def update_translations(vanish: bool = False, translation_files: None | list[str generate_translations_include() if translation_files is None: - with open(LANGUAGES_FILE, "r") as f: + with open(LANGUAGES_FILE) as f: translation_files = json.load(f).values() for file in translation_files: diff --git a/system/athena/__init__.py b/system/athena/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/athena/athenad.py b/system/athena/athenad.py similarity index 90% rename from selfdrive/athena/athenad.py rename to system/athena/athenad.py index 833bf841f5..9eec7a931b 100755 --- a/selfdrive/athena/athenad.py +++ b/system/athena/athenad.py @@ -19,7 +19,8 @@ from dataclasses import asdict, dataclass, replace from datetime import datetime from functools import partial from queue import Queue -from typing import Callable, Dict, List, Optional, Set, Union, cast +from typing import cast +from collections.abc import Callable import requests from jsonrpc import JSONRPCResponseManager, dispatcher @@ -36,7 +37,7 @@ from openpilot.common.realtime import set_core_affinity from openpilot.system.hardware import HARDWARE, PC from openpilot.system.loggerd.xattr_cache import getxattr, setxattr from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_commit, get_normalized_origin, get_short_branch, get_version +from openpilot.system.version import get_build_metadata from openpilot.system.hardware.hw import Paths @@ -55,17 +56,17 @@ WS_FRAME_SIZE = 4096 NetworkType = log.DeviceState.NetworkType -UploadFileDict = Dict[str, Union[str, int, float, bool]] -UploadItemDict = Dict[str, Union[str, bool, int, float, Dict[str, str]]] +UploadFileDict = dict[str, str | int | float | bool] +UploadItemDict = dict[str, str | bool | int | float | dict[str, str]] -UploadFilesToUrlResponse = Dict[str, Union[int, List[UploadItemDict], List[str]]] +UploadFilesToUrlResponse = dict[str, int | list[UploadItemDict] | list[str]] @dataclass class UploadFile: fn: str url: str - headers: Dict[str, str] + headers: dict[str, str] allow_cellular: bool @classmethod @@ -77,9 +78,9 @@ class UploadFile: class UploadItem: path: str url: str - headers: Dict[str, str] + headers: dict[str, str] created_at: int - id: Optional[str] + id: str | None retry_count: int = 0 current: bool = False progress: float = 0 @@ -97,9 +98,9 @@ send_queue: Queue[str] = queue.Queue() upload_queue: Queue[UploadItem] = queue.Queue() low_priority_send_queue: Queue[str] = queue.Queue() log_recv_queue: Queue[str] = queue.Queue() -cancelled_uploads: Set[str] = set() +cancelled_uploads: set[str] = set() -cur_upload_items: Dict[int, Optional[UploadItem]] = {} +cur_upload_items: dict[int, UploadItem | None] = {} def strip_bz2_extension(fn: str) -> str: @@ -127,14 +128,14 @@ class UploadQueueCache: @staticmethod def cache(upload_queue: Queue[UploadItem]) -> None: try: - queue: List[Optional[UploadItem]] = list(upload_queue.queue) + queue: list[UploadItem | None] = list(upload_queue.queue) items = [asdict(i) for i in queue if i is not None and (i.id not in cancelled_uploads)] Params().put("AthenadUploadQueue", json.dumps(items)) except Exception: cloudlog.exception("athena.UploadQueueCache.cache.exception") -def handle_long_poll(ws: WebSocket, exit_event: Optional[threading.Event]) -> None: +def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None: end_event = threading.Event() threads = [ @@ -206,13 +207,17 @@ def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = Tr break -def cb(sm, item, tid, sz: int, cur: int) -> None: +def cb(sm, item, tid, end_event: threading.Event, sz: int, cur: int) -> None: # Abort transfer if connection changed to metered after starting upload + # or if athenad is shutting down to re-connect the websocket sm.update(0) metered = sm['deviceState'].networkMetered if metered and (not item.allow_cellular): raise AbortTransferException + if end_event.is_set(): + raise AbortTransferException + cur_upload_items[tid] = replace(item, progress=cur / sz if sz else 1) @@ -252,7 +257,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, partial(cb, sm, item, tid)) + response = _do_upload(item, partial(cb, sm, item, tid, end_event)) 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) @@ -274,7 +279,7 @@ def upload_handler(end_event: threading.Event) -> None: cloudlog.exception("athena.upload_handler.exception") -def _do_upload(upload_item: UploadItem, callback: Optional[Callable] = None) -> requests.Response: +def _do_upload(upload_item: UploadItem, callback: Callable = None) -> requests.Response: path = upload_item.path compress = False @@ -313,17 +318,18 @@ def getMessage(service: str, timeout: int = 1000) -> dict: @dispatcher.add_method -def getVersion() -> Dict[str, str]: +def getVersion() -> dict[str, str]: + build_metadata = get_build_metadata() return { - "version": get_version(), - "remote": get_normalized_origin(), - "branch": get_short_branch(), - "commit": get_commit(), + "version": build_metadata.openpilot.version, + "remote": build_metadata.openpilot.git_normalized_origin, + "branch": build_metadata.channel, + "commit": build_metadata.openpilot.git_commit, } @dispatcher.add_method -def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optional[str] = None, place_details: Optional[str] = None) -> Dict[str, int]: +def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: str = None, place_details: str = None) -> dict[str, int]: destination = { "latitude": latitude, "longitude": longitude, @@ -335,7 +341,7 @@ def setNavDestination(latitude: int = 0, longitude: int = 0, place_name: Optiona return {"success": 1} -def scan_dir(path: str, prefix: str) -> List[str]: +def scan_dir(path: str, prefix: str) -> list[str]: files = [] # only walk directories that match the prefix # (glob and friends traverse entire dir tree) @@ -355,12 +361,12 @@ def scan_dir(path: str, prefix: str) -> List[str]: return files @dispatcher.add_method -def listDataDirectory(prefix='') -> List[str]: +def listDataDirectory(prefix='') -> list[str]: return scan_dir(Paths.log_root(), prefix) @dispatcher.add_method -def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesToUrlResponse: +def uploadFileToUrl(fn: str, url: str, headers: dict[str, str]) -> UploadFilesToUrlResponse: # this is because mypy doesn't understand that the decorator doesn't change the return type response: UploadFilesToUrlResponse = uploadFilesToUrls([{ "fn": fn, @@ -371,11 +377,11 @@ def uploadFileToUrl(fn: str, url: str, headers: Dict[str, str]) -> UploadFilesTo @dispatcher.add_method -def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlResponse: +def uploadFilesToUrls(files_data: list[UploadFileDict]) -> UploadFilesToUrlResponse: files = map(UploadFile.from_dict, files_data) - items: List[UploadItemDict] = [] - failed: List[str] = [] + items: list[UploadItemDict] = [] + failed: list[str] = [] for file in files: if len(file.fn) == 0 or file.fn[0] == '/' or '..' in file.fn or len(file.url) == 0: failed.append(file.fn) @@ -414,13 +420,13 @@ def uploadFilesToUrls(files_data: List[UploadFileDict]) -> UploadFilesToUrlRespo @dispatcher.add_method -def listUploadQueue() -> List[UploadItemDict]: +def listUploadQueue() -> list[UploadItemDict]: items = list(upload_queue.queue) + list(cur_upload_items.values()) return [asdict(i) for i in items if (i is not None) and (i.id not in cancelled_uploads)] @dispatcher.add_method -def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]]: +def cancelUpload(upload_id: str | list[str]) -> dict[str, int | str]: if not isinstance(upload_id, list): upload_id = [upload_id] @@ -433,7 +439,7 @@ def cancelUpload(upload_id: Union[str, List[str]]) -> Dict[str, Union[int, str]] return {"success": 1} @dispatcher.add_method -def setRouteViewed(route: str) -> Dict[str, Union[int, str]]: +def setRouteViewed(route: str) -> dict[str, int | str]: # maintain a list of the last 10 routes viewed in connect params = Params() @@ -448,7 +454,7 @@ def setRouteViewed(route: str) -> Dict[str, Union[int, str]]: return {"success": 1} -def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> Dict[str, int]: +def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> dict[str, int]: try: if local_port not in LOCAL_PORT_WHITELIST: raise Exception("Requested local port not whitelisted") @@ -461,6 +467,10 @@ def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local cookie="jwt=" + identity_token, enable_multithread=True) + # Set TOS to keep connection responsive while under load. + # DSCP of 36/HDD_LINUX_AC_VI with the minimum delay flag + ws.sock.setsockopt(socket.IPPROTO_IP, socket.IP_TOS, 0x90) + ssock, csock = socket.socketpair() local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) local_sock.connect(('127.0.0.1', local_port)) @@ -482,7 +492,7 @@ def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local @dispatcher.add_method -def getPublicKey() -> Optional[str]: +def getPublicKey() -> str | None: if not os.path.isfile(Paths.persist_root() + '/comma/id_rsa.pub'): return None @@ -522,7 +532,7 @@ def getNetworks(): @dispatcher.add_method -def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: +def takeSnapshot() -> str | dict[str, str] | None: from openpilot.system.camerad.snapshot.snapshot import jpeg_write, snapshot ret = snapshot() if ret is not None: @@ -539,7 +549,7 @@ def takeSnapshot() -> Optional[Union[str, Dict[str, str]]]: raise Exception("not available while camerad is started") -def get_logs_to_send_sorted() -> List[str]: +def get_logs_to_send_sorted() -> list[str]: # TODO: scan once then use inotify to detect file creation/deletion curr_time = int(time.time()) logs = [] @@ -651,10 +661,12 @@ def stat_handler(end_event: threading.Event) -> None: def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket, end_event: threading.Event, global_end_event: threading.Event) -> None: while not (end_event.is_set() or global_end_event.is_set()): try: - data = ws.recv() - if isinstance(data, str): - data = data.encode("utf-8") - local_sock.sendall(data) + r = select.select((ws.sock,), (), (), 30) + if r[0]: + data = ws.recv() + if isinstance(data, str): + data = data.encode("utf-8") + local_sock.sendall(data) except WebSocketTimeoutException: pass except Exception: @@ -664,6 +676,7 @@ def ws_proxy_recv(ws: WebSocket, local_sock: socket.socket, ssock: socket.socket cloudlog.debug("athena.ws_proxy_recv closing sockets") ssock.close() local_sock.close() + ws.close() cloudlog.debug("athena.ws_proxy_recv done closing sockets") end_event.set() @@ -746,6 +759,9 @@ def ws_manage(ws: WebSocket, end_event: threading.Event) -> None: onroad_prev = onroad if sock is not None: + # While not sending data, onroad, we can expect to time out in 7 + (7 * 2) = 21s + # offroad, we can expect to time out in 30 + (10 * 3) = 60s + # FIXME: TCP_USER_TIMEOUT is effectively 2x for some reason (32s), so it's mostly unused sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_USER_TIMEOUT, 16000 if onroad else 0) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10) @@ -759,7 +775,7 @@ def backoff(retries: int) -> int: return random.randrange(0, min(128, int(2 ** retries))) -def main(exit_event: Optional[threading.Event] = None): +def main(exit_event: threading.Event = None): try: set_core_affinity([0, 1, 2, 3]) except Exception: diff --git a/selfdrive/athena/manage_athenad.py b/system/athena/manage_athenad.py similarity index 57% rename from selfdrive/athena/manage_athenad.py rename to system/athena/manage_athenad.py index 486e426911..f5ab817206 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/system/athena/manage_athenad.py @@ -4,10 +4,10 @@ import time from multiprocessing import Process from openpilot.common.params import Params -from openpilot.selfdrive.manager.process import launcher +from openpilot.system.manager.process import launcher from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import HARDWARE -from openpilot.system.version import get_version, get_normalized_origin, get_short_branch, get_commit, is_dirty +from openpilot.system.version import get_build_metadata ATHENA_MGR_PID_PARAM = "AthenadPid" @@ -15,18 +15,20 @@ ATHENA_MGR_PID_PARAM = "AthenadPid" def main(): params = Params() dongle_id = params.get("DongleId").decode('utf-8') + build_metadata = get_build_metadata() + cloudlog.bind_global(dongle_id=dongle_id, - version=get_version(), - origin=get_normalized_origin(), - branch=get_short_branch(), - commit=get_commit(), - dirty=is_dirty(), + version=build_metadata.openpilot.version, + origin=build_metadata.openpilot.git_normalized_origin, + branch=build_metadata.channel, + commit=build_metadata.openpilot.git_commit, + dirty=build_metadata.openpilot.is_dirty, device=HARDWARE.get_device_type()) try: while 1: cloudlog.info("starting athena daemon") - proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad', 'athenad')) + proc = Process(name='athenad', target=launcher, args=('system.athena.athenad', 'athenad')) proc.start() proc.join() cloudlog.event("athenad exited", exitcode=proc.exitcode) diff --git a/selfdrive/athena/registration.py b/system/athena/registration.py similarity index 93% rename from selfdrive/athena/registration.py rename to system/athena/registration.py index 7db94c28c8..6574d9ac20 100755 --- a/selfdrive/athena/registration.py +++ b/system/athena/registration.py @@ -3,7 +3,6 @@ import time import json import jwt from pathlib import Path -from typing import Optional from datetime import datetime, timedelta from openpilot.common.api import api_get @@ -23,12 +22,12 @@ def is_registered_device() -> bool: return dongle not in (None, UNREGISTERED_DONGLE_ID) -def register(show_spinner=False) -> Optional[str]: +def register(show_spinner=False) -> str | None: params = Params() IMEI = params.get("IMEI", encoding='utf8') HardwareSerial = params.get("HardwareSerial", encoding='utf8') - dongle_id: Optional[str] = params.get("DongleId", encoding='utf8') + dongle_id: str | None = params.get("DongleId", encoding='utf8') needs_registration = None in (IMEI, HardwareSerial, dongle_id) pubkey = Path(Paths.persist_root()+"/comma/id_rsa.pub") @@ -48,8 +47,8 @@ def register(show_spinner=False) -> Optional[str]: # Block until we get the imei serial = HARDWARE.get_serial() start_time = time.monotonic() - imei1: Optional[str] = None - imei2: Optional[str] = None + imei1: str | None = None + imei2: str | None = None while imei1 is None and imei2 is None: try: imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) diff --git a/system/athena/tests/__init__.py b/system/athena/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/athena/tests/helpers.py b/system/athena/tests/helpers.py new file mode 100644 index 0000000000..a0a9cccdc1 --- /dev/null +++ b/system/athena/tests/helpers.py @@ -0,0 +1,70 @@ +import http.server +import socket + + +class MockResponse: + def __init__(self, json, status_code): + self.json = json + self.text = json + self.status_code = status_code + + +class EchoSocket: + def __init__(self, port): + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind(('127.0.0.1', port)) + self.socket.listen(1) + + def run(self): + conn, _ = self.socket.accept() + conn.settimeout(5.0) + + try: + while True: + data = conn.recv(4096) + if data: + print(f'EchoSocket got {data}') + conn.sendall(data) + else: + break + finally: + conn.shutdown(0) + conn.close() + self.socket.shutdown(0) + self.socket.close() + + +class MockApi: + def __init__(self, dongle_id): + pass + + def get_token(self): + return "fake-token" + + +class MockWebsocket: + sock = socket.socket() + + def __init__(self, recv_queue, send_queue): + self.recv_queue = recv_queue + self.send_queue = send_queue + + def recv(self): + data = self.recv_queue.get() + if isinstance(data, Exception): + raise data + return data + + def send(self, data, opcode): + self.send_queue.put_nowait((data, opcode)) + + def close(self): + pass + + +class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): + def do_PUT(self): + length = int(self.headers['Content-Length']) + self.rfile.read(length) + self.send_response(201, "Created") + self.end_headers() diff --git a/system/athena/tests/test_athenad.py b/system/athena/tests/test_athenad.py new file mode 100644 index 0000000000..48519a0ffd --- /dev/null +++ b/system/athena/tests/test_athenad.py @@ -0,0 +1,423 @@ +import pytest +from functools import wraps +import json +import multiprocessing +import os +import requests +import shutil +import time +import threading +import queue +from dataclasses import asdict, replace +from datetime import datetime, timedelta + +from websocket import ABNF +from websocket._exceptions import WebSocketConnectionClosedException + +from cereal import messaging + +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.system.athena import athenad +from openpilot.system.athena.athenad import MAX_RETRY_COUNT, dispatcher +from openpilot.system.athena.tests.helpers import HTTPRequestHandler, MockWebsocket, MockApi, EchoSocket +from openpilot.selfdrive.test.helpers import http_server_context +from openpilot.system.hardware.hw import Paths + + +def seed_athena_server(host, port): + with Timeout(2, 'HTTP Server seeding failed'): + while True: + try: + requests.put(f'http://{host}:{port}/qlog.bz2', data='', timeout=10) + break + except requests.exceptions.ConnectionError: + time.sleep(0.1) + +def with_upload_handler(func): + @wraps(func) + def wrapper(*args, **kwargs): + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + return func(*args, **kwargs) + finally: + end_event.set() + thread.join() + return wrapper + +@pytest.fixture +def mock_create_connection(mocker): + return mocker.patch('openpilot.system.athena.athenad.create_connection') + +@pytest.fixture +def host(): + with http_server_context(handler=HTTPRequestHandler, setup=seed_athena_server) as (host, port): + yield f"http://{host}:{port}" + +class TestAthenadMethods: + @classmethod + def setup_class(cls): + cls.SOCKET_PORT = 45454 + athenad.Api = MockApi + athenad.LOCAL_PORT_WHITELIST = {cls.SOCKET_PORT} + + def setup_method(self): + self.default_params = { + "DongleId": "0000000000000000", + "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private", # noqa: E501 + "GithubUsername": b"commaci", + "AthenadUploadQueue": '[]', + } + + self.params = Params() + for k, v in self.default_params.items(): + self.params.put(k, v) + self.params.put_bool("GsmMetered", True) + + athenad.upload_queue = queue.Queue() + athenad.cur_upload_items.clear() + athenad.cancelled_uploads.clear() + + for i in os.listdir(Paths.log_root()): + p = os.path.join(Paths.log_root(), i) + if os.path.isdir(p): + shutil.rmtree(p) + else: + os.unlink(p) + + # *** test helpers *** + + @staticmethod + def _wait_for_upload(): + now = time.time() + while time.time() - now < 5: + if athenad.upload_queue.qsize() == 0: + break + + @staticmethod + def _create_file(file: str, parent: str = None, data: bytes = b'') -> str: + fn = os.path.join(Paths.log_root() if parent is None else parent, file) + os.makedirs(os.path.dirname(fn), exist_ok=True) + with open(fn, 'wb') as f: + f.write(data) + return fn + + + # *** test cases *** + + def test_echo(self): + assert dispatcher["echo"]("bob") == "bob" + + def test_get_message(self): + with pytest.raises(TimeoutError) as _: + dispatcher["getMessage"]("controlsState") + + end_event = multiprocessing.Event() + + pub_sock = messaging.pub_sock("deviceState") + + def send_deviceState(): + while not end_event.is_set(): + msg = messaging.new_message('deviceState') + pub_sock.send(msg.to_bytes()) + time.sleep(0.01) + + p = multiprocessing.Process(target=send_deviceState) + p.start() + time.sleep(0.1) + try: + deviceState = dispatcher["getMessage"]("deviceState") + assert deviceState['deviceState'] + finally: + end_event.set() + p.join() + + def test_list_data_directory(self): + route = '2021-03-29--13-32-47' + segments = [0, 1, 2, 3, 11] + + filenames = ['qlog', 'qcamera.ts', 'rlog', 'fcamera.hevc', 'ecamera.hevc', 'dcamera.hevc'] + files = [f'{route}--{s}/{f}' for s in segments for f in filenames] + for file in files: + self._create_file(file) + + resp = dispatcher["listDataDirectory"]() + assert resp, 'list empty!' + assert len(resp) == len(files) + + resp = dispatcher["listDataDirectory"](f'{route}--123') + assert len(resp) == 0 + + prefix = f'{route}' + expected = list(filter(lambda f: f.startswith(prefix), files)) + resp = dispatcher["listDataDirectory"](prefix) + assert resp, 'list empty!' + assert len(resp) == len(expected) + + prefix = f'{route}--1' + expected = list(filter(lambda f: f.startswith(prefix), files)) + resp = dispatcher["listDataDirectory"](prefix) + assert resp, 'list empty!' + assert len(resp) == len(expected) + + prefix = f'{route}--1/' + expected = list(filter(lambda f: f.startswith(prefix), files)) + resp = dispatcher["listDataDirectory"](prefix) + assert resp, 'list empty!' + assert len(resp) == len(expected) + + prefix = f'{route}--1/q' + expected = list(filter(lambda f: f.startswith(prefix), files)) + resp = dispatcher["listDataDirectory"](prefix) + assert resp, 'list empty!' + assert len(resp) == len(expected) + + def test_strip_bz2_extension(self): + fn = self._create_file('qlog.bz2') + if fn.endswith('.bz2'): + assert athenad.strip_bz2_extension(fn) == fn[:-4] + + @pytest.mark.parametrize("compress", [True, False]) + def test_do_upload(self, host, compress): + # random bytes to ensure rather large object post-compression + fn = self._create_file('qlog', data=os.urandom(10000 * 1024)) + + upload_fn = fn + ('.bz2' if compress else '') + item = athenad.UploadItem(path=upload_fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') + with pytest.raises(requests.exceptions.ConnectionError): + athenad._do_upload(item) + + item = athenad.UploadItem(path=upload_fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + resp = athenad._do_upload(item) + assert resp.status_code == 201 + + def test_upload_file_to_url(self, host): + fn = self._create_file('qlog.bz2') + + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", f"{host}/qlog.bz2", {}) + assert resp['enqueued'] == 1 + assert 'failed' not in resp + assert {"path": fn, "url": f"{host}/qlog.bz2", "headers": {}}.items() <= resp['items'][0].items() + assert resp['items'][0].get('id') is not None + assert athenad.upload_queue.qsize() == 1 + + def test_upload_file_to_url_duplicate(self, host): + self._create_file('qlog.bz2') + + url1 = f"{host}/qlog.bz2?sig=sig1" + dispatcher["uploadFileToUrl"]("qlog.bz2", url1, {}) + + # Upload same file again, but with different signature + url2 = f"{host}/qlog.bz2?sig=sig2" + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", url2, {}) + assert resp == {'enqueued': 0, 'items': []} + + def test_upload_file_to_url_does_not_exist(self, host): + not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {}) + assert not_exists_resp == {'enqueued': 0, 'items': [], 'failed': ['does_not_exist.bz2']} + + @with_upload_handler + def test_upload_handler(self, host): + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + # TODO: verify that upload actually succeeded + # TODO: also check that end_event and metered network raises AbortTransferException + assert athenad.upload_queue.qsize() == 0 + + @pytest.mark.parametrize("status,retry", [(500,True), (412,False)]) + @with_upload_handler + def test_upload_handler_retry(self, mocker, host, status, retry): + mock_put = mocker.patch('requests.put') + mock_put.return_value.status_code = status + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + assert athenad.upload_queue.qsize() == (1 if retry else 0) + + if retry: + assert athenad.upload_queue.get().retry_count == 1 + + @with_upload_handler + def test_upload_handler_timeout(self): + """When an upload times out or fails to connect it should be placed back in the queue""" + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + item_no_retry = replace(item, retry_count=MAX_RETRY_COUNT) + + athenad.upload_queue.put_nowait(item_no_retry) + self._wait_for_upload() + time.sleep(0.1) + + # Check that upload with retry count exceeded is not put back + assert athenad.upload_queue.qsize() == 0 + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + # Check that upload item was put back in the queue with incremented retry count + assert athenad.upload_queue.qsize() == 1 + assert athenad.upload_queue.get().retry_count == 1 + + @with_upload_handler + def test_cancel_upload(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, + created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + dispatcher["cancelUpload"](item.id) + + assert item.id in athenad.cancelled_uploads + + self._wait_for_upload() + time.sleep(0.1) + + assert athenad.upload_queue.qsize() == 0 + assert len(athenad.cancelled_uploads) == 0 + + @with_upload_handler + def test_cancel_expiry(self): + t_future = datetime.now() - timedelta(days=40) + ts = int(t_future.strftime("%s")) * 1000 + + # Item that would time out if actually uploaded + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=ts, id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + time.sleep(0.1) + + assert athenad.upload_queue.qsize() == 0 + + def test_list_upload_queue_empty(self): + items = dispatcher["listUploadQueue"]() + assert len(items) == 0 + + @with_upload_handler + def test_list_upload_queue_current(self, host: str): + fn = self._create_file('qlog.bz2') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + athenad.upload_queue.put_nowait(item) + self._wait_for_upload() + + items = dispatcher["listUploadQueue"]() + assert len(items) == 1 + assert items[0]['current'] + + def test_list_upload_queue(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, + created_at=int(time.time()*1000), id='id', allow_cellular=True) + athenad.upload_queue.put_nowait(item) + + items = dispatcher["listUploadQueue"]() + assert len(items) == 1 + assert items[0] == asdict(item) + assert not items[0]['current'] + + athenad.cancelled_uploads.add(item.id) + items = dispatcher["listUploadQueue"]() + assert len(items) == 0 + + def test_upload_queue_persistence(self): + item1 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id1') + item2 = athenad.UploadItem(path="_", url="_", headers={}, created_at=int(time.time()), id='id2') + + athenad.upload_queue.put_nowait(item1) + athenad.upload_queue.put_nowait(item2) + + # Ensure cancelled items are not persisted + athenad.cancelled_uploads.add(item2.id) + + # serialize item + athenad.UploadQueueCache.cache(athenad.upload_queue) + + # deserialize item + athenad.upload_queue.queue.clear() + athenad.UploadQueueCache.initialize(athenad.upload_queue) + + assert athenad.upload_queue.qsize() == 1 + assert asdict(athenad.upload_queue.queue[-1]) == asdict(item1) + + def test_start_local_proxy(self, mock_create_connection): + end_event = threading.Event() + + ws_recv = queue.Queue() + ws_send = queue.Queue() + mock_ws = MockWebsocket(ws_recv, ws_send) + mock_create_connection.return_value = mock_ws + + echo_socket = EchoSocket(self.SOCKET_PORT) + socket_thread = threading.Thread(target=echo_socket.run) + socket_thread.start() + + athenad.startLocalProxy(end_event, 'ws://localhost:1234', self.SOCKET_PORT) + + ws_recv.put_nowait(b'ping') + try: + recv = ws_send.get(timeout=5) + assert recv == (b'ping', ABNF.OPCODE_BINARY), recv + finally: + # signal websocket close to athenad.ws_proxy_recv + ws_recv.put_nowait(WebSocketConnectionClosedException()) + socket_thread.join() + + def test_get_ssh_authorized_keys(self): + keys = dispatcher["getSshAuthorizedKeys"]() + assert keys == self.default_params["GithubSshKeys"].decode('utf-8') + + def test_get_github_username(self): + keys = dispatcher["getGithubUsername"]() + assert keys == self.default_params["GithubUsername"].decode('utf-8') + + def test_get_version(self): + resp = dispatcher["getVersion"]() + keys = ["version", "remote", "branch", "commit"] + assert list(resp.keys()) == keys + for k in keys: + assert isinstance(resp[k], str), f"{k} is not a string" + assert len(resp[k]) > 0, f"{k} has no value" + + def test_jsonrpc_handler(self): + end_event = threading.Event() + thread = threading.Thread(target=athenad.jsonrpc_handler, args=(end_event,)) + thread.daemon = True + thread.start() + try: + # with params + athenad.recv_queue.put_nowait(json.dumps({"method": "echo", "params": ["hello"], "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + assert json.loads(resp) == {'result': 'hello', 'id': 0, 'jsonrpc': '2.0'} + # without params + athenad.recv_queue.put_nowait(json.dumps({"method": "getNetworkType", "jsonrpc": "2.0", "id": 0})) + resp = athenad.send_queue.get(timeout=3) + assert json.loads(resp) == {'result': 1, 'id': 0, 'jsonrpc': '2.0'} + # log forwarding + athenad.recv_queue.put_nowait(json.dumps({'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'})) + resp = athenad.log_recv_queue.get(timeout=3) + assert json.loads(resp) == {'result': {'success': 1}, 'id': 0, 'jsonrpc': '2.0'} + finally: + end_event.set() + thread.join() + + def test_get_logs_to_send_sorted(self): + fl = list() + for i in range(10): + file = f'swaglog.{i:010}' + self._create_file(file, Paths.swaglog_root()) + fl.append(file) + + # ensure the list is all logs except most recent + sl = athenad.get_logs_to_send_sorted() + assert sl == fl[:-1] diff --git a/system/athena/tests/test_athenad_ping.py b/system/athena/tests/test_athenad_ping.py new file mode 100644 index 0000000000..73fe7783af --- /dev/null +++ b/system/athena/tests/test_athenad_ping.py @@ -0,0 +1,102 @@ +import pytest +import subprocess +import threading +import time +from typing import cast + +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.system.athena import athenad +from openpilot.system.manager.helpers import write_onroad_params +from openpilot.system.hardware import TICI + +TIMEOUT_TOLERANCE = 20 # seconds + + +def wifi_radio(on: bool) -> None: + if not TICI: + return + print(f"wifi {'on' if on else 'off'}") + subprocess.run(["nmcli", "radio", "wifi", "on" if on else "off"], check=True) + + +class TestAthenadPing: + params: Params + dongle_id: str + + athenad: threading.Thread + exit_event: threading.Event + + def _get_ping_time(self) -> str | None: + return cast(str | None, self.params.get("LastAthenaPingTime", encoding="utf-8")) + + def _clear_ping_time(self) -> None: + self.params.remove("LastAthenaPingTime") + + def _received_ping(self) -> bool: + return self._get_ping_time() is not None + + @classmethod + def teardown_class(cls) -> None: + wifi_radio(True) + + def setup_method(self) -> None: + self.params = Params() + self.dongle_id = self.params.get("DongleId", encoding="utf-8") + + wifi_radio(True) + self._clear_ping_time() + + self.exit_event = threading.Event() + self.athenad = threading.Thread(target=athenad.main, args=(self.exit_event,)) + + def teardown_method(self) -> None: + if self.athenad.is_alive(): + self.exit_event.set() + self.athenad.join() + + def assertTimeout(self, reconnect_time: float, subtests, mocker) -> None: + self.athenad.start() + + mock_create_connection = mocker.patch('openpilot.system.athena.athenad.create_connection', + new_callable=lambda: mocker.MagicMock(wraps=athenad.create_connection)) + + time.sleep(1) + mock_create_connection.assert_called_once() + mock_create_connection.reset_mock() + + # check normal behaviour, server pings on connection + with subtests.test("Wi-Fi: receives ping"), Timeout(70, "no ping received"): + while not self._received_ping(): + time.sleep(0.1) + print("ping received") + + mock_create_connection.assert_not_called() + + # websocket should attempt reconnect after short time + with subtests.test("LTE: attempt reconnect"): + wifi_radio(False) + print("waiting for reconnect attempt") + start_time = time.monotonic() + with Timeout(reconnect_time, "no reconnect attempt"): + while not mock_create_connection.called: + time.sleep(0.1) + print(f"reconnect attempt after {time.monotonic() - start_time:.2f}s") + + self._clear_ping_time() + + # check ping received after reconnect + with subtests.test("LTE: receives ping"), Timeout(70, "no ping received"): + while not self._received_ping(): + time.sleep(0.1) + print("ping received") + + @pytest.mark.skipif(not TICI, reason="only run on desk") + def test_offroad(self, subtests, mocker) -> None: + write_onroad_params(False, self.params) + self.assertTimeout(60 + TIMEOUT_TOLERANCE, subtests, mocker) # based using TCP keepalive settings + + @pytest.mark.skipif(not TICI, reason="only run on desk") + def test_onroad(self, subtests, mocker) -> None: + write_onroad_params(True, self.params) + self.assertTimeout(21 + TIMEOUT_TOLERANCE, subtests, mocker) diff --git a/system/athena/tests/test_registration.py b/system/athena/tests/test_registration.py new file mode 100644 index 0000000000..4f663fbc0a --- /dev/null +++ b/system/athena/tests/test_registration.py @@ -0,0 +1,74 @@ +import json +from Crypto.PublicKey import RSA +from pathlib import Path + +from openpilot.common.params import Params +from openpilot.system.athena.registration import register, UNREGISTERED_DONGLE_ID +from openpilot.system.athena.tests.helpers import MockResponse +from openpilot.system.hardware.hw import Paths + + +class TestRegistration: + + def setup_method(self): + # clear params and setup key paths + self.params = Params() + self.params.clear_all() + + persist_dir = Path(Paths.persist_root()) / "comma" + persist_dir.mkdir(parents=True, exist_ok=True) + + self.priv_key = persist_dir / "id_rsa" + self.pub_key = persist_dir / "id_rsa.pub" + + def _generate_keys(self): + self.pub_key.touch() + k = RSA.generate(2048) + with open(self.priv_key, "wb") as f: + f.write(k.export_key()) + with open(self.pub_key, "wb") as f: + f.write(k.publickey().export_key()) + + def test_valid_cache(self, mocker): + # if all params are written, return the cached dongle id + self.params.put("IMEI", "imei") + self.params.put("HardwareSerial", "serial") + self._generate_keys() + + m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) + dongle = "DONGLE_ID_123" + self.params.put("DongleId", dongle) + assert register() == dongle + assert not m.called + + def test_no_keys(self, mocker): + # missing pubkey + m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) + dongle = register() + assert m.call_count == 0 + assert dongle == UNREGISTERED_DONGLE_ID + assert self.params.get("DongleId", encoding='utf-8') == dongle + + def test_missing_cache(self, mocker): + # keys exist but no dongle id + self._generate_keys() + m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) + dongle = "DONGLE_ID_123" + m.return_value = MockResponse(json.dumps({'dongle_id': dongle}), 200) + assert register() == dongle + assert m.call_count == 1 + + # call again, shouldn't hit the API this time + assert register() == dongle + assert m.call_count == 1 + assert self.params.get("DongleId", encoding='utf-8') == dongle + + def test_unregistered(self, mocker): + # keys exist, but unregistered + self._generate_keys() + m = mocker.patch("openpilot.system.athena.registration.api_get", autospec=True) + m.return_value = MockResponse(None, 402) + dongle = register() + assert m.call_count == 1 + assert dongle == UNREGISTERED_DONGLE_ID + assert self.params.get("DongleId", encoding='utf-8') == dongle diff --git a/system/camerad/SConscript b/system/camerad/SConscript index 8f19e7ee19..511664c275 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -1,6 +1,6 @@ -Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc') -libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] +libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', messaging, visionipc, gpucommon, 'atomic'] camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) diff --git a/system/camerad/__init__.py b/system/camerad/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 91c6d44f84..9d82284d9f 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -8,7 +8,6 @@ #include "common/clutil.h" #include "common/swaglog.h" -#include "common/util.h" #include "third_party/linux/include/msm_media_info.h" #include "system/camerad/cameras/camera_qcom2.h" @@ -18,36 +17,38 @@ ExitHandler do_exit; -class Debayer { +class ImgProc { public: - Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { + ImgProc(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; const SensorInfo *ci = s->ci.get(); snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-cl-fast-relaxed-math -cl-denorms-are-zero -Isensors " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DYUV_STRIDE=%d -DUV_OFFSET=%d " - "-DIS_OX=%d -DCAM_NUM=%d%s", - ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, + "-DSENSOR_ID=%hu -DHDR_OFFSET=%d -DVIGNETTING=%d ", + ci->frame_width, ci->frame_height, ci->hdr_offset > 0 ? ci->frame_stride * 2 : ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, buf_width, uv_offset, - ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); - const char *cl_file = "cameras/real_debayer.cl"; - cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); - krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); - CL_CHECK(clReleaseProgram(prg_debayer)); + static_cast(ci->image_sensor), ci->hdr_offset, s->camera_num == 1); + const char *cl_file = "cameras/process_raw.cl"; + cl_program prg_imgproc = cl_program_from_file(context, device_id, cl_file, args); + krnl_ = CL_CHECK_ERR(clCreateKernel(prg_imgproc, "process_raw", &err)); + CL_CHECK(clReleaseProgram(prg_imgproc)); + } - void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, cl_event *debayer_event) { + void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, cl_event *imgproc_event, int expo_time) { CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl)); CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl)); + CL_CHECK(clSetKernelArg(krnl_, 2, sizeof(cl_int), &expo_time)); const size_t globalWorkSize[] = {size_t(width / 2), size_t(height / 2)}; - const int debayer_local_worksize = 16; - const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize}; - CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event)); + const int imgproc_local_worksize = 16; + const size_t localWorkSize[] = {imgproc_local_worksize, imgproc_local_worksize}; + CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, imgproc_event)); } - ~Debayer() { + ~ImgProc() { CL_CHECK(clReleaseKernel(krnl_)); } @@ -73,18 +74,22 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, LOGD("allocated %d CL buffers", frame_buf_count); rgb_width = ci->frame_width; - rgb_height = ci->frame_height; + rgb_height = ci->hdr_offset > 0 ? (ci->frame_height - ci->hdr_offset) / 2 : ci->frame_height; int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, rgb_width); int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, rgb_height); assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, rgb_width)); assert(nv12_height/2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, rgb_height)); - size_t nv12_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage size_t nv12_uv_offset = nv12_width * nv12_height; + + // the encoder HW tells us the size it wants after setting it up. + // TODO: VENUS_BUFFER_SIZE should give the size, but it's too small. dependent on encoder settings? + size_t nv12_size = (rgb_width >= 2688 ? 2900 : 2346)*nv12_width; + vipc_server->create_buffers_with_sizes(stream_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height, nv12_size, nv12_width, nv12_uv_offset); LOGD("created %d YUV vipc buffers with size %dx%d", YUV_BUFFER_COUNT, nv12_width, nv12_height); - debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); + imgproc = new ImgProc(device_id, context, this, s, nv12_width, nv12_uv_offset); const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); @@ -94,7 +99,7 @@ CameraBuf::~CameraBuf() { for (int i = 0; i < frame_buf_count; i++) { camera_bufs[i].free(); } - if (debayer) delete debayer; + if (imgproc) delete imgproc; if (q) CL_CHECK(clReleaseCommandQueue(q)); } @@ -112,7 +117,7 @@ bool CameraBuf::acquire() { double start_time = millis_since_boot(); cl_event event; - debayer->queue(q, camera_bufs[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event); + imgproc->queue(q, camera_bufs[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, rgb_width, rgb_height, &event, cur_frame_data.integ_lines); clWaitForEvents(1, &event); CL_CHECK(clReleaseEvent(event)); cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0; @@ -252,14 +257,14 @@ static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) { pm->send("thumbnail", msg); } -float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) { +float set_exposure_target(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip) { int lum_med; uint32_t lum_binning[256] = {0}; const uint8_t *pix_ptr = b->cur_yuv_buf->y; unsigned int lum_total = 0; - for (int y = y_start; y < y_end; y += y_skip) { - for (int x = x_start; x < x_end; x += x_skip) { + for (int y = ae_xywh.y; y < ae_xywh.y + ae_xywh.h; y += y_skip) { + for (int x = ae_xywh.x; x < ae_xywh.x + ae_xywh.w; x += x_skip) { uint8_t lum = pix_ptr[(y * b->rgb_width) + x]; lum_binning[lum]++; lum_total += 1; diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index f98691ef00..555362ab8b 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -5,8 +5,9 @@ #include #include "cereal/messaging/messaging.h" -#include "cereal/visionipc/visionipc_server.h" +#include "msgq/visionipc/visionipc_server.h" #include "common/queue.h" +#include "common/util.h" const int YUV_BUFFER_COUNT = 20; @@ -44,12 +45,12 @@ typedef struct FrameMetadata { struct MultiCameraState; class CameraState; -class Debayer; +class ImgProc; class CameraBuf { private: VisionIpcServer *vipc_server; - Debayer *debayer = nullptr; + ImgProc *imgproc = nullptr; VisionStreamType stream_type; int cur_buf_idx; SafeQueue safe_queue; @@ -75,7 +76,7 @@ typedef void (*process_thread_cb)(MultiCameraState *s, CameraState *c, int cnt); void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &frame_data, CameraState *c); kj::Array get_raw_frame_image(const CameraBuf *b); -float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip); +float set_exposure_target(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip); std::thread start_process_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback); void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 3191b821ac..096e288cc2 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -31,8 +31,7 @@ int CameraState::clear_req_queue() { req_mgr_flush_request.session_hdl = session_handle; req_mgr_flush_request.link_hdl = link_handle; req_mgr_flush_request.flush_type = CAM_REQ_MGR_FLUSH_TYPE_ALL; - int ret; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); + int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_FLUSH_REQ, &req_mgr_flush_request, sizeof(req_mgr_flush_request)); // LOGD("flushed all req: %d", ret); return ret; } @@ -396,6 +395,35 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* +void CameraState::set_exposure_rect() { + // set areas for each camera, shouldn't be changed + std::vector> ae_targets = { + // (Rect, F) + std::make_pair((Rect){96, 250, 1734, 524}, 567.0), // wide + std::make_pair((Rect){96, 160, 1734, 986}, 2648.0), // road + std::make_pair((Rect){96, 242, 1736, 906}, 567.0) // driver + }; + int h_ref = 1208; + /* + exposure target intrinics is + [ + [F, 0, 0.5*ae_xywh[2]] + [0, F, 0.5*H-ae_xywh[1]] + [0, 0, 1] + ] + */ + auto ae_target = ae_targets[camera_num]; + Rect xywh_ref = ae_target.first; + float fl_ref = ae_target.second; + + ae_xywh = (Rect){ + std::max(0, buf.rgb_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::max(0, buf.rgb_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))), + std::min((int)(fl_pix / fl_ref * xywh_ref.w), buf.rgb_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)), + std::min((int)(fl_pix / fl_ref * xywh_ref.h), buf.rgb_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))) + }; +} + void CameraState::sensor_set_parameters() { target_grey_fraction = 0.3; @@ -421,7 +449,7 @@ void CameraState::camera_map_bufs(MultiCameraState *s) { enqueue_req_multi(1, FRAME_BUF_COUNT, 0); } -void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, float focal_len) { if (!enabled) return; LOGD("camera init %d", camera_num); @@ -430,6 +458,9 @@ void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, cl_devic buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); camera_map_bufs(s); + + fl_pix = focal_len / ci->pixel_size_mm; + set_exposure_rect(); } void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num_, bool enabled_) { @@ -438,7 +469,6 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num enabled = enabled_; if (!enabled) return; - int ret; sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num); assert(sensor_fd >= 0); LOGD("opened sensor for %d", camera_num); @@ -469,7 +499,7 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num // create session struct cam_req_mgr_session_info session_info = {}; - ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); + int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); LOGD("get session: %d 0x%X", ret, session_info.session_hdl); session_handle = session_info.session_hdl; @@ -614,16 +644,14 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_DRIVER); - s->road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_ROAD); - s->wide_road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_WIDE_ROAD); + s->driver_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_DRIVER, DRIVER_FL_MM); + s->road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_ROAD, ROAD_FL_MM); + s->wide_road_cam.camera_init(s, v, device_id, ctx, VISION_STREAM_WIDE_ROAD, WIDE_FL_MM); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } void cameras_open(MultiCameraState *s) { - int ret; - LOG("-- Opening devices"); // video0 is req_mgr, the target of many ioctls s->video0_fd = HANDLE_EINTR(open("/dev/v4l/by-path/platform-soc:qcom_cam-req-mgr-video-index0", O_RDWR | O_NONBLOCK)); @@ -647,7 +675,7 @@ void cameras_open(MultiCameraState *s) { query_cap_cmd.handle_type = 1; query_cap_cmd.caps_handle = (uint64_t)&isp_query_cap_cmd; query_cap_cmd.size = sizeof(isp_query_cap_cmd); - ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); + int ret = do_cam_control(s->isp_fd, CAM_QUERY_CAP, &query_cap_cmd, sizeof(query_cap_cmd)); assert(ret == 0); LOGD("using MMU handle: %x", isp_query_cap_cmd.device_iommu.non_secure); LOGD("using MMU handle: %x", isp_query_cap_cmd.cdm_iommu.non_secure); @@ -671,15 +699,13 @@ void cameras_open(MultiCameraState *s) { } void CameraState::camera_close() { - int ret; - // stop devices LOG("-- Stop devices %d", camera_num); if (enabled) { // ret = device_control(sensor_fd, CAM_STOP_DEV, session_handle, sensor_dev_handle); // LOGD("stop sensor: %d", ret); - ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle); + int ret = device_control(multi_cam_state->isp_fd, CAM_STOP_DEV, session_handle, isp_dev_handle); LOGD("stop isp: %d", ret); ret = device_control(csiphy_fd, CAM_STOP_DEV, session_handle, csiphy_dev_handle); LOGD("stop csiphy: %d", ret); @@ -714,7 +740,7 @@ void CameraState::camera_close() { LOGD("released buffers"); } - ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); + int ret = device_control(sensor_fd, CAM_RELEASE_DEV, session_handle, sensor_dev_handle); LOGD("release sensor: %d", ret); // destroyed session @@ -902,7 +928,7 @@ void CameraState::set_camera_exposure(float grey_frac) { } static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { - c->set_camera_exposure(set_exposure_target(&c->buf, 96, 1832, 2, 242, 1148, 4)); + c->set_camera_exposure(set_exposure_target(&c->buf, c->ae_xywh, 2, 4)); MessageBuilder msg; auto framed = msg.initEvent().initDriverCameraState(); @@ -927,9 +953,8 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { c->ci->processRegisters(c, framed); s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); - const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); const int skip = 2; - c->set_camera_exposure(set_exposure_target(b, x, x + w, skip, y, y + h, skip)); + c->set_camera_exposure(set_exposure_target(b, c->ae_xywh, skip, skip)); } void cameras_run(MultiCameraState *s) { diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 47ca578b99..0b15c9c3f0 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -11,6 +11,10 @@ #define FRAME_BUF_COUNT 4 +#define ROAD_FL_MM 8.0f +#define WIDE_FL_MM 1.71f +#define DRIVER_FL_MM 1.71f + class CameraState { public: MultiCameraState *multi_cam_state; @@ -30,6 +34,7 @@ public: int new_exp_g; int new_exp_t; + Rect ae_xywh; float measured_grey_fraction; float target_grey_fraction; @@ -37,6 +42,7 @@ public: unique_fd csiphy_fd; int camera_num; + float fl_pix; void handle_camera_event(void *evdat); void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain); @@ -45,9 +51,10 @@ public: void sensors_start(); void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); + void set_exposure_rect(); void sensor_set_parameters(); void camera_map_bufs(MultiCameraState *s); - void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); + void camera_init(MultiCameraState *s, VisionIpcServer *v, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type, float focal_len); void camera_close(); int32_t session_handle; diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc index 4fe38997ce..7bd23fd9fe 100644 --- a/system/camerad/cameras/camera_util.cc +++ b/system/camerad/cameras/camera_util.cc @@ -86,11 +86,10 @@ void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align, int f } void release(int video0_fd, uint32_t handle) { - int ret; struct cam_mem_mgr_release_cmd mem_mgr_release_cmd = {0}; mem_mgr_release_cmd.buf_handle = handle; - ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); + int ret = do_cam_control(video0_fd, CAM_REQ_MGR_RELEASE_BUF, &mem_mgr_release_cmd, sizeof(mem_mgr_release_cmd)); assert(ret == 0); } diff --git a/system/camerad/cameras/process_raw.cl b/system/camerad/cameras/process_raw.cl new file mode 100644 index 0000000000..6f6612fab0 --- /dev/null +++ b/system/camerad/cameras/process_raw.cl @@ -0,0 +1,238 @@ +#include "ar0231_cl.h" +#include "ox03c10_cl.h" +#include "os04c10_cl.h" + +#define UV_WIDTH RGB_WIDTH / 2 +#define UV_HEIGHT RGB_HEIGHT / 2 + +#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) +#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) +#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) +#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) + +#if defined(BGGR) + #define ROW_READ_ORDER (int[]){3, 2, 1, 0} + #define RGB_WRITE_ORDER (int[]){2, 3, 0, 1} +#else + #define ROW_READ_ORDER (int[]){0, 1, 2, 3} + #define RGB_WRITE_ORDER (int[]){0, 1, 2, 3} +#endif + +float get_vignetting_s(float r) { + if (r < 62500) { + return (1.0f + 0.0000008f*r); + } else if (r < 490000) { + return (0.9625f + 0.0000014f*r); + } else if (r < 1102500) { + return (1.26434f + 0.0000000000016f*r*r); + } else { + return (0.53503625f + 0.0000000000022f*r*r); + } +} + +int4 parse_12bit(uchar8 pvs) { + // lower bits scambled? + return (int4)(((int)pvs.s0<<4) + (pvs.s1>>4), + ((int)pvs.s2<<4) + (pvs.s4&0xF), + ((int)pvs.s3<<4) + (pvs.s4>>4), + ((int)pvs.s5<<4) + (pvs.s7&0xF)); +} + +int4 parse_10bit(uchar8 pvs, uchar ext, bool aligned) { + if (aligned) { + return (int4)(((int)pvs.s0 << 2) + (pvs.s1 & 0b00000011), + ((int)pvs.s2 << 2) + ((pvs.s6 & 0b11000000) / 64), + ((int)pvs.s3 << 2) + ((pvs.s6 & 0b00110000) / 16), + ((int)pvs.s4 << 2) + ((pvs.s6 & 0b00001100) / 4)); + } else { + return (int4)(((int)pvs.s0 << 2) + ((pvs.s3 & 0b00110000) / 16), + ((int)pvs.s1 << 2) + ((pvs.s3 & 0b00001100) / 4), + ((int)pvs.s2 << 2) + ((pvs.s3 & 0b00000011)), + ((int)pvs.s4 << 2) + ((ext & 0b11000000) / 64)); + } +} + +float get_k(float a, float b, float c, float d) { + return 2.0 - (fabs(a - b) + fabs(c - d)); +} + +__kernel void process_raw(const __global uchar * in, __global uchar * out, int expo_time) +{ + const int gid_x = get_global_id(0); + const int gid_y = get_global_id(1); + + // estimate vignetting + #if VIGNETTING + int gx = (gid_x*2 - RGB_WIDTH/2); + int gy = (gid_y*2 - RGB_HEIGHT/2); + const float vignette_factor = get_vignetting_s((gx*gx + gy*gy) / VIGNETTE_RSZ); + #else + const float vignette_factor = 1.0; + #endif + + const int row_before_offset = (gid_y == 0) ? 2 : 0; + const int row_after_offset = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1 : 3; + + float3 rgb_tmp; + uchar3 rgb_out[4]; // output is 2x2 window + + // read offset + int start_idx; + #if BIT_DEPTH == 10 + bool aligned10; + if (gid_x % 2 == 0) { + aligned10 = true; + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * gid_x / 2 - 2) + (FRAME_STRIDE * FRAME_OFFSET); + } else { + aligned10 = false; + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (5 * (gid_x - 1) / 2 + 1) + (FRAME_STRIDE * FRAME_OFFSET); + } + #else + start_idx = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); + #endif + + // read in 4 rows, 8 uchars each + uchar8 dat[4]; + // row_before + dat[0] = vload8(0, in + start_idx + FRAME_STRIDE*row_before_offset); + // row_0 + if (gid_x == 0 && gid_y == 0) { + // this wasn't a problem due to extra rows + dat[1] = vload8(0, in + start_idx + FRAME_STRIDE*1 + 2); + dat[1] = (uchar8)(0, 0, dat[1].s0, dat[1].s1, dat[1].s2, dat[1].s3, dat[1].s4, dat[1].s5); + } else { + dat[1] = vload8(0, in + start_idx + FRAME_STRIDE*1); + } + // row_1 + dat[2] = vload8(0, in + start_idx + FRAME_STRIDE*2); + // row_after + dat[3] = vload8(0, in + start_idx + FRAME_STRIDE*row_after_offset); + // need extra bit for 10-bit, 4 rows, 1 uchar each + #if BIT_DEPTH == 10 + uchar extra_dat[4]; + if (!aligned10) { + extra_dat[0] = in[start_idx + FRAME_STRIDE*row_before_offset + 8]; + extra_dat[1] = in[start_idx + FRAME_STRIDE*1 + 8]; + extra_dat[2] = in[start_idx + FRAME_STRIDE*2 + 8]; + extra_dat[3] = in[start_idx + FRAME_STRIDE*row_after_offset + 8]; + } + #endif + + // read odd rows for staggered second exposure + #if HDR_OFFSET > 0 + uchar8 short_dat[4]; + short_dat[0] = vload8(0, in + start_idx + FRAME_STRIDE*(row_before_offset+HDR_OFFSET/2) + FRAME_STRIDE/2); + short_dat[1] = vload8(0, in + start_idx + FRAME_STRIDE*(1+HDR_OFFSET/2) + FRAME_STRIDE/2); + short_dat[2] = vload8(0, in + start_idx + FRAME_STRIDE*(2+HDR_OFFSET/2) + FRAME_STRIDE/2); + short_dat[3] = vload8(0, in + start_idx + FRAME_STRIDE*(row_after_offset+HDR_OFFSET/2) + FRAME_STRIDE/2); + #if BIT_DEPTH == 10 + uchar short_extra_dat[4]; + if (!aligned10) { + short_extra_dat[0] = in[start_idx + FRAME_STRIDE*(row_before_offset+HDR_OFFSET/2) + FRAME_STRIDE/2 + 8]; + short_extra_dat[1] = in[start_idx + FRAME_STRIDE*(1+HDR_OFFSET/2) + FRAME_STRIDE/2 + 8]; + short_extra_dat[2] = in[start_idx + FRAME_STRIDE*(2+HDR_OFFSET/2) + FRAME_STRIDE/2 + 8]; + short_extra_dat[3] = in[start_idx + FRAME_STRIDE*(row_after_offset+HDR_OFFSET/2) + FRAME_STRIDE/2 + 8]; + } + #endif + #endif + + // parse into floats 0.0-1.0 + float4 v_rows[4]; + #if BIT_DEPTH == 10 + // for now it's always HDR + int4 parsed = parse_10bit(dat[0], extra_dat[0], aligned10); + int4 short_parsed = parse_10bit(short_dat[0], short_extra_dat[0], aligned10); + v_rows[ROW_READ_ORDER[0]] = normalize_pv_hdr(parsed, short_parsed, vignette_factor, expo_time); + parsed = parse_10bit(dat[1], extra_dat[1], aligned10); + short_parsed = parse_10bit(short_dat[1], short_extra_dat[1], aligned10); + v_rows[ROW_READ_ORDER[1]] = normalize_pv_hdr(parsed, short_parsed, vignette_factor, expo_time); + parsed = parse_10bit(dat[2], extra_dat[2], aligned10); + short_parsed = parse_10bit(short_dat[2], short_extra_dat[2], aligned10); + v_rows[ROW_READ_ORDER[2]] = normalize_pv_hdr(parsed, short_parsed, vignette_factor, expo_time); + parsed = parse_10bit(dat[3], extra_dat[3], aligned10); + short_parsed = parse_10bit(short_dat[3], short_extra_dat[3], aligned10); + v_rows[ROW_READ_ORDER[3]] = normalize_pv_hdr(parsed, short_parsed, vignette_factor, expo_time); + #else + // no HDR here + int4 parsed = parse_12bit(dat[0]); + v_rows[ROW_READ_ORDER[0]] = normalize_pv(parsed, vignette_factor); + parsed = parse_12bit(dat[1]); + v_rows[ROW_READ_ORDER[1]] = normalize_pv(parsed, vignette_factor); + parsed = parse_12bit(dat[2]); + v_rows[ROW_READ_ORDER[2]] = normalize_pv(parsed, vignette_factor); + parsed = parse_12bit(dat[3]); + v_rows[ROW_READ_ORDER[3]] = normalize_pv(parsed, vignette_factor); + #endif + + // mirror padding + if (gid_x == 0) { + v_rows[0].s0 = v_rows[0].s2; + v_rows[1].s0 = v_rows[1].s2; + v_rows[2].s0 = v_rows[2].s2; + v_rows[3].s0 = v_rows[3].s2; + } else if (gid_x == RGB_WIDTH/2 - 1) { + v_rows[0].s3 = v_rows[0].s1; + v_rows[1].s3 = v_rows[1].s1; + v_rows[2].s3 = v_rows[2].s1; + v_rows[3].s3 = v_rows[3].s1; + } + + // debayering + // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf + const float k01 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[0].s2, v_rows[1].s1); + const float k02 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1); + const float k03 = get_k(v_rows[2].s0, v_rows[1].s1, v_rows[2].s2, v_rows[1].s1); + const float k04 = get_k(v_rows[0].s0, v_rows[1].s1, v_rows[2].s0, v_rows[1].s1); + rgb_tmp.x = (k02*v_rows[1].s2+k04*v_rows[1].s0)/(k02+k04); // R_G1 + rgb_tmp.y = v_rows[1].s1; // G1(R) + rgb_tmp.z = (k01*v_rows[0].s1+k03*v_rows[2].s1)/(k01+k03); // B_G1 + rgb_out[RGB_WRITE_ORDER[0]] = convert_uchar3_sat(apply_gamma(color_correct(clamp(rgb_tmp, 0.0, 1.0)), expo_time) * 255.0); + + const float k11 = get_k(v_rows[0].s1, v_rows[2].s1, v_rows[0].s3, v_rows[2].s3); + const float k12 = get_k(v_rows[0].s2, v_rows[1].s1, v_rows[1].s3, v_rows[2].s2); + const float k13 = get_k(v_rows[0].s1, v_rows[0].s3, v_rows[2].s1, v_rows[2].s3); + const float k14 = get_k(v_rows[0].s2, v_rows[1].s3, v_rows[2].s2, v_rows[1].s1); + rgb_tmp.x = v_rows[1].s2; // R + rgb_tmp.y = (k11*(v_rows[0].s2+v_rows[2].s2)*0.5+k13*(v_rows[1].s3+v_rows[1].s1)*0.5)/(k11+k13); // G_R + rgb_tmp.z = (k12*(v_rows[0].s3+v_rows[2].s1)*0.5+k14*(v_rows[0].s1+v_rows[2].s3)*0.5)/(k12+k14); // B_R + rgb_out[RGB_WRITE_ORDER[1]] = convert_uchar3_sat(apply_gamma(color_correct(clamp(rgb_tmp, 0.0, 1.0)), expo_time) * 255.0); + + const float k21 = get_k(v_rows[1].s0, v_rows[3].s0, v_rows[1].s2, v_rows[3].s2); + const float k22 = get_k(v_rows[1].s1, v_rows[2].s0, v_rows[2].s2, v_rows[3].s1); + const float k23 = get_k(v_rows[1].s0, v_rows[1].s2, v_rows[3].s0, v_rows[3].s2); + const float k24 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s0); + rgb_tmp.x = (k22*(v_rows[1].s2+v_rows[3].s0)*0.5+k24*(v_rows[1].s0+v_rows[3].s2)*0.5)/(k22+k24); // R_B + rgb_tmp.y = (k21*(v_rows[1].s1+v_rows[3].s1)*0.5+k23*(v_rows[2].s2+v_rows[2].s0)*0.5)/(k21+k23); // G_B + rgb_tmp.z = v_rows[2].s1; // B + rgb_out[RGB_WRITE_ORDER[2]] = convert_uchar3_sat(apply_gamma(color_correct(clamp(rgb_tmp, 0.0, 1.0)), expo_time) * 255.0); + + const float k31 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[1].s3, v_rows[2].s2); + const float k32 = get_k(v_rows[1].s3, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2); + const float k33 = get_k(v_rows[3].s1, v_rows[2].s2, v_rows[3].s3, v_rows[2].s2); + const float k34 = get_k(v_rows[1].s1, v_rows[2].s2, v_rows[3].s1, v_rows[2].s2); + rgb_tmp.x = (k31*v_rows[1].s2+k33*v_rows[3].s2)/(k31+k33); // R_G2 + rgb_tmp.y = v_rows[2].s2; // G2(B) + rgb_tmp.z = (k32*v_rows[2].s3+k34*v_rows[2].s1)/(k32+k34); // B_G2 + rgb_out[RGB_WRITE_ORDER[3]] = convert_uchar3_sat(apply_gamma(color_correct(clamp(rgb_tmp, 0.0, 1.0)), expo_time) * 255.0); + + // rgb2yuv(nv12) + uchar2 yy = (uchar2)( + RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), + RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) + ); + vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2)); + yy = (uchar2)( + RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), + RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) + ); + vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2)); + + const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); + const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); + const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); + uchar2 uv = (uchar2)( + RGB_TO_U(ar, ag, ab), + RGB_TO_V(ar, ag, ab) + ); + vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2)); +} diff --git a/system/camerad/main.cc b/system/camerad/main.cc index 19de21c9bb..b86b4f57ff 100644 --- a/system/camerad/main.cc +++ b/system/camerad/main.cc @@ -12,8 +12,7 @@ int main(int argc, char *argv[]) { return 0; } - int ret; - ret = util::set_realtime_priority(53); + int ret = util::set_realtime_priority(53); assert(ret == 0); ret = util::set_core_affinity({6}); assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 1ca4b3f1ad..9b688389c4 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -79,15 +79,16 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r AR0231::AR0231() { image_sensor = cereal::FrameData::ImageSensor::AR0231; + pixel_size_mm = 0.003; data_word = true; - frame_width = FRAME_WIDTH; - frame_height = FRAME_HEIGHT; - frame_stride = FRAME_STRIDE; + frame_width = 1928; + frame_height = 1208; + frame_stride = (frame_width * 12 / 8) + 4; extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT; registers_offset = 0; frame_offset = AR0231_REGISTERS_HEIGHT; - stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT; + stats_offset = AR0231_REGISTERS_HEIGHT + frame_height; start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231)); init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231)); diff --git a/system/camerad/sensors/ar0231_cl.h b/system/camerad/sensors/ar0231_cl.h new file mode 100644 index 0000000000..8e96bbce5b --- /dev/null +++ b/system/camerad/sensors/ar0231_cl.h @@ -0,0 +1,33 @@ +#if SENSOR_ID == 1 + +#define BIT_DEPTH 12 +#define PV_MAX 4096 +#define BLACK_LVL 168 +#define VIGNETTE_RSZ 1.0f + +float4 normalize_pv(int4 parsed, float vignette_factor) { + float4 pv = (convert_float4(parsed) - BLACK_LVL) / (PV_MAX - BLACK_LVL); + return clamp(pv*vignette_factor, 0.0, 1.0); +} + +float3 color_correct(float3 rgb) { + float3 corrected = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); + corrected += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); + corrected += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); + return corrected; +} + +float3 apply_gamma(float3 rgb, int expo_time) { + // tone mapping params + const float gamma_k = 0.75; + const float gamma_b = 0.125; + const float mp = 0.01; // ideally midpoint should be adaptive + const float rk = 9 - 100*mp; + + // poly approximation for s curve + return (rgb > mp) ? + ((rk * (rgb-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(rgb-mp))) + gamma_k*mp + gamma_b) : + ((rk * (rgb-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(rgb-mp))) + gamma_k*mp + gamma_b); +} + +#endif \ No newline at end of file diff --git a/system/camerad/sensors/os04c10.cc b/system/camerad/sensors/os04c10.cc index 449e06be83..97a317407a 100644 --- a/system/camerad/sensors/os04c10.cc +++ b/system/camerad/sensors/os04c10.cc @@ -10,30 +10,23 @@ const float sensor_analog_gains_OS04C10[] = { 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; const uint32_t os04c10_analog_gains_reg[] = { - 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, - 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, - 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, - 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, - 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; - -const uint32_t VS_TIME_MIN_OS04C10 = 1; -//const uint32_t VS_TIME_MAX_OS04C10 = 34; // vs < 35 + 0x080, 0x088, 0x090, 0x098, 0x0A0, 0x0A8, 0x0B0, 0x0B8, 0x0C0, 0x0C8, 0x0D8, + 0x0E8, 0x0F8, 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, + 0x190, 0x1B0, 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, + 0x2E0, 0x300, 0x320, 0x340, 0x380, 0x3C0, 0x400, 0x440, 0x480, 0x4C0, 0x500, + 0x540, 0x580, 0x5C0, 0x600, 0x640, 0x680, 0x6C0, 0x700, 0x740, 0x780, 0x7C0}; } // namespace OS04C10::OS04C10() { image_sensor = cereal::FrameData::ImageSensor::OS04C10; + pixel_size_mm = 0.002; data_word = false; - frame_width = 1920; - frame_height = 1080; - frame_stride = (1920*10/8); - - /* - frame_width = 0xa80; - frame_height = 0x5f0; - frame_stride = 0xd20; - */ + hdr_offset = 64 * 2 + 8; // stagger + frame_width = 2688; + frame_height = 1520 * 2 + hdr_offset; + frame_stride = (frame_width * 10 / 8); // no alignment extra_height = 0; frame_offset = 0; @@ -46,13 +39,13 @@ OS04C10::OS04C10() { frame_data_type = 0x2b; mclk_frequency = 24000000; // Hz - dc_gain_factor = 7.32; + dc_gain_factor = 1; dc_gain_min_weight = 1; // always on is fine dc_gain_max_weight = 1; dc_gain_on_grey = 0.9; dc_gain_off_grey = 1.0; - exposure_time_min = 2; // 1x - exposure_time_max = 2016; + exposure_time_min = 2; + exposure_time_max = 2400; analog_gain_min_idx = 0x0; analog_gain_rec_idx = 0x0; // 1x analog_gain_max_idx = 0x36; @@ -62,30 +55,19 @@ OS04C10::OS04C10() { for (int i = 0; i <= analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_OS04C10[i]; } - min_ev = (exposure_time_min + VS_TIME_MIN_OS04C10) * sensor_analog_gains[analog_gain_min_idx]; + min_ev = (exposure_time_min) * sensor_analog_gains[analog_gain_min_idx]; max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = 0.01; } std::vector OS04C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { - // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD - uint32_t hcg_time = exposure_time; - //uint32_t lcg_time = hcg_time; - //uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OS04C10) / 3), exposure_time_max + VS_TIME_MAX_OS04C10); - //uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OS04C10), VS_TIME_MAX_OS04C10); - + uint32_t long_time = exposure_time; uint32_t real_gain = os04c10_analog_gains_reg[new_exp_g]; - hcg_time = 100; - real_gain = 0x320; - return { - {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, - //{0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, - //{0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, - //{0x35c2, vs_time&0xFF}, - + {0x3501, long_time>>8}, {0x3502, long_time&0xFF}, {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + {0x350c, real_gain>>8}, {0x350d, real_gain&0xFF}, }; } @@ -100,6 +82,6 @@ float OS04C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, floa score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * - std::abs(exp_g_idx - gain_idx) * 5.0; + std::abs(exp_g_idx - gain_idx) * 3.0; return score; } diff --git a/system/camerad/sensors/os04c10_cl.h b/system/camerad/sensors/os04c10_cl.h new file mode 100644 index 0000000000..61775dcdc8 --- /dev/null +++ b/system/camerad/sensors/os04c10_cl.h @@ -0,0 +1,55 @@ +#if SENSOR_ID == 3 + +#define BGGR + +#define BIT_DEPTH 10 +#define PV_MAX10 1023 +#define PV_MAX16 65536 // gamma curve is calibrated to 16bit +#define BLACK_LVL 64 +#define VIGNETTE_RSZ 2.2545f + +float combine_dual_pvs(float lv, float sv, int expo_time) { + float svc = fmax(sv * expo_time, (float)(64 * (PV_MAX10 - BLACK_LVL))); + float svd = sv * fmin(expo_time, 8.0) / 8; + + if (expo_time > 64) { + if (lv < PV_MAX10 - BLACK_LVL) { + return lv / (PV_MAX16 - BLACK_LVL); + } else { + return (svc / 64) / (PV_MAX16 - BLACK_LVL); + } + } else { + if (lv > 32) { + return (lv * 64 / fmax(expo_time, 8.0)) / (PV_MAX16 - BLACK_LVL); + } else { + return svd / (PV_MAX16 - BLACK_LVL); + } + } +} + +float4 normalize_pv_hdr(int4 parsed, int4 short_parsed, float vignette_factor, int expo_time) { + float4 pl = convert_float4(parsed - BLACK_LVL); + float4 ps = convert_float4(short_parsed - BLACK_LVL); + float4 pv; + pv.s0 = combine_dual_pvs(pl.s0, ps.s0, expo_time); + pv.s1 = combine_dual_pvs(pl.s1, ps.s1, expo_time); + pv.s2 = combine_dual_pvs(pl.s2, ps.s2, expo_time); + pv.s3 = combine_dual_pvs(pl.s3, ps.s3, expo_time); + return clamp(pv*vignette_factor, 0.0, 1.0); +} + +float3 color_correct(float3 rgb) { + float3 corrected = rgb.x * (float3)(1.55361989, -0.268894615, -0.000593219); + corrected += rgb.y * (float3)(-0.421217301, 1.51883144, -0.69760146); + corrected += rgb.z * (float3)(-0.132402589, -0.249936825, 1.69819468); + return corrected; +} + +float3 apply_gamma(float3 rgb, int expo_time) { + float s = log2((float)expo_time); + if (s < 6) {s = fmin(12.0 - s, 9.0);} + // log function adaptive to number of bits + return clamp(log(1 + rgb*(PV_MAX16 - BLACK_LVL)) * (0.48*s*s - 12.92*s + 115.0) - (1.08*s*s - 29.2*s + 260.0), 0.0, 255.0) / 255.0; +} + +#endif diff --git a/system/camerad/sensors/os04c10_registers.h b/system/camerad/sensors/os04c10_registers.h index ad91a02950..91eb48b24f 100644 --- a/system/camerad/sensors/os04c10_registers.h +++ b/system/camerad/sensors/os04c10_registers.h @@ -4,69 +4,59 @@ const struct i2c_random_wr_payload start_reg_array_os04c10[] = {{0x100, 1}}; const struct i2c_random_wr_payload stop_reg_array_os04c10[] = {{0x100, 0}}; const struct i2c_random_wr_payload init_array_os04c10[] = { - // OS04C10_AA_00_02_17_wAO_1920x1080_MIPI728Mbps_Linear12bit_20FPS_4Lane_MCLK24MHz + // DP_2688X1520_NEWSTG_MIPI0776Mbps_30FPS_10BIT_FOURLANE {0x0103, 0x01}, + + // PLL {0x0301, 0x84}, {0x0303, 0x01}, - {0x0305, 0x5b}, + {0x0305, 0x61}, {0x0306, 0x01}, {0x0307, 0x17}, {0x0323, 0x04}, {0x0324, 0x01}, - {0x0325, 0x62}, + {0x0325, 0x7a}, + {0x3012, 0x06}, {0x3013, 0x02}, {0x3016, 0x72}, {0x3021, 0x03}, {0x3106, 0x21}, {0x3107, 0xa1}, - {0x3500, 0x00}, - {0x3501, 0x00}, - {0x3502, 0x40}, - {0x3503, 0x88}, - {0x3508, 0x07}, - {0x3509, 0xc0}, - {0x350a, 0x04}, - {0x350b, 0x00}, - {0x350c, 0x07}, - {0x350d, 0xc0}, - {0x350e, 0x04}, - {0x350f, 0x00}, - {0x3510, 0x00}, - {0x3511, 0x00}, - {0x3512, 0x20}, + + // ? {0x3624, 0x00}, {0x3625, 0x4c}, - {0x3660, 0x00}, + {0x3660, 0x04}, {0x3666, 0xa5}, {0x3667, 0xa5}, - {0x366a, 0x64}, + {0x366a, 0x54}, {0x3673, 0x0d}, {0x3672, 0x0d}, {0x3671, 0x0d}, {0x3670, 0x0d}, - {0x3685, 0x00}, + {0x3685, 0x0a}, {0x3694, 0x0d}, {0x3693, 0x0d}, {0x3692, 0x0d}, {0x3691, 0x0d}, {0x3696, 0x4c}, {0x3697, 0x4c}, - {0x3698, 0x40}, + {0x3698, 0x00}, {0x3699, 0x80}, - {0x369a, 0x18}, + {0x369a, 0x80}, {0x369b, 0x1f}, - {0x369c, 0x14}, + {0x369c, 0x1f}, {0x369d, 0x80}, {0x369e, 0x40}, {0x369f, 0x21}, {0x36a0, 0x12}, - {0x36a1, 0x5d}, + {0x36a1, 0xdd}, {0x36a2, 0x66}, {0x370a, 0x00}, - {0x370e, 0x0c}, + {0x370e, 0x00}, {0x3710, 0x00}, - {0x3713, 0x00}, + {0x3713, 0x04}, {0x3725, 0x02}, {0x372a, 0x03}, {0x3738, 0xce}, @@ -75,10 +65,10 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x374c, 0x00}, {0x374e, 0x00}, {0x3756, 0x00}, - {0x3757, 0x0e}, + {0x3757, 0x00}, {0x3767, 0x00}, {0x3771, 0x00}, - {0x377b, 0x20}, + {0x377b, 0x28}, {0x377c, 0x00}, {0x377d, 0x0c}, {0x3781, 0x03}, @@ -91,26 +81,29 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37ba, 0x03}, {0x37bb, 0x00}, {0x37bc, 0x04}, - {0x37be, 0x08}, + {0x37be, 0x26}, {0x37c4, 0x11}, {0x37c5, 0x80}, {0x37c6, 0x14}, - {0x37c7, 0x08}, + {0x37c7, 0xa8}, {0x37da, 0x11}, {0x381f, 0x08}, - {0x3829, 0x03}, + // {0x3829, 0x03}, + // {0x3832, 0x00}, {0x3881, 0x00}, {0x3888, 0x04}, {0x388b, 0x00}, {0x3c80, 0x10}, {0x3c86, 0x00}, - {0x3c8c, 0x20}, + // {0x3c8c, 0x20}, {0x3c9f, 0x01}, {0x3d85, 0x1b}, {0x3d8c, 0x71}, {0x3d8d, 0xe2}, {0x3f00, 0x0b}, {0x3f06, 0x04}, + + // BLC {0x400a, 0x01}, {0x400b, 0x50}, {0x400e, 0x08}, @@ -128,18 +121,19 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x40b7, 0x00}, {0x40b8, 0x00}, {0x40b9, 0x00}, - {0x40ba, 0x00}, + {0x40ba, 0x01}, + {0x4301, 0x00}, {0x4303, 0x00}, {0x4502, 0x04}, {0x4503, 0x00}, {0x4504, 0x06}, {0x4506, 0x00}, - {0x4507, 0x64}, + {0x4507, 0x57}, {0x4803, 0x00}, {0x480c, 0x32}, - {0x480e, 0x00}, - {0x4813, 0x00}, + {0x480e, 0x04}, + {0x4813, 0xe4}, {0x4819, 0x70}, {0x481f, 0x30}, {0x4823, 0x3f}, @@ -156,27 +150,33 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x4d0b, 0x01}, {0x4e00, 0x2a}, {0x4e0d, 0x00}, - {0x5001, 0x09}, + + // ISP + {0x5001, 0x00}, {0x5004, 0x00}, {0x5080, 0x04}, - {0x5036, 0x00}, + {0x5036, 0x80}, {0x5180, 0x70}, {0x5181, 0x10}, + + // DPC {0x520a, 0x03}, {0x520b, 0x06}, {0x520c, 0x0c}, + {0x580b, 0x0f}, {0x580d, 0x00}, {0x580f, 0x00}, {0x5820, 0x00}, {0x5821, 0x00}, + {0x301c, 0xf8}, {0x301e, 0xb4}, - {0x301f, 0xd0}, + {0x301f, 0xf0}, {0x3022, 0x01}, {0x3109, 0xe7}, {0x3600, 0x00}, - {0x3610, 0x65}, + {0x3610, 0x75}, {0x3611, 0x85}, {0x3613, 0x3a}, {0x3615, 0x60}, @@ -186,10 +186,10 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x3661, 0x04}, {0x3664, 0x70}, {0x3665, 0x00}, - {0x3681, 0xa6}, - {0x3682, 0x53}, - {0x3683, 0x2a}, - {0x3684, 0x15}, + {0x3681, 0x80}, + {0x3682, 0x40}, + {0x3683, 0x21}, + {0x3684, 0x12}, {0x3700, 0x2a}, {0x3701, 0x12}, {0x3703, 0x28}, @@ -198,7 +198,7 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x3709, 0x4a}, {0x370b, 0xa2}, {0x370c, 0x01}, - {0x370f, 0x04}, + {0x370f, 0x00}, {0x3714, 0x24}, {0x3716, 0x04}, {0x3719, 0x11}, @@ -233,66 +233,103 @@ const struct i2c_random_wr_payload init_array_os04c10[] = { {0x37c0, 0x11}, {0x37c2, 0x04}, {0x37cd, 0x19}, - {0x37e0, 0x08}, - {0x37e6, 0x04}, + // {0x37e0, 0x08}, + // {0x37e6, 0x04}, {0x37e5, 0x02}, - {0x37e1, 0x0c}, - {0x3737, 0x04}, + // {0x37e1, 0x0c}, + // {0x3737, 0x04}, {0x37d8, 0x02}, - {0x37e2, 0x10}, + // {0x37e2, 0x10}, {0x3739, 0x10}, {0x3662, 0x10}, - {0x37e4, 0x20}, - {0x37e3, 0x08}, + // {0x37e4, 0x20}, + // {0x37e3, 0x08}, {0x37d9, 0x08}, {0x4040, 0x00}, {0x4041, 0x07}, {0x4008, 0x02}, {0x4009, 0x0d}, - {0x3800, 0x01}, - {0x3801, 0x80}, - {0x3802, 0x00}, - {0x3803, 0xdc}, - {0x3804, 0x09}, - {0x3805, 0x0f}, - {0x3806, 0x05}, - {0x3807, 0x23}, - {0x3808, 0x07}, - {0x3809, 0x80}, - {0x380a, 0x04}, - {0x380b, 0x38}, - {0x380c, 0x04}, - {0x380d, 0x2e}, - {0x380e, 0x12}, - {0x380f, 0x70}, + + // FSIN + {0x3002, 0x22}, + {0x3663, 0x22}, + {0x368a, 0x04}, + {0x3822, 0x44}, + {0x3823, 0x00}, + {0x3829, 0x03}, + {0x3832, 0xf8}, + {0x382c, 0x00}, + {0x3844, 0x06}, + {0x3843, 0x00}, + {0x382a, 0x00}, + {0x382b, 0x0c}, + + // 2704x1536 -> 2688x1520 out + {0x3800, 0x00}, {0x3801, 0x00}, + {0x3802, 0x00}, {0x3803, 0x00}, + {0x3804, 0x0a}, {0x3805, 0x8f}, + {0x3806, 0x05}, {0x3807, 0xff}, + {0x3808, 0x0a}, {0x3809, 0x80}, + {0x380a, 0x05}, {0x380b, 0xf0}, {0x3811, 0x08}, {0x3813, 0x08}, {0x3814, 0x01}, {0x3815, 0x01}, {0x3816, 0x01}, {0x3817, 0x01}, - {0x3820, 0xB0}, - {0x3821, 0x00}, - {0x3880, 0x25}, + + {0x380c, 0x04}, {0x380d, 0x2e}, // HTS + {0x380e, 0x09}, {0x380f, 0xdb}, // VTS + + {0x3820, 0xb0}, + {0x3821, 0x04}, + {0x3880, 0x00}, {0x3882, 0x20}, {0x3c91, 0x0b}, {0x3c94, 0x45}, - {0x3cad, 0x00}, - {0x3cae, 0x00}, + // {0x3cad, 0x00}, + // {0x3cae, 0x00}, {0x4000, 0xf3}, {0x4001, 0x60}, {0x4003, 0x40}, {0x4300, 0xff}, {0x4302, 0x0f}, - {0x4305, 0x83}, + {0x4305, 0x93}, {0x4505, 0x84}, - {0x4809, 0x1e}, + {0x4809, 0x0e}, {0x480a, 0x04}, - {0x4837, 0x15}, + {0x4837, 0x14}, {0x4c00, 0x08}, {0x4c01, 0x08}, {0x4c04, 0x00}, {0x4c05, 0x00}, {0x5000, 0xf9}, - {0x3c8c, 0x10}, + // {0x0100, 0x01}, + // {0x320d, 0x00}, + // {0x3208, 0xa0}, + // {0x3822, 0x14}, + + // initialize exposure + {0x3503, 0x88}, + + // long + {0x3500, 0x00}, {0x3501, 0x00}, {0x3502, 0x10}, + {0x3508, 0x00}, {0x3509, 0x80}, + {0x350a, 0x04}, {0x350b, 0x00}, + + // short + {0x3510, 0x00}, {0x3511, 0x00}, {0x3512, 0x40}, + {0x350c, 0x00}, {0x350d, 0x80}, + {0x350e, 0x04}, {0x350f, 0x00}, + + // wb + // b + {0x5100, 0x06}, {0x5101, 0x7e}, + {0x5140, 0x06}, {0x5141, 0x7e}, + // g + {0x5102, 0x04}, {0x5103, 0x00}, + {0x5142, 0x04}, {0x5143, 0x00}, + // r + {0x5104, 0x08}, {0x5105, 0xd6}, + {0x5144, 0x08}, {0x5145, 0xd6}, }; diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index 1f0609820b..94efa0ea24 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -23,10 +23,11 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 OX03C10::OX03C10() { image_sensor = cereal::FrameData::ImageSensor::OX03C10; + pixel_size_mm = 0.003; data_word = false; - frame_width = FRAME_WIDTH; - frame_height = FRAME_HEIGHT; - frame_stride = FRAME_STRIDE; // (0xa80*12//8) + frame_width = 1928; + frame_height = 1208; + frame_stride = (frame_width * 12 / 8) + 4; extra_height = 16; // top 2 + bot 14 frame_offset = 2; diff --git a/system/camerad/cameras/real_debayer.cl b/system/camerad/sensors/ox03c10_cl.h similarity index 88% rename from system/camerad/cameras/real_debayer.cl rename to system/camerad/sensors/ox03c10_cl.h index e15a873d6d..21441902fc 100644 --- a/system/camerad/cameras/real_debayer.cl +++ b/system/camerad/sensors/ox03c10_cl.h @@ -1,197 +1,43 @@ -#define UV_WIDTH RGB_WIDTH / 2 -#define UV_HEIGHT RGB_HEIGHT / 2 +#if SENSOR_ID == 2 -#define RGB_TO_Y(r, g, b) ((((mul24(b, 13) + mul24(g, 65) + mul24(r, 33)) + 64) >> 7) + 16) -#define RGB_TO_U(r, g, b) ((mul24(b, 56) - mul24(g, 37) - mul24(r, 19) + 0x8080) >> 8) -#define RGB_TO_V(r, g, b) ((mul24(r, 56) - mul24(g, 47) - mul24(b, 9) + 0x8080) >> 8) -#define AVERAGE(x, y, z, w) ((convert_ushort(x) + convert_ushort(y) + convert_ushort(z) + convert_ushort(w) + 1) >> 1) - -float3 color_correct(float3 rgb) { - // color correction - #if IS_OX - float3 x = rgb.x * (float3)(1.5664815 , -0.29808738, -0.03973474); - x += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248); - x += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722); - #else - float3 x = rgb.x * (float3)(1.82717181, -0.31231438, 0.07307673); - x += rgb.y * (float3)(-0.5743977, 1.36858544, -0.53183455); - x += rgb.z * (float3)(-0.25277411, -0.05627105, 1.45875782); - #endif - - #if IS_OX - return -0.507089*exp(-12.54124638*x)+0.9655*powr(x,0.5)-0.472597*x+0.507089; - #else - // tone mapping params - const float gamma_k = 0.75; - const float gamma_b = 0.125; - const float mp = 0.01; // ideally midpoint should be adaptive - const float rk = 9 - 100*mp; - - // poly approximation for s curve - return (x > mp) ? - ((rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b) : - ((rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b); - #endif -} - -float get_vignetting_s(float r) { - if (r < 62500) { - return (1.0f + 0.0000008f*r); - } else if (r < 490000) { - return (0.9625f + 0.0000014f*r); - } else if (r < 1102500) { - return (1.26434f + 0.0000000000016f*r*r); - } else { - return (0.53503625f + 0.0000000000022f*r*r); - } -} +#define BIT_DEPTH 12 +#define BLACK_LVL 64 +#define VIGNETTE_RSZ 1.0f constant float ox03c10_lut[] = { - 0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05, - 1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05, - 3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05, - 6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04, - 1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04, - 1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04, - 2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04, - 4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04, - 9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03, - 1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03, - 3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03, - 7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02, - 1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02, - 6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01, - 1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01, - 3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00 + 0.0000e+00, 5.9488e-08, 1.1898e-07, 1.7846e-07, 2.3795e-07, 2.9744e-07, 3.5693e-07, 4.1642e-07, 4.7591e-07, 5.3539e-07, 5.9488e-07, 6.5437e-07, 7.1386e-07, 7.7335e-07, 8.3284e-07, 8.9232e-07, 9.5181e-07, 1.0113e-06, 1.0708e-06, 1.1303e-06, 1.1898e-06, 1.2493e-06, 1.3087e-06, 1.3682e-06, 1.4277e-06, 1.4872e-06, 1.5467e-06, 1.6062e-06, 1.6657e-06, 1.7252e-06, 1.7846e-06, 1.8441e-06, 1.9036e-06, 1.9631e-06, 2.0226e-06, 2.0821e-06, 2.1416e-06, 2.2011e-06, 2.2606e-06, 2.3200e-06, 2.3795e-06, 2.4390e-06, 2.4985e-06, 2.5580e-06, 2.6175e-06, 2.6770e-06, 2.7365e-06, 2.7959e-06, 2.8554e-06, 2.9149e-06, 2.9744e-06, 3.0339e-06, 3.0934e-06, 3.1529e-06, 3.2124e-06, 3.2719e-06, 3.3313e-06, 3.3908e-06, 3.4503e-06, 3.5098e-06, 3.5693e-06, 3.6288e-06, 3.6883e-06, 3.7478e-06, 3.8072e-06, 3.8667e-06, 3.9262e-06, 3.9857e-06, 4.0452e-06, 4.1047e-06, 4.1642e-06, 4.2237e-06, 4.2832e-06, 4.3426e-06, 4.4021e-06, 4.4616e-06, 4.5211e-06, 4.5806e-06, 4.6401e-06, 4.6996e-06, 4.7591e-06, 4.8185e-06, 4.8780e-06, 4.9375e-06, 4.9970e-06, 5.0565e-06, 5.1160e-06, 5.1755e-06, 5.2350e-06, 5.2945e-06, 5.3539e-06, 5.4134e-06, 5.4729e-06, 5.5324e-06, 5.5919e-06, 5.6514e-06, 5.7109e-06, 5.7704e-06, 5.8298e-06, 5.8893e-06, 5.9488e-06, 6.0083e-06, 6.0678e-06, 6.1273e-06, 6.1868e-06, 6.2463e-06, 6.3058e-06, 6.3652e-06, 6.4247e-06, 6.4842e-06, 6.5437e-06, 6.6032e-06, 6.6627e-06, 6.7222e-06, 6.7817e-06, 6.8411e-06, 6.9006e-06, 6.9601e-06, 7.0196e-06, 7.0791e-06, 7.1386e-06, 7.1981e-06, 7.2576e-06, 7.3171e-06, 7.3765e-06, 7.4360e-06, 7.4955e-06, 7.5550e-06, 7.6145e-06, 7.6740e-06, 7.7335e-06, 7.7930e-06, 7.8524e-06, 7.9119e-06, 7.9714e-06, 8.0309e-06, 8.0904e-06, 8.1499e-06, 8.2094e-06, 8.2689e-06, 8.3284e-06, 8.3878e-06, 8.4473e-06, 8.5068e-06, 8.5663e-06, 8.6258e-06, 8.6853e-06, 8.7448e-06, 8.8043e-06, 8.8637e-06, 8.9232e-06, 8.9827e-06, 9.0422e-06, 9.1017e-06, 9.1612e-06, 9.2207e-06, 9.2802e-06, 9.3397e-06, 9.3991e-06, 9.4586e-06, 9.5181e-06, 9.5776e-06, 9.6371e-06, 9.6966e-06, 9.7561e-06, 9.8156e-06, 9.8750e-06, 9.9345e-06, 9.9940e-06, 1.0054e-05, 1.0113e-05, 1.0172e-05, 1.0232e-05, 1.0291e-05, 1.0351e-05, 1.0410e-05, 1.0470e-05, 1.0529e-05, 1.0589e-05, 1.0648e-05, 1.0708e-05, 1.0767e-05, 1.0827e-05, 1.0886e-05, 1.0946e-05, 1.1005e-05, 1.1065e-05, 1.1124e-05, 1.1184e-05, 1.1243e-05, 1.1303e-05, 1.1362e-05, 1.1422e-05, 1.1481e-05, 1.1541e-05, 1.1600e-05, 1.1660e-05, 1.1719e-05, 1.1779e-05, 1.1838e-05, 1.1898e-05, 1.1957e-05, 1.2017e-05, 1.2076e-05, 1.2136e-05, 1.2195e-05, 1.2255e-05, 1.2314e-05, 1.2374e-05, 1.2433e-05, 1.2493e-05, 1.2552e-05, 1.2612e-05, 1.2671e-05, 1.2730e-05, 1.2790e-05, 1.2849e-05, 1.2909e-05, 1.2968e-05, 1.3028e-05, 1.3087e-05, 1.3147e-05, 1.3206e-05, 1.3266e-05, 1.3325e-05, 1.3385e-05, 1.3444e-05, 1.3504e-05, 1.3563e-05, 1.3623e-05, 1.3682e-05, 1.3742e-05, 1.3801e-05, 1.3861e-05, 1.3920e-05, 1.3980e-05, 1.4039e-05, 1.4099e-05, 1.4158e-05, 1.4218e-05, 1.4277e-05, 1.4337e-05, 1.4396e-05, 1.4456e-05, 1.4515e-05, 1.4575e-05, 1.4634e-05, 1.4694e-05, 1.4753e-05, 1.4813e-05, 1.4872e-05, 1.4932e-05, 1.4991e-05, 1.5051e-05, 1.5110e-05, 1.5169e-05, // NOLINT + 1.5229e-05, 1.5288e-05, 1.5348e-05, 1.5407e-05, 1.5467e-05, 1.5526e-05, 1.5586e-05, 1.5645e-05, 1.5705e-05, 1.5764e-05, 1.5824e-05, 1.5883e-05, 1.5943e-05, 1.6002e-05, 1.6062e-05, 1.6121e-05, 1.6181e-05, 1.6240e-05, 1.6300e-05, 1.6359e-05, 1.6419e-05, 1.6478e-05, 1.6538e-05, 1.6597e-05, 1.6657e-05, 1.6716e-05, 1.6776e-05, 1.6835e-05, 1.6895e-05, 1.6954e-05, 1.7014e-05, 1.7073e-05, 1.7133e-05, 1.7192e-05, 1.7252e-05, 1.7311e-05, 1.7371e-05, 1.7430e-05, 1.7490e-05, 1.7549e-05, 1.7609e-05, 1.7668e-05, 1.7727e-05, 1.7787e-05, 1.7846e-05, 1.7906e-05, 1.7965e-05, 1.8025e-05, 1.8084e-05, 1.8144e-05, 1.8203e-05, 1.8263e-05, 1.8322e-05, 1.8382e-05, 1.8441e-05, 1.8501e-05, 1.8560e-05, 1.8620e-05, 1.8679e-05, 1.8739e-05, 1.8798e-05, 1.8858e-05, 1.8917e-05, 1.8977e-05, 1.9036e-05, 1.9096e-05, 1.9155e-05, 1.9215e-05, 1.9274e-05, 1.9334e-05, 1.9393e-05, 1.9453e-05, 1.9512e-05, 1.9572e-05, 1.9631e-05, 1.9691e-05, 1.9750e-05, 1.9810e-05, 1.9869e-05, 1.9929e-05, 1.9988e-05, 2.0048e-05, 2.0107e-05, 2.0167e-05, 2.0226e-05, 2.0285e-05, 2.0345e-05, 2.0404e-05, 2.0464e-05, 2.0523e-05, 2.0583e-05, 2.0642e-05, 2.0702e-05, 2.0761e-05, 2.0821e-05, 2.0880e-05, 2.0940e-05, 2.0999e-05, 2.1059e-05, 2.1118e-05, 2.1178e-05, 2.1237e-05, 2.1297e-05, 2.1356e-05, 2.1416e-05, 2.1475e-05, 2.1535e-05, 2.1594e-05, 2.1654e-05, 2.1713e-05, 2.1773e-05, 2.1832e-05, 2.1892e-05, 2.1951e-05, 2.2011e-05, 2.2070e-05, 2.2130e-05, 2.2189e-05, 2.2249e-05, 2.2308e-05, 2.2368e-05, 2.2427e-05, 2.2487e-05, 2.2546e-05, 2.2606e-05, 2.2665e-05, 2.2725e-05, 2.2784e-05, 2.2843e-05, 2.2903e-05, 2.2962e-05, 2.3022e-05, 2.3081e-05, 2.3141e-05, 2.3200e-05, 2.3260e-05, 2.3319e-05, 2.3379e-05, 2.3438e-05, 2.3498e-05, 2.3557e-05, 2.3617e-05, 2.3676e-05, 2.3736e-05, 2.3795e-05, 2.3855e-05, 2.3914e-05, 2.3974e-05, 2.4033e-05, 2.4093e-05, 2.4152e-05, 2.4212e-05, 2.4271e-05, 2.4331e-05, 2.4390e-05, 2.4450e-05, 2.4509e-05, 2.4569e-05, 2.4628e-05, 2.4688e-05, 2.4747e-05, 2.4807e-05, 2.4866e-05, 2.4926e-05, 2.4985e-05, 2.5045e-05, 2.5104e-05, 2.5164e-05, 2.5223e-05, 2.5282e-05, 2.5342e-05, 2.5401e-05, 2.5461e-05, 2.5520e-05, 2.5580e-05, 2.5639e-05, 2.5699e-05, 2.5758e-05, 2.5818e-05, 2.5877e-05, 2.5937e-05, 2.5996e-05, 2.6056e-05, 2.6115e-05, 2.6175e-05, 2.6234e-05, 2.6294e-05, 2.6353e-05, 2.6413e-05, 2.6472e-05, 2.6532e-05, 2.6591e-05, 2.6651e-05, 2.6710e-05, 2.6770e-05, 2.6829e-05, 2.6889e-05, 2.6948e-05, 2.7008e-05, 2.7067e-05, 2.7127e-05, 2.7186e-05, 2.7246e-05, 2.7305e-05, 2.7365e-05, 2.7424e-05, 2.7484e-05, 2.7543e-05, 2.7603e-05, 2.7662e-05, 2.7722e-05, 2.7781e-05, 2.7840e-05, 2.7900e-05, 2.7959e-05, 2.8019e-05, 2.8078e-05, 2.8138e-05, 2.8197e-05, 2.8257e-05, 2.8316e-05, 2.8376e-05, 2.8435e-05, 2.8495e-05, 2.8554e-05, 2.8614e-05, 2.8673e-05, 2.8733e-05, 2.8792e-05, 2.8852e-05, 2.8911e-05, 2.8971e-05, 2.9030e-05, 2.9090e-05, 2.9149e-05, 2.9209e-05, 2.9268e-05, 2.9328e-05, 2.9387e-05, 2.9447e-05, 2.9506e-05, 2.9566e-05, 2.9625e-05, 2.9685e-05, 2.9744e-05, 2.9804e-05, 2.9863e-05, 2.9923e-05, 2.9982e-05, 3.0042e-05, 3.0101e-05, 3.0161e-05, 3.0220e-05, 3.0280e-05, 3.0339e-05, 3.0398e-05, // NOLINT + 3.0458e-05, 3.0577e-05, 3.0697e-05, 3.0816e-05, 3.0936e-05, 3.1055e-05, 3.1175e-05, 3.1294e-05, 3.1414e-05, 3.1533e-05, 3.1652e-05, 3.1772e-05, 3.1891e-05, 3.2011e-05, 3.2130e-05, 3.2250e-05, 3.2369e-05, 3.2489e-05, 3.2608e-05, 3.2727e-05, 3.2847e-05, 3.2966e-05, 3.3086e-05, 3.3205e-05, 3.3325e-05, 3.3444e-05, 3.3563e-05, 3.3683e-05, 3.3802e-05, 3.3922e-05, 3.4041e-05, 3.4161e-05, 3.4280e-05, 3.4400e-05, 3.4519e-05, 3.4638e-05, 3.4758e-05, 3.4877e-05, 3.4997e-05, 3.5116e-05, 3.5236e-05, 3.5355e-05, 3.5475e-05, 3.5594e-05, 3.5713e-05, 3.5833e-05, 3.5952e-05, 3.6072e-05, 3.6191e-05, 3.6311e-05, 3.6430e-05, 3.6550e-05, 3.6669e-05, 3.6788e-05, 3.6908e-05, 3.7027e-05, 3.7147e-05, 3.7266e-05, 3.7386e-05, 3.7505e-05, 3.7625e-05, 3.7744e-05, 3.7863e-05, 3.7983e-05, 3.8102e-05, 3.8222e-05, 3.8341e-05, 3.8461e-05, 3.8580e-05, 3.8700e-05, 3.8819e-05, 3.8938e-05, 3.9058e-05, 3.9177e-05, 3.9297e-05, 3.9416e-05, 3.9536e-05, 3.9655e-05, 3.9775e-05, 3.9894e-05, 4.0013e-05, 4.0133e-05, 4.0252e-05, 4.0372e-05, 4.0491e-05, 4.0611e-05, 4.0730e-05, 4.0850e-05, 4.0969e-05, 4.1088e-05, 4.1208e-05, 4.1327e-05, 4.1447e-05, 4.1566e-05, 4.1686e-05, 4.1805e-05, 4.1925e-05, 4.2044e-05, 4.2163e-05, 4.2283e-05, 4.2402e-05, 4.2522e-05, 4.2641e-05, 4.2761e-05, 4.2880e-05, 4.2999e-05, 4.3119e-05, 4.3238e-05, 4.3358e-05, 4.3477e-05, 4.3597e-05, 4.3716e-05, 4.3836e-05, 4.3955e-05, 4.4074e-05, 4.4194e-05, 4.4313e-05, 4.4433e-05, 4.4552e-05, 4.4672e-05, 4.4791e-05, 4.4911e-05, 4.5030e-05, 4.5149e-05, 4.5269e-05, 4.5388e-05, 4.5508e-05, 4.5627e-05, 4.5747e-05, 4.5866e-05, 4.5986e-05, 4.6105e-05, 4.6224e-05, 4.6344e-05, 4.6463e-05, 4.6583e-05, 4.6702e-05, 4.6822e-05, 4.6941e-05, 4.7061e-05, 4.7180e-05, 4.7299e-05, 4.7419e-05, 4.7538e-05, 4.7658e-05, 4.7777e-05, 4.7897e-05, 4.8016e-05, 4.8136e-05, 4.8255e-05, 4.8374e-05, 4.8494e-05, 4.8613e-05, 4.8733e-05, 4.8852e-05, 4.8972e-05, 4.9091e-05, 4.9211e-05, 4.9330e-05, 4.9449e-05, 4.9569e-05, 4.9688e-05, 4.9808e-05, 4.9927e-05, 5.0047e-05, 5.0166e-05, 5.0286e-05, 5.0405e-05, 5.0524e-05, 5.0644e-05, 5.0763e-05, 5.0883e-05, 5.1002e-05, 5.1122e-05, 5.1241e-05, 5.1361e-05, 5.1480e-05, 5.1599e-05, 5.1719e-05, 5.1838e-05, 5.1958e-05, 5.2077e-05, 5.2197e-05, 5.2316e-05, 5.2435e-05, 5.2555e-05, 5.2674e-05, 5.2794e-05, 5.2913e-05, 5.3033e-05, 5.3152e-05, 5.3272e-05, 5.3391e-05, 5.3510e-05, 5.3630e-05, 5.3749e-05, 5.3869e-05, 5.3988e-05, 5.4108e-05, 5.4227e-05, 5.4347e-05, 5.4466e-05, 5.4585e-05, 5.4705e-05, 5.4824e-05, 5.4944e-05, 5.5063e-05, 5.5183e-05, 5.5302e-05, 5.5422e-05, 5.5541e-05, 5.5660e-05, 5.5780e-05, 5.5899e-05, 5.6019e-05, 5.6138e-05, 5.6258e-05, 5.6377e-05, 5.6497e-05, 5.6616e-05, 5.6735e-05, 5.6855e-05, 5.6974e-05, 5.7094e-05, 5.7213e-05, 5.7333e-05, 5.7452e-05, 5.7572e-05, 5.7691e-05, 5.7810e-05, 5.7930e-05, 5.8049e-05, 5.8169e-05, 5.8288e-05, 5.8408e-05, 5.8527e-05, 5.8647e-05, 5.8766e-05, 5.8885e-05, 5.9005e-05, 5.9124e-05, 5.9244e-05, 5.9363e-05, 5.9483e-05, 5.9602e-05, 5.9722e-05, 5.9841e-05, 5.9960e-05, 6.0080e-05, 6.0199e-05, 6.0319e-05, 6.0438e-05, 6.0558e-05, 6.0677e-05, 6.0797e-05, 6.0916e-05, // NOLINT + 6.1154e-05, 6.1392e-05, 6.1631e-05, 6.1869e-05, 6.2107e-05, 6.2345e-05, 6.2583e-05, 6.2821e-05, 6.3060e-05, 6.3298e-05, 6.3536e-05, 6.3774e-05, 6.4012e-05, 6.4251e-05, 6.4489e-05, 6.4727e-05, 6.4965e-05, 6.5203e-05, 6.5441e-05, 6.5680e-05, 6.5918e-05, 6.6156e-05, 6.6394e-05, 6.6632e-05, 6.6871e-05, 6.7109e-05, 6.7347e-05, 6.7585e-05, 6.7823e-05, 6.8062e-05, 6.8300e-05, 6.8538e-05, 6.8776e-05, 6.9014e-05, 6.9252e-05, 6.9491e-05, 6.9729e-05, 6.9967e-05, 7.0205e-05, 7.0443e-05, 7.0682e-05, 7.0920e-05, 7.1158e-05, 7.1396e-05, 7.1634e-05, 7.1872e-05, 7.2111e-05, 7.2349e-05, 7.2587e-05, 7.2825e-05, 7.3063e-05, 7.3302e-05, 7.3540e-05, 7.3778e-05, 7.4016e-05, 7.4254e-05, 7.4493e-05, 7.4731e-05, 7.4969e-05, 7.5207e-05, 7.5445e-05, 7.5683e-05, 7.5922e-05, 7.6160e-05, 7.6398e-05, 7.6636e-05, 7.6874e-05, 7.7113e-05, 7.7351e-05, 7.7589e-05, 7.7827e-05, 7.8065e-05, 7.8304e-05, 7.8542e-05, 7.8780e-05, 7.9018e-05, 7.9256e-05, 7.9494e-05, 7.9733e-05, 7.9971e-05, 8.0209e-05, 8.0447e-05, 8.0685e-05, 8.0924e-05, 8.1162e-05, 8.1400e-05, 8.1638e-05, 8.1876e-05, 8.2114e-05, 8.2353e-05, 8.2591e-05, 8.2829e-05, 8.3067e-05, 8.3305e-05, 8.3544e-05, 8.3782e-05, 8.4020e-05, 8.4258e-05, 8.4496e-05, 8.4735e-05, 8.4973e-05, 8.5211e-05, 8.5449e-05, 8.5687e-05, 8.5925e-05, 8.6164e-05, 8.6402e-05, 8.6640e-05, 8.6878e-05, 8.7116e-05, 8.7355e-05, 8.7593e-05, 8.7831e-05, 8.8069e-05, 8.8307e-05, 8.8545e-05, 8.8784e-05, 8.9022e-05, 8.9260e-05, 8.9498e-05, 8.9736e-05, 8.9975e-05, 9.0213e-05, 9.0451e-05, 9.0689e-05, 9.0927e-05, 9.1166e-05, 9.1404e-05, 9.1642e-05, 9.1880e-05, 9.2118e-05, 9.2356e-05, 9.2595e-05, 9.2833e-05, 9.3071e-05, 9.3309e-05, 9.3547e-05, 9.3786e-05, 9.4024e-05, 9.4262e-05, 9.4500e-05, 9.4738e-05, 9.4977e-05, 9.5215e-05, 9.5453e-05, 9.5691e-05, 9.5929e-05, 9.6167e-05, 9.6406e-05, 9.6644e-05, 9.6882e-05, 9.7120e-05, 9.7358e-05, 9.7597e-05, 9.7835e-05, 9.8073e-05, 9.8311e-05, 9.8549e-05, 9.8787e-05, 9.9026e-05, 9.9264e-05, 9.9502e-05, 9.9740e-05, 9.9978e-05, 1.0022e-04, 1.0045e-04, 1.0069e-04, 1.0093e-04, 1.0117e-04, 1.0141e-04, 1.0165e-04, 1.0188e-04, 1.0212e-04, 1.0236e-04, 1.0260e-04, 1.0284e-04, 1.0307e-04, 1.0331e-04, 1.0355e-04, 1.0379e-04, 1.0403e-04, 1.0427e-04, 1.0450e-04, 1.0474e-04, 1.0498e-04, 1.0522e-04, 1.0546e-04, 1.0569e-04, 1.0593e-04, 1.0617e-04, 1.0641e-04, 1.0665e-04, 1.0689e-04, 1.0712e-04, 1.0736e-04, 1.0760e-04, 1.0784e-04, 1.0808e-04, 1.0831e-04, 1.0855e-04, 1.0879e-04, 1.0903e-04, 1.0927e-04, 1.0951e-04, 1.0974e-04, 1.0998e-04, 1.1022e-04, 1.1046e-04, 1.1070e-04, 1.1093e-04, 1.1117e-04, 1.1141e-04, 1.1165e-04, 1.1189e-04, 1.1213e-04, 1.1236e-04, 1.1260e-04, 1.1284e-04, 1.1308e-04, 1.1332e-04, 1.1355e-04, 1.1379e-04, 1.1403e-04, 1.1427e-04, 1.1451e-04, 1.1475e-04, 1.1498e-04, 1.1522e-04, 1.1546e-04, 1.1570e-04, 1.1594e-04, 1.1618e-04, 1.1641e-04, 1.1665e-04, 1.1689e-04, 1.1713e-04, 1.1737e-04, 1.1760e-04, 1.1784e-04, 1.1808e-04, 1.1832e-04, 1.1856e-04, 1.1880e-04, 1.1903e-04, 1.1927e-04, 1.1951e-04, 1.1975e-04, 1.1999e-04, 1.2022e-04, 1.2046e-04, 1.2070e-04, 1.2094e-04, 1.2118e-04, 1.2142e-04, 1.2165e-04, 1.2189e-04, // NOLINT + 1.2213e-04, 1.2237e-04, 1.2261e-04, 1.2284e-04, 1.2308e-04, 1.2332e-04, 1.2356e-04, 1.2380e-04, 1.2404e-04, 1.2427e-04, 1.2451e-04, 1.2475e-04, 1.2499e-04, 1.2523e-04, 1.2546e-04, 1.2570e-04, 1.2594e-04, 1.2618e-04, 1.2642e-04, 1.2666e-04, 1.2689e-04, 1.2713e-04, 1.2737e-04, 1.2761e-04, 1.2785e-04, 1.2808e-04, 1.2832e-04, 1.2856e-04, 1.2880e-04, 1.2904e-04, 1.2928e-04, 1.2951e-04, 1.2975e-04, 1.2999e-04, 1.3023e-04, 1.3047e-04, 1.3070e-04, 1.3094e-04, 1.3118e-04, 1.3142e-04, 1.3166e-04, 1.3190e-04, 1.3213e-04, 1.3237e-04, 1.3261e-04, 1.3285e-04, 1.3309e-04, 1.3332e-04, 1.3356e-04, 1.3380e-04, 1.3404e-04, 1.3428e-04, 1.3452e-04, 1.3475e-04, 1.3499e-04, 1.3523e-04, 1.3547e-04, 1.3571e-04, 1.3594e-04, 1.3618e-04, 1.3642e-04, 1.3666e-04, 1.3690e-04, 1.3714e-04, 1.3737e-04, 1.3761e-04, 1.3785e-04, 1.3809e-04, 1.3833e-04, 1.3856e-04, 1.3880e-04, 1.3904e-04, 1.3928e-04, 1.3952e-04, 1.3976e-04, 1.3999e-04, 1.4023e-04, 1.4047e-04, 1.4071e-04, 1.4095e-04, 1.4118e-04, 1.4142e-04, 1.4166e-04, 1.4190e-04, 1.4214e-04, 1.4238e-04, 1.4261e-04, 1.4285e-04, 1.4309e-04, 1.4333e-04, 1.4357e-04, 1.4380e-04, 1.4404e-04, 1.4428e-04, 1.4452e-04, 1.4476e-04, 1.4500e-04, 1.4523e-04, 1.4547e-04, 1.4571e-04, 1.4595e-04, 1.4619e-04, 1.4642e-04, 1.4666e-04, 1.4690e-04, 1.4714e-04, 1.4738e-04, 1.4762e-04, 1.4785e-04, 1.4809e-04, 1.4833e-04, 1.4857e-04, 1.4881e-04, 1.4904e-04, 1.4928e-04, 1.4952e-04, 1.4976e-04, 1.5000e-04, 1.5024e-04, 1.5047e-04, 1.5071e-04, 1.5095e-04, 1.5119e-04, 1.5143e-04, 1.5166e-04, 1.5190e-04, 1.5214e-04, 1.5238e-04, 1.5262e-04, 1.5286e-04, 1.5309e-04, 1.5333e-04, 1.5357e-04, 1.5381e-04, 1.5405e-04, 1.5428e-04, 1.5452e-04, 1.5476e-04, 1.5500e-04, 1.5524e-04, 1.5548e-04, 1.5571e-04, 1.5595e-04, 1.5619e-04, 1.5643e-04, 1.5667e-04, 1.5690e-04, 1.5714e-04, 1.5738e-04, 1.5762e-04, 1.5786e-04, 1.5810e-04, 1.5833e-04, 1.5857e-04, 1.5881e-04, 1.5905e-04, 1.5929e-04, 1.5952e-04, 1.5976e-04, 1.6000e-04, 1.6024e-04, 1.6048e-04, 1.6072e-04, 1.6095e-04, 1.6119e-04, 1.6143e-04, 1.6167e-04, 1.6191e-04, 1.6214e-04, 1.6238e-04, 1.6262e-04, 1.6286e-04, 1.6310e-04, 1.6334e-04, 1.6357e-04, 1.6381e-04, 1.6405e-04, 1.6429e-04, 1.6453e-04, 1.6476e-04, 1.6500e-04, 1.6524e-04, 1.6548e-04, 1.6572e-04, 1.6596e-04, 1.6619e-04, 1.6643e-04, 1.6667e-04, 1.6691e-04, 1.6715e-04, 1.6738e-04, 1.6762e-04, 1.6786e-04, 1.6810e-04, 1.6834e-04, 1.6858e-04, 1.6881e-04, 1.6905e-04, 1.6929e-04, 1.6953e-04, 1.6977e-04, 1.7001e-04, 1.7024e-04, 1.7048e-04, 1.7072e-04, 1.7096e-04, 1.7120e-04, 1.7143e-04, 1.7167e-04, 1.7191e-04, 1.7215e-04, 1.7239e-04, 1.7263e-04, 1.7286e-04, 1.7310e-04, 1.7334e-04, 1.7358e-04, 1.7382e-04, 1.7405e-04, 1.7429e-04, 1.7453e-04, 1.7477e-04, 1.7501e-04, 1.7525e-04, 1.7548e-04, 1.7572e-04, 1.7596e-04, 1.7620e-04, 1.7644e-04, 1.7667e-04, 1.7691e-04, 1.7715e-04, 1.7739e-04, 1.7763e-04, 1.7787e-04, 1.7810e-04, 1.7834e-04, 1.7858e-04, 1.7882e-04, 1.7906e-04, 1.7929e-04, 1.7953e-04, 1.7977e-04, 1.8001e-04, 1.8025e-04, 1.8049e-04, 1.8072e-04, 1.8096e-04, 1.8120e-04, 1.8144e-04, 1.8168e-04, 1.8191e-04, 1.8215e-04, 1.8239e-04, 1.8263e-04, 1.8287e-04, // NOLINT + 1.8311e-04, 1.8334e-04, 1.8358e-04, 1.8382e-04, 1.8406e-04, 1.8430e-04, 1.8453e-04, 1.8477e-04, 1.8501e-04, 1.8525e-04, 1.8549e-04, 1.8573e-04, 1.8596e-04, 1.8620e-04, 1.8644e-04, 1.8668e-04, 1.8692e-04, 1.8715e-04, 1.8739e-04, 1.8763e-04, 1.8787e-04, 1.8811e-04, 1.8835e-04, 1.8858e-04, 1.8882e-04, 1.8906e-04, 1.8930e-04, 1.8954e-04, 1.8977e-04, 1.9001e-04, 1.9025e-04, 1.9049e-04, 1.9073e-04, 1.9097e-04, 1.9120e-04, 1.9144e-04, 1.9168e-04, 1.9192e-04, 1.9216e-04, 1.9239e-04, 1.9263e-04, 1.9287e-04, 1.9311e-04, 1.9335e-04, 1.9359e-04, 1.9382e-04, 1.9406e-04, 1.9430e-04, 1.9454e-04, 1.9478e-04, 1.9501e-04, 1.9525e-04, 1.9549e-04, 1.9573e-04, 1.9597e-04, 1.9621e-04, 1.9644e-04, 1.9668e-04, 1.9692e-04, 1.9716e-04, 1.9740e-04, 1.9763e-04, 1.9787e-04, 1.9811e-04, 1.9835e-04, 1.9859e-04, 1.9883e-04, 1.9906e-04, 1.9930e-04, 1.9954e-04, 1.9978e-04, 2.0002e-04, 2.0025e-04, 2.0049e-04, 2.0073e-04, 2.0097e-04, 2.0121e-04, 2.0145e-04, 2.0168e-04, 2.0192e-04, 2.0216e-04, 2.0240e-04, 2.0264e-04, 2.0287e-04, 2.0311e-04, 2.0335e-04, 2.0359e-04, 2.0383e-04, 2.0407e-04, 2.0430e-04, 2.0454e-04, 2.0478e-04, 2.0502e-04, 2.0526e-04, 2.0549e-04, 2.0573e-04, 2.0597e-04, 2.0621e-04, 2.0645e-04, 2.0669e-04, 2.0692e-04, 2.0716e-04, 2.0740e-04, 2.0764e-04, 2.0788e-04, 2.0811e-04, 2.0835e-04, 2.0859e-04, 2.0883e-04, 2.0907e-04, 2.0931e-04, 2.0954e-04, 2.0978e-04, 2.1002e-04, 2.1026e-04, 2.1050e-04, 2.1073e-04, 2.1097e-04, 2.1121e-04, 2.1145e-04, 2.1169e-04, 2.1193e-04, 2.1216e-04, 2.1240e-04, 2.1264e-04, 2.1288e-04, 2.1312e-04, 2.1335e-04, 2.1359e-04, 2.1383e-04, 2.1407e-04, 2.1431e-04, 2.1455e-04, 2.1478e-04, 2.1502e-04, 2.1526e-04, 2.1550e-04, 2.1574e-04, 2.1597e-04, 2.1621e-04, 2.1645e-04, 2.1669e-04, 2.1693e-04, 2.1717e-04, 2.1740e-04, 2.1764e-04, 2.1788e-04, 2.1812e-04, 2.1836e-04, 2.1859e-04, 2.1883e-04, 2.1907e-04, 2.1931e-04, 2.1955e-04, 2.1979e-04, 2.2002e-04, 2.2026e-04, 2.2050e-04, 2.2074e-04, 2.2098e-04, 2.2121e-04, 2.2145e-04, 2.2169e-04, 2.2193e-04, 2.2217e-04, 2.2241e-04, 2.2264e-04, 2.2288e-04, 2.2312e-04, 2.2336e-04, 2.2360e-04, 2.2383e-04, 2.2407e-04, 2.2431e-04, 2.2455e-04, 2.2479e-04, 2.2503e-04, 2.2526e-04, 2.2550e-04, 2.2574e-04, 2.2598e-04, 2.2622e-04, 2.2646e-04, 2.2669e-04, 2.2693e-04, 2.2717e-04, 2.2741e-04, 2.2765e-04, 2.2788e-04, 2.2812e-04, 2.2836e-04, 2.2860e-04, 2.2884e-04, 2.2908e-04, 2.2931e-04, 2.2955e-04, 2.2979e-04, 2.3003e-04, 2.3027e-04, 2.3050e-04, 2.3074e-04, 2.3098e-04, 2.3122e-04, 2.3146e-04, 2.3170e-04, 2.3193e-04, 2.3217e-04, 2.3241e-04, 2.3265e-04, 2.3289e-04, 2.3312e-04, 2.3336e-04, 2.3360e-04, 2.3384e-04, 2.3408e-04, 2.3432e-04, 2.3455e-04, 2.3479e-04, 2.3503e-04, 2.3527e-04, 2.3551e-04, 2.3574e-04, 2.3598e-04, 2.3622e-04, 2.3646e-04, 2.3670e-04, 2.3694e-04, 2.3717e-04, 2.3741e-04, 2.3765e-04, 2.3789e-04, 2.3813e-04, 2.3836e-04, 2.3860e-04, 2.3884e-04, 2.3908e-04, 2.3932e-04, 2.3956e-04, 2.3979e-04, 2.4003e-04, 2.4027e-04, 2.4051e-04, 2.4075e-04, 2.4098e-04, 2.4122e-04, 2.4146e-04, 2.4170e-04, 2.4194e-04, 2.4218e-04, 2.4241e-04, 2.4265e-04, 2.4289e-04, 2.4313e-04, 2.4337e-04, 2.4360e-04, 2.4384e-04, // NOLINT + 2.4480e-04, 2.4575e-04, 2.4670e-04, 2.4766e-04, 2.4861e-04, 2.4956e-04, 2.5052e-04, 2.5147e-04, 2.5242e-04, 2.5337e-04, 2.5433e-04, 2.5528e-04, 2.5623e-04, 2.5719e-04, 2.5814e-04, 2.5909e-04, 2.6005e-04, 2.6100e-04, 2.6195e-04, 2.6291e-04, 2.6386e-04, 2.6481e-04, 2.6577e-04, 2.6672e-04, 2.6767e-04, 2.6863e-04, 2.6958e-04, 2.7053e-04, 2.7149e-04, 2.7244e-04, 2.7339e-04, 2.7435e-04, 2.7530e-04, 2.7625e-04, 2.7720e-04, 2.7816e-04, 2.7911e-04, 2.8006e-04, 2.8102e-04, 2.8197e-04, 2.8292e-04, 2.8388e-04, 2.8483e-04, 2.8578e-04, 2.8674e-04, 2.8769e-04, 2.8864e-04, 2.8960e-04, 2.9055e-04, 2.9150e-04, 2.9246e-04, 2.9341e-04, 2.9436e-04, 2.9532e-04, 2.9627e-04, 2.9722e-04, 2.9818e-04, 2.9913e-04, 3.0008e-04, 3.0104e-04, 3.0199e-04, 3.0294e-04, 3.0389e-04, 3.0485e-04, 3.0580e-04, 3.0675e-04, 3.0771e-04, 3.0866e-04, 3.0961e-04, 3.1057e-04, 3.1152e-04, 3.1247e-04, 3.1343e-04, 3.1438e-04, 3.1533e-04, 3.1629e-04, 3.1724e-04, 3.1819e-04, 3.1915e-04, 3.2010e-04, 3.2105e-04, 3.2201e-04, 3.2296e-04, 3.2391e-04, 3.2487e-04, 3.2582e-04, 3.2677e-04, 3.2772e-04, 3.2868e-04, 3.2963e-04, 3.3058e-04, 3.3154e-04, 3.3249e-04, 3.3344e-04, 3.3440e-04, 3.3535e-04, 3.3630e-04, 3.3726e-04, 3.3821e-04, 3.3916e-04, 3.4012e-04, 3.4107e-04, 3.4202e-04, 3.4298e-04, 3.4393e-04, 3.4488e-04, 3.4584e-04, 3.4679e-04, 3.4774e-04, 3.4870e-04, 3.4965e-04, 3.5060e-04, 3.5156e-04, 3.5251e-04, 3.5346e-04, 3.5441e-04, 3.5537e-04, 3.5632e-04, 3.5727e-04, 3.5823e-04, 3.5918e-04, 3.6013e-04, 3.6109e-04, 3.6204e-04, 3.6299e-04, 3.6395e-04, 3.6490e-04, 3.6585e-04, 3.6681e-04, 3.6776e-04, 3.6871e-04, 3.6967e-04, 3.7062e-04, 3.7157e-04, 3.7253e-04, 3.7348e-04, 3.7443e-04, 3.7539e-04, 3.7634e-04, 3.7729e-04, 3.7825e-04, 3.7920e-04, 3.8015e-04, 3.8110e-04, 3.8206e-04, 3.8301e-04, 3.8396e-04, 3.8492e-04, 3.8587e-04, 3.8682e-04, 3.8778e-04, 3.8873e-04, 3.8968e-04, 3.9064e-04, 3.9159e-04, 3.9254e-04, 3.9350e-04, 3.9445e-04, 3.9540e-04, 3.9636e-04, 3.9731e-04, 3.9826e-04, 3.9922e-04, 4.0017e-04, 4.0112e-04, 4.0208e-04, 4.0303e-04, 4.0398e-04, 4.0493e-04, 4.0589e-04, 4.0684e-04, 4.0779e-04, 4.0875e-04, 4.0970e-04, 4.1065e-04, 4.1161e-04, 4.1256e-04, 4.1351e-04, 4.1447e-04, 4.1542e-04, 4.1637e-04, 4.1733e-04, 4.1828e-04, 4.1923e-04, 4.2019e-04, 4.2114e-04, 4.2209e-04, 4.2305e-04, 4.2400e-04, 4.2495e-04, 4.2591e-04, 4.2686e-04, 4.2781e-04, 4.2877e-04, 4.2972e-04, 4.3067e-04, 4.3162e-04, 4.3258e-04, 4.3353e-04, 4.3448e-04, 4.3544e-04, 4.3639e-04, 4.3734e-04, 4.3830e-04, 4.3925e-04, 4.4020e-04, 4.4116e-04, 4.4211e-04, 4.4306e-04, 4.4402e-04, 4.4497e-04, 4.4592e-04, 4.4688e-04, 4.4783e-04, 4.4878e-04, 4.4974e-04, 4.5069e-04, 4.5164e-04, 4.5260e-04, 4.5355e-04, 4.5450e-04, 4.5545e-04, 4.5641e-04, 4.5736e-04, 4.5831e-04, 4.5927e-04, 4.6022e-04, 4.6117e-04, 4.6213e-04, 4.6308e-04, 4.6403e-04, 4.6499e-04, 4.6594e-04, 4.6689e-04, 4.6785e-04, 4.6880e-04, 4.6975e-04, 4.7071e-04, 4.7166e-04, 4.7261e-04, 4.7357e-04, 4.7452e-04, 4.7547e-04, 4.7643e-04, 4.7738e-04, 4.7833e-04, 4.7929e-04, 4.8024e-04, 4.8119e-04, 4.8214e-04, 4.8310e-04, 4.8405e-04, 4.8500e-04, 4.8596e-04, 4.8691e-04, 4.8786e-04, // NOLINT + 4.8977e-04, 4.9168e-04, 4.9358e-04, 4.9549e-04, 4.9740e-04, 4.9931e-04, 5.0121e-04, 5.0312e-04, 5.0503e-04, 5.0693e-04, 5.0884e-04, 5.1075e-04, 5.1265e-04, 5.1456e-04, 5.1647e-04, 5.1837e-04, 5.2028e-04, 5.2219e-04, 5.2409e-04, 5.2600e-04, 5.2791e-04, 5.2982e-04, 5.3172e-04, 5.3363e-04, 5.3554e-04, 5.3744e-04, 5.3935e-04, 5.4126e-04, 5.4316e-04, 5.4507e-04, 5.4698e-04, 5.4888e-04, 5.5079e-04, 5.5270e-04, 5.5460e-04, 5.5651e-04, 5.5842e-04, 5.6033e-04, 5.6223e-04, 5.6414e-04, 5.6605e-04, 5.6795e-04, 5.6986e-04, 5.7177e-04, 5.7367e-04, 5.7558e-04, 5.7749e-04, 5.7939e-04, 5.8130e-04, 5.8321e-04, 5.8512e-04, 5.8702e-04, 5.8893e-04, 5.9084e-04, 5.9274e-04, 5.9465e-04, 5.9656e-04, 5.9846e-04, 6.0037e-04, 6.0228e-04, 6.0418e-04, 6.0609e-04, 6.0800e-04, 6.0990e-04, 6.1181e-04, 6.1372e-04, 6.1563e-04, 6.1753e-04, 6.1944e-04, 6.2135e-04, 6.2325e-04, 6.2516e-04, 6.2707e-04, 6.2897e-04, 6.3088e-04, 6.3279e-04, 6.3469e-04, 6.3660e-04, 6.3851e-04, 6.4041e-04, 6.4232e-04, 6.4423e-04, 6.4614e-04, 6.4804e-04, 6.4995e-04, 6.5186e-04, 6.5376e-04, 6.5567e-04, 6.5758e-04, 6.5948e-04, 6.6139e-04, 6.6330e-04, 6.6520e-04, 6.6711e-04, 6.6902e-04, 6.7092e-04, 6.7283e-04, 6.7474e-04, 6.7665e-04, 6.7855e-04, 6.8046e-04, 6.8237e-04, 6.8427e-04, 6.8618e-04, 6.8809e-04, 6.8999e-04, 6.9190e-04, 6.9381e-04, 6.9571e-04, 6.9762e-04, 6.9953e-04, 7.0143e-04, 7.0334e-04, 7.0525e-04, 7.0716e-04, 7.0906e-04, 7.1097e-04, 7.1288e-04, 7.1478e-04, 7.1669e-04, 7.1860e-04, 7.2050e-04, 7.2241e-04, 7.2432e-04, 7.2622e-04, 7.2813e-04, 7.3004e-04, 7.3195e-04, 7.3385e-04, 7.3576e-04, 7.3767e-04, 7.3957e-04, 7.4148e-04, 7.4339e-04, 7.4529e-04, 7.4720e-04, 7.4911e-04, 7.5101e-04, 7.5292e-04, 7.5483e-04, 7.5673e-04, 7.5864e-04, 7.6055e-04, 7.6246e-04, 7.6436e-04, 7.6627e-04, 7.6818e-04, 7.7008e-04, 7.7199e-04, 7.7390e-04, 7.7580e-04, 7.7771e-04, 7.7962e-04, 7.8152e-04, 7.8343e-04, 7.8534e-04, 7.8724e-04, 7.8915e-04, 7.9106e-04, 7.9297e-04, 7.9487e-04, 7.9678e-04, 7.9869e-04, 8.0059e-04, 8.0250e-04, 8.0441e-04, 8.0631e-04, 8.0822e-04, 8.1013e-04, 8.1203e-04, 8.1394e-04, 8.1585e-04, 8.1775e-04, 8.1966e-04, 8.2157e-04, 8.2348e-04, 8.2538e-04, 8.2729e-04, 8.2920e-04, 8.3110e-04, 8.3301e-04, 8.3492e-04, 8.3682e-04, 8.3873e-04, 8.4064e-04, 8.4254e-04, 8.4445e-04, 8.4636e-04, 8.4826e-04, 8.5017e-04, 8.5208e-04, 8.5399e-04, 8.5589e-04, 8.5780e-04, 8.5971e-04, 8.6161e-04, 8.6352e-04, 8.6543e-04, 8.6733e-04, 8.6924e-04, 8.7115e-04, 8.7305e-04, 8.7496e-04, 8.7687e-04, 8.7878e-04, 8.8068e-04, 8.8259e-04, 8.8450e-04, 8.8640e-04, 8.8831e-04, 8.9022e-04, 8.9212e-04, 8.9403e-04, 8.9594e-04, 8.9784e-04, 8.9975e-04, 9.0166e-04, 9.0356e-04, 9.0547e-04, 9.0738e-04, 9.0929e-04, 9.1119e-04, 9.1310e-04, 9.1501e-04, 9.1691e-04, 9.1882e-04, 9.2073e-04, 9.2263e-04, 9.2454e-04, 9.2645e-04, 9.2835e-04, 9.3026e-04, 9.3217e-04, 9.3407e-04, 9.3598e-04, 9.3789e-04, 9.3980e-04, 9.4170e-04, 9.4361e-04, 9.4552e-04, 9.4742e-04, 9.4933e-04, 9.5124e-04, 9.5314e-04, 9.5505e-04, 9.5696e-04, 9.5886e-04, 9.6077e-04, 9.6268e-04, 9.6458e-04, 9.6649e-04, 9.6840e-04, 9.7031e-04, 9.7221e-04, 9.7412e-04, 9.7603e-04, // NOLINT + 9.7984e-04, 9.8365e-04, 9.8747e-04, 9.9128e-04, 9.9510e-04, 9.9891e-04, 1.0027e-03, 1.0065e-03, 1.0104e-03, 1.0142e-03, 1.0180e-03, 1.0218e-03, 1.0256e-03, 1.0294e-03, 1.0332e-03, 1.0371e-03, 1.0409e-03, 1.0447e-03, 1.0485e-03, 1.0523e-03, 1.0561e-03, 1.0599e-03, 1.0638e-03, 1.0676e-03, 1.0714e-03, 1.0752e-03, 1.0790e-03, 1.0828e-03, 1.0866e-03, 1.0905e-03, 1.0943e-03, 1.0981e-03, 1.1019e-03, 1.1057e-03, 1.1095e-03, 1.1133e-03, 1.1172e-03, 1.1210e-03, 1.1248e-03, 1.1286e-03, 1.1324e-03, 1.1362e-03, 1.1400e-03, 1.1439e-03, 1.1477e-03, 1.1515e-03, 1.1553e-03, 1.1591e-03, 1.1629e-03, 1.1667e-03, 1.1706e-03, 1.1744e-03, 1.1782e-03, 1.1820e-03, 1.1858e-03, 1.1896e-03, 1.1934e-03, 1.1973e-03, 1.2011e-03, 1.2049e-03, 1.2087e-03, 1.2125e-03, 1.2163e-03, 1.2201e-03, 1.2240e-03, 1.2278e-03, 1.2316e-03, 1.2354e-03, 1.2392e-03, 1.2430e-03, 1.2468e-03, 1.2507e-03, 1.2545e-03, 1.2583e-03, 1.2621e-03, 1.2659e-03, 1.2697e-03, 1.2735e-03, 1.2774e-03, 1.2812e-03, 1.2850e-03, 1.2888e-03, 1.2926e-03, 1.2964e-03, 1.3002e-03, 1.3040e-03, 1.3079e-03, 1.3117e-03, 1.3155e-03, 1.3193e-03, 1.3231e-03, 1.3269e-03, 1.3307e-03, 1.3346e-03, 1.3384e-03, 1.3422e-03, 1.3460e-03, 1.3498e-03, 1.3536e-03, 1.3574e-03, 1.3613e-03, 1.3651e-03, 1.3689e-03, 1.3727e-03, 1.3765e-03, 1.3803e-03, 1.3841e-03, 1.3880e-03, 1.3918e-03, 1.3956e-03, 1.3994e-03, 1.4032e-03, 1.4070e-03, 1.4108e-03, 1.4147e-03, 1.4185e-03, 1.4223e-03, 1.4261e-03, 1.4299e-03, 1.4337e-03, 1.4375e-03, 1.4414e-03, 1.4452e-03, 1.4490e-03, 1.4528e-03, 1.4566e-03, 1.4604e-03, 1.4642e-03, 1.4681e-03, 1.4719e-03, 1.4757e-03, 1.4795e-03, 1.4833e-03, 1.4871e-03, 1.4909e-03, 1.4948e-03, 1.4986e-03, 1.5024e-03, 1.5062e-03, 1.5100e-03, 1.5138e-03, 1.5176e-03, 1.5215e-03, 1.5253e-03, 1.5291e-03, 1.5329e-03, 1.5367e-03, 1.5405e-03, 1.5443e-03, 1.5482e-03, 1.5520e-03, 1.5558e-03, 1.5596e-03, 1.5634e-03, 1.5672e-03, 1.5710e-03, 1.5749e-03, 1.5787e-03, 1.5825e-03, 1.5863e-03, 1.5901e-03, 1.5939e-03, 1.5977e-03, 1.6016e-03, 1.6054e-03, 1.6092e-03, 1.6130e-03, 1.6168e-03, 1.6206e-03, 1.6244e-03, 1.6283e-03, 1.6321e-03, 1.6359e-03, 1.6397e-03, 1.6435e-03, 1.6473e-03, 1.6511e-03, 1.6550e-03, 1.6588e-03, 1.6626e-03, 1.6664e-03, 1.6702e-03, 1.6740e-03, 1.6778e-03, 1.6817e-03, 1.6855e-03, 1.6893e-03, 1.6931e-03, 1.6969e-03, 1.7007e-03, 1.7045e-03, 1.7084e-03, 1.7122e-03, 1.7160e-03, 1.7198e-03, 1.7236e-03, 1.7274e-03, 1.7312e-03, 1.7351e-03, 1.7389e-03, 1.7427e-03, 1.7465e-03, 1.7503e-03, 1.7541e-03, 1.7579e-03, 1.7618e-03, 1.7656e-03, 1.7694e-03, 1.7732e-03, 1.7770e-03, 1.7808e-03, 1.7846e-03, 1.7885e-03, 1.7923e-03, 1.7961e-03, 1.7999e-03, 1.8037e-03, 1.8075e-03, 1.8113e-03, 1.8152e-03, 1.8190e-03, 1.8228e-03, 1.8266e-03, 1.8304e-03, 1.8342e-03, 1.8380e-03, 1.8419e-03, 1.8457e-03, 1.8495e-03, 1.8533e-03, 1.8571e-03, 1.8609e-03, 1.8647e-03, 1.8686e-03, 1.8724e-03, 1.8762e-03, 1.8800e-03, 1.8838e-03, 1.8876e-03, 1.8914e-03, 1.8953e-03, 1.8991e-03, 1.9029e-03, 1.9067e-03, 1.9105e-03, 1.9143e-03, 1.9181e-03, 1.9220e-03, 1.9258e-03, 1.9296e-03, 1.9334e-03, 1.9372e-03, 1.9410e-03, 1.9448e-03, 1.9487e-03, 1.9525e-03, // NOLINT + 1.9601e-03, 1.9677e-03, 1.9754e-03, 1.9830e-03, 1.9906e-03, 1.9982e-03, 2.0059e-03, 2.0135e-03, 2.0211e-03, 2.0288e-03, 2.0364e-03, 2.0440e-03, 2.0516e-03, 2.0593e-03, 2.0669e-03, 2.0745e-03, 2.0822e-03, 2.0898e-03, 2.0974e-03, 2.1050e-03, 2.1127e-03, 2.1203e-03, 2.1279e-03, 2.1356e-03, 2.1432e-03, 2.1508e-03, 2.1585e-03, 2.1661e-03, 2.1737e-03, 2.1813e-03, 2.1890e-03, 2.1966e-03, 2.2042e-03, 2.2119e-03, 2.2195e-03, 2.2271e-03, 2.2347e-03, 2.2424e-03, 2.2500e-03, 2.2576e-03, 2.2653e-03, 2.2729e-03, 2.2805e-03, 2.2881e-03, 2.2958e-03, 2.3034e-03, 2.3110e-03, 2.3187e-03, 2.3263e-03, 2.3339e-03, 2.3415e-03, 2.3492e-03, 2.3568e-03, 2.3644e-03, 2.3721e-03, 2.3797e-03, 2.3873e-03, 2.3949e-03, 2.4026e-03, 2.4102e-03, 2.4178e-03, 2.4255e-03, 2.4331e-03, 2.4407e-03, 2.4483e-03, 2.4560e-03, 2.4636e-03, 2.4712e-03, 2.4789e-03, 2.4865e-03, 2.4941e-03, 2.5018e-03, 2.5094e-03, 2.5170e-03, 2.5246e-03, 2.5323e-03, 2.5399e-03, 2.5475e-03, 2.5552e-03, 2.5628e-03, 2.5704e-03, 2.5780e-03, 2.5857e-03, 2.5933e-03, 2.6009e-03, 2.6086e-03, 2.6162e-03, 2.6238e-03, 2.6314e-03, 2.6391e-03, 2.6467e-03, 2.6543e-03, 2.6620e-03, 2.6696e-03, 2.6772e-03, 2.6848e-03, 2.6925e-03, 2.7001e-03, 2.7077e-03, 2.7154e-03, 2.7230e-03, 2.7306e-03, 2.7382e-03, 2.7459e-03, 2.7535e-03, 2.7611e-03, 2.7688e-03, 2.7764e-03, 2.7840e-03, 2.7917e-03, 2.7993e-03, 2.8069e-03, 2.8145e-03, 2.8222e-03, 2.8298e-03, 2.8374e-03, 2.8451e-03, 2.8527e-03, 2.8603e-03, 2.8679e-03, 2.8756e-03, 2.8832e-03, 2.8908e-03, 2.8985e-03, 2.9061e-03, 2.9137e-03, 2.9213e-03, 2.9290e-03, 2.9366e-03, 2.9442e-03, 2.9519e-03, 2.9595e-03, 2.9671e-03, 2.9747e-03, 2.9824e-03, 2.9900e-03, 2.9976e-03, 3.0053e-03, 3.0129e-03, 3.0205e-03, 3.0281e-03, 3.0358e-03, 3.0434e-03, 3.0510e-03, 3.0587e-03, 3.0663e-03, 3.0739e-03, 3.0816e-03, 3.0892e-03, 3.0968e-03, 3.1044e-03, 3.1121e-03, 3.1197e-03, 3.1273e-03, 3.1350e-03, 3.1426e-03, 3.1502e-03, 3.1578e-03, 3.1655e-03, 3.1731e-03, 3.1807e-03, 3.1884e-03, 3.1960e-03, 3.2036e-03, 3.2112e-03, 3.2189e-03, 3.2265e-03, 3.2341e-03, 3.2418e-03, 3.2494e-03, 3.2570e-03, 3.2646e-03, 3.2723e-03, 3.2799e-03, 3.2875e-03, 3.2952e-03, 3.3028e-03, 3.3104e-03, 3.3180e-03, 3.3257e-03, 3.3333e-03, 3.3409e-03, 3.3486e-03, 3.3562e-03, 3.3638e-03, 3.3715e-03, 3.3791e-03, 3.3867e-03, 3.3943e-03, 3.4020e-03, 3.4096e-03, 3.4172e-03, 3.4249e-03, 3.4325e-03, 3.4401e-03, 3.4477e-03, 3.4554e-03, 3.4630e-03, 3.4706e-03, 3.4783e-03, 3.4859e-03, 3.4935e-03, 3.5011e-03, 3.5088e-03, 3.5164e-03, 3.5240e-03, 3.5317e-03, 3.5393e-03, 3.5469e-03, 3.5545e-03, 3.5622e-03, 3.5698e-03, 3.5774e-03, 3.5851e-03, 3.5927e-03, 3.6003e-03, 3.6079e-03, 3.6156e-03, 3.6232e-03, 3.6308e-03, 3.6385e-03, 3.6461e-03, 3.6537e-03, 3.6613e-03, 3.6690e-03, 3.6766e-03, 3.6842e-03, 3.6919e-03, 3.6995e-03, 3.7071e-03, 3.7148e-03, 3.7224e-03, 3.7300e-03, 3.7376e-03, 3.7453e-03, 3.7529e-03, 3.7605e-03, 3.7682e-03, 3.7758e-03, 3.7834e-03, 3.7910e-03, 3.7987e-03, 3.8063e-03, 3.8139e-03, 3.8216e-03, 3.8292e-03, 3.8368e-03, 3.8444e-03, 3.8521e-03, 3.8597e-03, 3.8673e-03, 3.8750e-03, 3.8826e-03, 3.8902e-03, 3.8978e-03, 3.9055e-03, // NOLINT + 3.9207e-03, 3.9360e-03, 3.9513e-03, 3.9665e-03, 3.9818e-03, 3.9970e-03, 4.0123e-03, 4.0275e-03, 4.0428e-03, 4.0581e-03, 4.0733e-03, 4.0886e-03, 4.1038e-03, 4.1191e-03, 4.1343e-03, 4.1496e-03, 4.1649e-03, 4.1801e-03, 4.1954e-03, 4.2106e-03, 4.2259e-03, 4.2412e-03, 4.2564e-03, 4.2717e-03, 4.2869e-03, 4.3022e-03, 4.3174e-03, 4.3327e-03, 4.3480e-03, 4.3632e-03, 4.3785e-03, 4.3937e-03, 4.4090e-03, 4.4243e-03, 4.4395e-03, 4.4548e-03, 4.4700e-03, 4.4853e-03, 4.5005e-03, 4.5158e-03, 4.5311e-03, 4.5463e-03, 4.5616e-03, 4.5768e-03, 4.5921e-03, 4.6074e-03, 4.6226e-03, 4.6379e-03, 4.6531e-03, 4.6684e-03, 4.6836e-03, 4.6989e-03, 4.7142e-03, 4.7294e-03, 4.7447e-03, 4.7599e-03, 4.7752e-03, 4.7905e-03, 4.8057e-03, 4.8210e-03, 4.8362e-03, 4.8515e-03, 4.8667e-03, 4.8820e-03, 4.8973e-03, 4.9125e-03, 4.9278e-03, 4.9430e-03, 4.9583e-03, 4.9736e-03, 4.9888e-03, 5.0041e-03, 5.0193e-03, 5.0346e-03, 5.0498e-03, 5.0651e-03, 5.0804e-03, 5.0956e-03, 5.1109e-03, 5.1261e-03, 5.1414e-03, 5.1567e-03, 5.1719e-03, 5.1872e-03, 5.2024e-03, 5.2177e-03, 5.2329e-03, 5.2482e-03, 5.2635e-03, 5.2787e-03, 5.2940e-03, 5.3092e-03, 5.3245e-03, 5.3398e-03, 5.3550e-03, 5.3703e-03, 5.3855e-03, 5.4008e-03, 5.4160e-03, 5.4313e-03, 5.4466e-03, 5.4618e-03, 5.4771e-03, 5.4923e-03, 5.5076e-03, 5.5229e-03, 5.5381e-03, 5.5534e-03, 5.5686e-03, 5.5839e-03, 5.5991e-03, 5.6144e-03, 5.6297e-03, 5.6449e-03, 5.6602e-03, 5.6754e-03, 5.6907e-03, 5.7060e-03, 5.7212e-03, 5.7365e-03, 5.7517e-03, 5.7670e-03, 5.7822e-03, 5.7975e-03, 5.8128e-03, 5.8280e-03, 5.8433e-03, 5.8585e-03, 5.8738e-03, 5.8891e-03, 5.9043e-03, 5.9196e-03, 5.9348e-03, 5.9501e-03, 5.9653e-03, 5.9806e-03, 5.9959e-03, 6.0111e-03, 6.0264e-03, 6.0416e-03, 6.0569e-03, 6.0722e-03, 6.0874e-03, 6.1027e-03, 6.1179e-03, 6.1332e-03, 6.1484e-03, 6.1637e-03, 6.1790e-03, 6.1942e-03, 6.2095e-03, 6.2247e-03, 6.2400e-03, 6.2553e-03, 6.2705e-03, 6.2858e-03, 6.3010e-03, 6.3163e-03, 6.3315e-03, 6.3468e-03, 6.3621e-03, 6.3773e-03, 6.3926e-03, 6.4078e-03, 6.4231e-03, 6.4384e-03, 6.4536e-03, 6.4689e-03, 6.4841e-03, 6.4994e-03, 6.5146e-03, 6.5299e-03, 6.5452e-03, 6.5604e-03, 6.5757e-03, 6.5909e-03, 6.6062e-03, 6.6215e-03, 6.6367e-03, 6.6520e-03, 6.6672e-03, 6.6825e-03, 6.6977e-03, 6.7130e-03, 6.7283e-03, 6.7435e-03, 6.7588e-03, 6.7740e-03, 6.7893e-03, 6.8046e-03, 6.8198e-03, 6.8351e-03, 6.8503e-03, 6.8656e-03, 6.8808e-03, 6.8961e-03, 6.9114e-03, 6.9266e-03, 6.9419e-03, 6.9571e-03, 6.9724e-03, 6.9877e-03, 7.0029e-03, 7.0182e-03, 7.0334e-03, 7.0487e-03, 7.0639e-03, 7.0792e-03, 7.0945e-03, 7.1097e-03, 7.1250e-03, 7.1402e-03, 7.1555e-03, 7.1708e-03, 7.1860e-03, 7.2013e-03, 7.2165e-03, 7.2318e-03, 7.2470e-03, 7.2623e-03, 7.2776e-03, 7.2928e-03, 7.3081e-03, 7.3233e-03, 7.3386e-03, 7.3539e-03, 7.3691e-03, 7.3844e-03, 7.3996e-03, 7.4149e-03, 7.4301e-03, 7.4454e-03, 7.4607e-03, 7.4759e-03, 7.4912e-03, 7.5064e-03, 7.5217e-03, 7.5370e-03, 7.5522e-03, 7.5675e-03, 7.5827e-03, 7.5980e-03, 7.6132e-03, 7.6285e-03, 7.6438e-03, 7.6590e-03, 7.6743e-03, 7.6895e-03, 7.7048e-03, 7.7201e-03, 7.7353e-03, 7.7506e-03, 7.7658e-03, 7.7811e-03, 7.7963e-03, 7.8116e-03, // NOLINT + 7.8421e-03, 7.8726e-03, 7.9032e-03, 7.9337e-03, 7.9642e-03, 7.9947e-03, 8.0252e-03, 8.0557e-03, 8.0863e-03, 8.1168e-03, 8.1473e-03, 8.1778e-03, 8.2083e-03, 8.2388e-03, 8.2694e-03, 8.2999e-03, 8.3304e-03, 8.3609e-03, 8.3914e-03, 8.4219e-03, 8.4525e-03, 8.4830e-03, 8.5135e-03, 8.5440e-03, 8.5745e-03, 8.6051e-03, 8.6356e-03, 8.6661e-03, 8.6966e-03, 8.7271e-03, 8.7576e-03, 8.7882e-03, 8.8187e-03, 8.8492e-03, 8.8797e-03, 8.9102e-03, 8.9407e-03, 8.9713e-03, 9.0018e-03, 9.0323e-03, 9.0628e-03, 9.0933e-03, 9.1238e-03, 9.1544e-03, 9.1849e-03, 9.2154e-03, 9.2459e-03, 9.2764e-03, 9.3069e-03, 9.3375e-03, 9.3680e-03, 9.3985e-03, 9.4290e-03, 9.4595e-03, 9.4900e-03, 9.5206e-03, 9.5511e-03, 9.5816e-03, 9.6121e-03, 9.6426e-03, 9.6731e-03, 9.7037e-03, 9.7342e-03, 9.7647e-03, 9.7952e-03, 9.8257e-03, 9.8563e-03, 9.8868e-03, 9.9173e-03, 9.9478e-03, 9.9783e-03, 1.0009e-02, 1.0039e-02, 1.0070e-02, 1.0100e-02, 1.0131e-02, 1.0161e-02, 1.0192e-02, 1.0222e-02, 1.0253e-02, 1.0283e-02, 1.0314e-02, 1.0345e-02, 1.0375e-02, 1.0406e-02, 1.0436e-02, 1.0467e-02, 1.0497e-02, 1.0528e-02, 1.0558e-02, 1.0589e-02, 1.0619e-02, 1.0650e-02, 1.0680e-02, 1.0711e-02, 1.0741e-02, 1.0772e-02, 1.0802e-02, 1.0833e-02, 1.0863e-02, 1.0894e-02, 1.0924e-02, 1.0955e-02, 1.0985e-02, 1.1016e-02, 1.1046e-02, 1.1077e-02, 1.1107e-02, 1.1138e-02, 1.1168e-02, 1.1199e-02, 1.1230e-02, 1.1260e-02, 1.1291e-02, 1.1321e-02, 1.1352e-02, 1.1382e-02, 1.1413e-02, 1.1443e-02, 1.1474e-02, 1.1504e-02, 1.1535e-02, 1.1565e-02, 1.1596e-02, 1.1626e-02, 1.1657e-02, 1.1687e-02, 1.1718e-02, 1.1748e-02, 1.1779e-02, 1.1809e-02, 1.1840e-02, 1.1870e-02, 1.1901e-02, 1.1931e-02, 1.1962e-02, 1.1992e-02, 1.2023e-02, 1.2053e-02, 1.2084e-02, 1.2115e-02, 1.2145e-02, 1.2176e-02, 1.2206e-02, 1.2237e-02, 1.2267e-02, 1.2298e-02, 1.2328e-02, 1.2359e-02, 1.2389e-02, 1.2420e-02, 1.2450e-02, 1.2481e-02, 1.2511e-02, 1.2542e-02, 1.2572e-02, 1.2603e-02, 1.2633e-02, 1.2664e-02, 1.2694e-02, 1.2725e-02, 1.2755e-02, 1.2786e-02, 1.2816e-02, 1.2847e-02, 1.2877e-02, 1.2908e-02, 1.2938e-02, 1.2969e-02, 1.3000e-02, 1.3030e-02, 1.3061e-02, 1.3091e-02, 1.3122e-02, 1.3152e-02, 1.3183e-02, 1.3213e-02, 1.3244e-02, 1.3274e-02, 1.3305e-02, 1.3335e-02, 1.3366e-02, 1.3396e-02, 1.3427e-02, 1.3457e-02, 1.3488e-02, 1.3518e-02, 1.3549e-02, 1.3579e-02, 1.3610e-02, 1.3640e-02, 1.3671e-02, 1.3701e-02, 1.3732e-02, 1.3762e-02, 1.3793e-02, 1.3823e-02, 1.3854e-02, 1.3885e-02, 1.3915e-02, 1.3946e-02, 1.3976e-02, 1.4007e-02, 1.4037e-02, 1.4068e-02, 1.4098e-02, 1.4129e-02, 1.4159e-02, 1.4190e-02, 1.4220e-02, 1.4251e-02, 1.4281e-02, 1.4312e-02, 1.4342e-02, 1.4373e-02, 1.4403e-02, 1.4434e-02, 1.4464e-02, 1.4495e-02, 1.4525e-02, 1.4556e-02, 1.4586e-02, 1.4617e-02, 1.4647e-02, 1.4678e-02, 1.4708e-02, 1.4739e-02, 1.4770e-02, 1.4800e-02, 1.4831e-02, 1.4861e-02, 1.4892e-02, 1.4922e-02, 1.4953e-02, 1.4983e-02, 1.5014e-02, 1.5044e-02, 1.5075e-02, 1.5105e-02, 1.5136e-02, 1.5166e-02, 1.5197e-02, 1.5227e-02, 1.5258e-02, 1.5288e-02, 1.5319e-02, 1.5349e-02, 1.5380e-02, 1.5410e-02, 1.5441e-02, 1.5471e-02, 1.5502e-02, 1.5532e-02, 1.5563e-02, 1.5593e-02, 1.5624e-02, // NOLINT + 1.5746e-02, 1.5868e-02, 1.5990e-02, 1.6112e-02, 1.6234e-02, 1.6356e-02, 1.6478e-02, 1.6601e-02, 1.6723e-02, 1.6845e-02, 1.6967e-02, 1.7089e-02, 1.7211e-02, 1.7333e-02, 1.7455e-02, 1.7577e-02, 1.7699e-02, 1.7821e-02, 1.7943e-02, 1.8065e-02, 1.8187e-02, 1.8310e-02, 1.8432e-02, 1.8554e-02, 1.8676e-02, 1.8798e-02, 1.8920e-02, 1.9042e-02, 1.9164e-02, 1.9286e-02, 1.9408e-02, 1.9530e-02, 1.9652e-02, 1.9774e-02, 1.9896e-02, 2.0018e-02, 2.0141e-02, 2.0263e-02, 2.0385e-02, 2.0507e-02, 2.0629e-02, 2.0751e-02, 2.0873e-02, 2.0995e-02, 2.1117e-02, 2.1239e-02, 2.1361e-02, 2.1483e-02, 2.1605e-02, 2.1727e-02, 2.1850e-02, 2.1972e-02, 2.2094e-02, 2.2216e-02, 2.2338e-02, 2.2460e-02, 2.2582e-02, 2.2704e-02, 2.2826e-02, 2.2948e-02, 2.3070e-02, 2.3192e-02, 2.3314e-02, 2.3436e-02, 2.3558e-02, 2.3681e-02, 2.3803e-02, 2.3925e-02, 2.4047e-02, 2.4169e-02, 2.4291e-02, 2.4413e-02, 2.4535e-02, 2.4657e-02, 2.4779e-02, 2.4901e-02, 2.5023e-02, 2.5145e-02, 2.5267e-02, 2.5390e-02, 2.5512e-02, 2.5634e-02, 2.5756e-02, 2.5878e-02, 2.6000e-02, 2.6122e-02, 2.6244e-02, 2.6366e-02, 2.6488e-02, 2.6610e-02, 2.6732e-02, 2.6854e-02, 2.6976e-02, 2.7099e-02, 2.7221e-02, 2.7343e-02, 2.7465e-02, 2.7587e-02, 2.7709e-02, 2.7831e-02, 2.7953e-02, 2.8075e-02, 2.8197e-02, 2.8319e-02, 2.8441e-02, 2.8563e-02, 2.8685e-02, 2.8807e-02, 2.8930e-02, 2.9052e-02, 2.9174e-02, 2.9296e-02, 2.9418e-02, 2.9540e-02, 2.9662e-02, 2.9784e-02, 2.9906e-02, 3.0028e-02, 3.0150e-02, 3.0272e-02, 3.0394e-02, 3.0516e-02, 3.0639e-02, 3.0761e-02, 3.0883e-02, 3.1005e-02, 3.1127e-02, 3.1249e-02, 3.1493e-02, 3.1737e-02, 3.1981e-02, 3.2225e-02, 3.2470e-02, 3.2714e-02, 3.2958e-02, 3.3202e-02, 3.3446e-02, 3.3690e-02, 3.3934e-02, 3.4179e-02, 3.4423e-02, 3.4667e-02, 3.4911e-02, 3.5155e-02, 3.5399e-02, 3.5643e-02, 3.5888e-02, 3.6132e-02, 3.6376e-02, 3.6620e-02, 3.6864e-02, 3.7108e-02, 3.7352e-02, 3.7596e-02, 3.7841e-02, 3.8085e-02, 3.8329e-02, 3.8573e-02, 3.8817e-02, 3.9061e-02, 3.9305e-02, 3.9550e-02, 3.9794e-02, 4.0038e-02, 4.0282e-02, 4.0526e-02, 4.0770e-02, 4.1014e-02, 4.1259e-02, 4.1503e-02, 4.1747e-02, 4.1991e-02, 4.2235e-02, 4.2479e-02, 4.2723e-02, 4.2968e-02, 4.3212e-02, 4.3456e-02, 4.3700e-02, 4.3944e-02, 4.4188e-02, 4.4432e-02, 4.4677e-02, 4.4921e-02, 4.5165e-02, 4.5409e-02, 4.5653e-02, 4.5897e-02, 4.6141e-02, 4.6386e-02, 4.6630e-02, 4.6874e-02, 4.7118e-02, 4.7362e-02, 4.7606e-02, 4.7850e-02, 4.8095e-02, 4.8339e-02, 4.8583e-02, 4.8827e-02, 4.9071e-02, 4.9315e-02, 4.9559e-02, 4.9803e-02, 5.0048e-02, 5.0292e-02, 5.0536e-02, 5.0780e-02, 5.1024e-02, 5.1268e-02, 5.1512e-02, 5.1757e-02, 5.2001e-02, 5.2245e-02, 5.2489e-02, 5.2733e-02, 5.2977e-02, 5.3221e-02, 5.3466e-02, 5.3710e-02, 5.3954e-02, 5.4198e-02, 5.4442e-02, 5.4686e-02, 5.4930e-02, 5.5175e-02, 5.5419e-02, 5.5663e-02, 5.5907e-02, 5.6151e-02, 5.6395e-02, 5.6639e-02, 5.6884e-02, 5.7128e-02, 5.7372e-02, 5.7616e-02, 5.7860e-02, 5.8104e-02, 5.8348e-02, 5.8593e-02, 5.8837e-02, 5.9081e-02, 5.9325e-02, 5.9569e-02, 5.9813e-02, 6.0057e-02, 6.0301e-02, 6.0546e-02, 6.0790e-02, 6.1034e-02, 6.1278e-02, 6.1522e-02, 6.1766e-02, 6.2010e-02, 6.2255e-02, 6.2499e-02, // NOLINT + 6.2743e-02, 6.2987e-02, 6.3231e-02, 6.3475e-02, 6.3719e-02, 6.3964e-02, 6.4208e-02, 6.4452e-02, 6.4696e-02, 6.4940e-02, 6.5184e-02, 6.5428e-02, 6.5673e-02, 6.5917e-02, 6.6161e-02, 6.6405e-02, 6.6649e-02, 6.6893e-02, 6.7137e-02, 6.7382e-02, 6.7626e-02, 6.7870e-02, 6.8114e-02, 6.8358e-02, 6.8602e-02, 6.8846e-02, 6.9091e-02, 6.9335e-02, 6.9579e-02, 6.9823e-02, 7.0067e-02, 7.0311e-02, 7.0555e-02, 7.0799e-02, 7.1044e-02, 7.1288e-02, 7.1532e-02, 7.1776e-02, 7.2020e-02, 7.2264e-02, 7.2508e-02, 7.2753e-02, 7.2997e-02, 7.3241e-02, 7.3485e-02, 7.3729e-02, 7.3973e-02, 7.4217e-02, 7.4462e-02, 7.4706e-02, 7.4950e-02, 7.5194e-02, 7.5438e-02, 7.5682e-02, 7.5926e-02, 7.6171e-02, 7.6415e-02, 7.6659e-02, 7.6903e-02, 7.7147e-02, 7.7391e-02, 7.7635e-02, 7.7880e-02, 7.8124e-02, 7.8368e-02, 7.8612e-02, 7.8856e-02, 7.9100e-02, 7.9344e-02, 7.9589e-02, 7.9833e-02, 8.0077e-02, 8.0321e-02, 8.0565e-02, 8.0809e-02, 8.1053e-02, 8.1298e-02, 8.1542e-02, 8.1786e-02, 8.2030e-02, 8.2274e-02, 8.2518e-02, 8.2762e-02, 8.3006e-02, 8.3251e-02, 8.3495e-02, 8.3739e-02, 8.3983e-02, 8.4227e-02, 8.4471e-02, 8.4715e-02, 8.4960e-02, 8.5204e-02, 8.5448e-02, 8.5692e-02, 8.5936e-02, 8.6180e-02, 8.6424e-02, 8.6669e-02, 8.6913e-02, 8.7157e-02, 8.7401e-02, 8.7645e-02, 8.7889e-02, 8.8133e-02, 8.8378e-02, 8.8622e-02, 8.8866e-02, 8.9110e-02, 8.9354e-02, 8.9598e-02, 8.9842e-02, 9.0087e-02, 9.0331e-02, 9.0575e-02, 9.0819e-02, 9.1063e-02, 9.1307e-02, 9.1551e-02, 9.1796e-02, 9.2040e-02, 9.2284e-02, 9.2528e-02, 9.2772e-02, 9.3016e-02, 9.3260e-02, 9.3504e-02, 9.3749e-02, 9.4237e-02, 9.4725e-02, 9.5213e-02, 9.5702e-02, 9.6190e-02, 9.6678e-02, 9.7167e-02, 9.7655e-02, 9.8143e-02, 9.8631e-02, 9.9120e-02, 9.9608e-02, 1.0010e-01, 1.0058e-01, 1.0107e-01, 1.0156e-01, 1.0205e-01, 1.0254e-01, 1.0303e-01, 1.0351e-01, 1.0400e-01, 1.0449e-01, 1.0498e-01, 1.0547e-01, 1.0596e-01, 1.0644e-01, 1.0693e-01, 1.0742e-01, 1.0791e-01, 1.0840e-01, 1.0889e-01, 1.0937e-01, 1.0986e-01, 1.1035e-01, 1.1084e-01, 1.1133e-01, 1.1182e-01, 1.1230e-01, 1.1279e-01, 1.1328e-01, 1.1377e-01, 1.1426e-01, 1.1474e-01, 1.1523e-01, 1.1572e-01, 1.1621e-01, 1.1670e-01, 1.1719e-01, 1.1767e-01, 1.1816e-01, 1.1865e-01, 1.1914e-01, 1.1963e-01, 1.2012e-01, 1.2060e-01, 1.2109e-01, 1.2158e-01, 1.2207e-01, 1.2256e-01, 1.2305e-01, 1.2353e-01, 1.2402e-01, 1.2451e-01, 1.2500e-01, 1.2549e-01, 1.2598e-01, 1.2646e-01, 1.2695e-01, 1.2744e-01, 1.2793e-01, 1.2842e-01, 1.2890e-01, 1.2939e-01, 1.2988e-01, 1.3037e-01, 1.3086e-01, 1.3135e-01, 1.3183e-01, 1.3232e-01, 1.3281e-01, 1.3330e-01, 1.3379e-01, 1.3428e-01, 1.3476e-01, 1.3525e-01, 1.3574e-01, 1.3623e-01, 1.3672e-01, 1.3721e-01, 1.3769e-01, 1.3818e-01, 1.3867e-01, 1.3916e-01, 1.3965e-01, 1.4014e-01, 1.4062e-01, 1.4111e-01, 1.4160e-01, 1.4209e-01, 1.4258e-01, 1.4306e-01, 1.4355e-01, 1.4404e-01, 1.4453e-01, 1.4502e-01, 1.4551e-01, 1.4599e-01, 1.4648e-01, 1.4697e-01, 1.4746e-01, 1.4795e-01, 1.4844e-01, 1.4892e-01, 1.4941e-01, 1.4990e-01, 1.5039e-01, 1.5088e-01, 1.5137e-01, 1.5185e-01, 1.5234e-01, 1.5283e-01, 1.5332e-01, 1.5381e-01, 1.5430e-01, 1.5478e-01, 1.5527e-01, 1.5576e-01, 1.5625e-01, // NOLINT + 1.5674e-01, 1.5723e-01, 1.5771e-01, 1.5820e-01, 1.5869e-01, 1.5918e-01, 1.5967e-01, 1.6015e-01, 1.6064e-01, 1.6113e-01, 1.6162e-01, 1.6211e-01, 1.6260e-01, 1.6308e-01, 1.6357e-01, 1.6406e-01, 1.6455e-01, 1.6504e-01, 1.6553e-01, 1.6601e-01, 1.6650e-01, 1.6699e-01, 1.6748e-01, 1.6797e-01, 1.6846e-01, 1.6894e-01, 1.6943e-01, 1.6992e-01, 1.7041e-01, 1.7090e-01, 1.7139e-01, 1.7187e-01, 1.7236e-01, 1.7285e-01, 1.7334e-01, 1.7383e-01, 1.7431e-01, 1.7480e-01, 1.7529e-01, 1.7578e-01, 1.7627e-01, 1.7676e-01, 1.7724e-01, 1.7773e-01, 1.7822e-01, 1.7871e-01, 1.7920e-01, 1.7969e-01, 1.8017e-01, 1.8066e-01, 1.8115e-01, 1.8164e-01, 1.8213e-01, 1.8262e-01, 1.8310e-01, 1.8359e-01, 1.8408e-01, 1.8457e-01, 1.8506e-01, 1.8555e-01, 1.8603e-01, 1.8652e-01, 1.8701e-01, 1.8750e-01, 1.8848e-01, 1.8945e-01, 1.9043e-01, 1.9140e-01, 1.9238e-01, 1.9336e-01, 1.9433e-01, 1.9531e-01, 1.9629e-01, 1.9726e-01, 1.9824e-01, 1.9922e-01, 2.0019e-01, 2.0117e-01, 2.0215e-01, 2.0312e-01, 2.0410e-01, 2.0508e-01, 2.0605e-01, 2.0703e-01, 2.0801e-01, 2.0898e-01, 2.0996e-01, 2.1094e-01, 2.1191e-01, 2.1289e-01, 2.1387e-01, 2.1484e-01, 2.1582e-01, 2.1680e-01, 2.1777e-01, 2.1875e-01, 2.1972e-01, 2.2070e-01, 2.2168e-01, 2.2265e-01, 2.2363e-01, 2.2461e-01, 2.2558e-01, 2.2656e-01, 2.2754e-01, 2.2851e-01, 2.2949e-01, 2.3047e-01, 2.3144e-01, 2.3242e-01, 2.3340e-01, 2.3437e-01, 2.3535e-01, 2.3633e-01, 2.3730e-01, 2.3828e-01, 2.3926e-01, 2.4023e-01, 2.4121e-01, 2.4219e-01, 2.4316e-01, 2.4414e-01, 2.4512e-01, 2.4609e-01, 2.4707e-01, 2.4805e-01, 2.4902e-01, 2.5000e-01, 2.5097e-01, 2.5195e-01, 2.5293e-01, 2.5390e-01, 2.5488e-01, 2.5586e-01, 2.5683e-01, 2.5781e-01, 2.5879e-01, 2.5976e-01, 2.6074e-01, 2.6172e-01, 2.6269e-01, 2.6367e-01, 2.6465e-01, 2.6562e-01, 2.6660e-01, 2.6758e-01, 2.6855e-01, 2.6953e-01, 2.7051e-01, 2.7148e-01, 2.7246e-01, 2.7344e-01, 2.7441e-01, 2.7539e-01, 2.7637e-01, 2.7734e-01, 2.7832e-01, 2.7930e-01, 2.8027e-01, 2.8125e-01, 2.8222e-01, 2.8320e-01, 2.8418e-01, 2.8515e-01, 2.8613e-01, 2.8711e-01, 2.8808e-01, 2.8906e-01, 2.9004e-01, 2.9101e-01, 2.9199e-01, 2.9297e-01, 2.9394e-01, 2.9492e-01, 2.9590e-01, 2.9687e-01, 2.9785e-01, 2.9883e-01, 2.9980e-01, 3.0078e-01, 3.0176e-01, 3.0273e-01, 3.0371e-01, 3.0469e-01, 3.0566e-01, 3.0664e-01, 3.0762e-01, 3.0859e-01, 3.0957e-01, 3.1055e-01, 3.1152e-01, 3.1250e-01, 3.1347e-01, 3.1445e-01, 3.1543e-01, 3.1640e-01, 3.1738e-01, 3.1836e-01, 3.1933e-01, 3.2031e-01, 3.2129e-01, 3.2226e-01, 3.2324e-01, 3.2422e-01, 3.2519e-01, 3.2617e-01, 3.2715e-01, 3.2812e-01, 3.2910e-01, 3.3008e-01, 3.3105e-01, 3.3203e-01, 3.3301e-01, 3.3398e-01, 3.3496e-01, 3.3594e-01, 3.3691e-01, 3.3789e-01, 3.3887e-01, 3.3984e-01, 3.4082e-01, 3.4180e-01, 3.4277e-01, 3.4375e-01, 3.4472e-01, 3.4570e-01, 3.4668e-01, 3.4765e-01, 3.4863e-01, 3.4961e-01, 3.5058e-01, 3.5156e-01, 3.5254e-01, 3.5351e-01, 3.5449e-01, 3.5547e-01, 3.5644e-01, 3.5742e-01, 3.5840e-01, 3.5937e-01, 3.6035e-01, 3.6133e-01, 3.6230e-01, 3.6328e-01, 3.6426e-01, 3.6523e-01, 3.6621e-01, 3.6719e-01, 3.6816e-01, 3.6914e-01, 3.7012e-01, 3.7109e-01, 3.7207e-01, 3.7305e-01, 3.7402e-01, 3.7500e-01, // NOLINT + 3.7695e-01, 3.7890e-01, 3.8086e-01, 3.8281e-01, 3.8476e-01, 3.8672e-01, 3.8867e-01, 3.9062e-01, 3.9258e-01, 3.9453e-01, 3.9648e-01, 3.9844e-01, 4.0039e-01, 4.0234e-01, 4.0430e-01, 4.0625e-01, 4.0820e-01, 4.1015e-01, 4.1211e-01, 4.1406e-01, 4.1601e-01, 4.1797e-01, 4.1992e-01, 4.2187e-01, 4.2383e-01, 4.2578e-01, 4.2773e-01, 4.2969e-01, 4.3164e-01, 4.3359e-01, 4.3555e-01, 4.3750e-01, 4.3945e-01, 4.4140e-01, 4.4336e-01, 4.4531e-01, 4.4726e-01, 4.4922e-01, 4.5117e-01, 4.5312e-01, 4.5508e-01, 4.5703e-01, 4.5898e-01, 4.6094e-01, 4.6289e-01, 4.6484e-01, 4.6680e-01, 4.6875e-01, 4.7070e-01, 4.7265e-01, 4.7461e-01, 4.7656e-01, 4.7851e-01, 4.8047e-01, 4.8242e-01, 4.8437e-01, 4.8633e-01, 4.8828e-01, 4.9023e-01, 4.9219e-01, 4.9414e-01, 4.9609e-01, 4.9805e-01, 5.0000e-01, 5.0195e-01, 5.0390e-01, 5.0586e-01, 5.0781e-01, 5.0976e-01, 5.1172e-01, 5.1367e-01, 5.1562e-01, 5.1758e-01, 5.1953e-01, 5.2148e-01, 5.2344e-01, 5.2539e-01, 5.2734e-01, 5.2930e-01, 5.3125e-01, 5.3320e-01, 5.3515e-01, 5.3711e-01, 5.3906e-01, 5.4101e-01, 5.4297e-01, 5.4492e-01, 5.4687e-01, 5.4883e-01, 5.5078e-01, 5.5273e-01, 5.5469e-01, 5.5664e-01, 5.5859e-01, 5.6055e-01, 5.6250e-01, 5.6445e-01, 5.6640e-01, 5.6836e-01, 5.7031e-01, 5.7226e-01, 5.7422e-01, 5.7617e-01, 5.7812e-01, 5.8008e-01, 5.8203e-01, 5.8398e-01, 5.8594e-01, 5.8789e-01, 5.8984e-01, 5.9180e-01, 5.9375e-01, 5.9570e-01, 5.9765e-01, 5.9961e-01, 6.0156e-01, 6.0351e-01, 6.0547e-01, 6.0742e-01, 6.0937e-01, 6.1133e-01, 6.1328e-01, 6.1523e-01, 6.1719e-01, 6.1914e-01, 6.2109e-01, 6.2305e-01, 6.2500e-01, 6.2695e-01, 6.2890e-01, 6.3086e-01, 6.3281e-01, 6.3476e-01, 6.3672e-01, 6.3867e-01, 6.4062e-01, 6.4258e-01, 6.4453e-01, 6.4648e-01, 6.4844e-01, 6.5039e-01, 6.5234e-01, 6.5430e-01, 6.5625e-01, 6.5820e-01, 6.6015e-01, 6.6211e-01, 6.6406e-01, 6.6601e-01, 6.6797e-01, 6.6992e-01, 6.7187e-01, 6.7383e-01, 6.7578e-01, 6.7773e-01, 6.7969e-01, 6.8164e-01, 6.8359e-01, 6.8554e-01, 6.8750e-01, 6.8945e-01, 6.9140e-01, 6.9336e-01, 6.9531e-01, 6.9726e-01, 6.9922e-01, 7.0117e-01, 7.0312e-01, 7.0508e-01, 7.0703e-01, 7.0898e-01, 7.1094e-01, 7.1289e-01, 7.1484e-01, 7.1679e-01, 7.1875e-01, 7.2070e-01, 7.2265e-01, 7.2461e-01, 7.2656e-01, 7.2851e-01, 7.3047e-01, 7.3242e-01, 7.3437e-01, 7.3633e-01, 7.3828e-01, 7.4023e-01, 7.4219e-01, 7.4414e-01, 7.4609e-01, 7.4804e-01, 7.5000e-01, 7.5390e-01, 7.5781e-01, 7.6172e-01, 7.6562e-01, 7.6953e-01, 7.7344e-01, 7.7734e-01, 7.8125e-01, 7.8515e-01, 7.8906e-01, 7.9297e-01, 7.9687e-01, 8.0078e-01, 8.0469e-01, 8.0859e-01, 8.1250e-01, 8.1640e-01, 8.2031e-01, 8.2422e-01, 8.2812e-01, 8.3203e-01, 8.3594e-01, 8.3984e-01, 8.4375e-01, 8.4765e-01, 8.5156e-01, 8.5547e-01, 8.5937e-01, 8.6328e-01, 8.6719e-01, 8.7109e-01, 8.7500e-01, 8.7890e-01, 8.8281e-01, 8.8672e-01, 8.9062e-01, 8.9453e-01, 8.9844e-01, 9.0234e-01, 9.0625e-01, 9.1015e-01, 9.1406e-01, 9.1797e-01, 9.2187e-01, 9.2578e-01, 9.2969e-01, 9.3359e-01, 9.3750e-01, 9.4140e-01, 9.4531e-01, 9.4922e-01, 9.5312e-01, 9.5703e-01, 9.6094e-01, 9.6484e-01, 9.6875e-01, 9.7265e-01, 9.7656e-01, 9.8047e-01, 9.8437e-01, 9.8828e-01, 9.9219e-01, 9.9609e-01, 1.0000e+00 // NOLINT }; -float4 val4_from_12(uchar8 pvs, float gain) { - uint4 parsed = (uint4)(((uint)pvs.s0<<4) + (pvs.s1>>4), // is from the previous 10 bit - ((uint)pvs.s2<<4) + (pvs.s4&0xF), - ((uint)pvs.s3<<4) + (pvs.s4>>4), - ((uint)pvs.s5<<4) + (pvs.s7&0xF)); - #if IS_OX +float4 normalize_pv(int4 parsed, float vignette_factor) { // PWL - //float4 pv = (convert_float4(parsed) - 64.0) / (4096.0 - 64.0); float4 pv = {ox03c10_lut[parsed.s0], ox03c10_lut[parsed.s1], ox03c10_lut[parsed.s2], ox03c10_lut[parsed.s3]}; - - // it's a 24 bit signal, center in the middle 8 bits - return clamp(pv*gain*256.0, 0.0, 1.0); - #else // AR - // normalize and scale - float4 pv = (convert_float4(parsed) - 168.0) / (4096.0 - 168.0); - return clamp(pv*gain, 0.0, 1.0); - #endif - + return clamp(pv*vignette_factor*256.0, 0.0, 1.0); } -float get_k(float a, float b, float c, float d) { - return 2.0 - (fabs(a - b) + fabs(c - d)); +float3 color_correct(float3 rgb) { + float3 corrected = rgb.x * (float3)(1.5664815, -0.29808738, -0.03973474); + corrected += rgb.y * (float3)(-0.48672447, 1.41914433, -0.40295248); + corrected += rgb.z * (float3)(-0.07975703, -0.12105695, 1.44268722); + return corrected; } -__kernel void debayer10(const __global uchar * in, __global uchar * out) -{ - const int gid_x = get_global_id(0); - const int gid_y = get_global_id(1); - - const int y_top_mod = (gid_y == 0) ? 2: 0; - const int y_bot_mod = (gid_y == (RGB_HEIGHT/2 - 1)) ? 1: 3; - - float3 rgb; - uchar3 rgb_out[4]; - - int start = (2 * gid_y - 1) * FRAME_STRIDE + (3 * gid_x - 2) + (FRAME_STRIDE * FRAME_OFFSET); - - // read in 8x4 chars - uchar8 dat[4]; - dat[0] = vload8(0, in + start + FRAME_STRIDE*y_top_mod); - dat[1] = vload8(0, in + start + FRAME_STRIDE*1); - dat[2] = vload8(0, in + start + FRAME_STRIDE*2); - dat[3] = vload8(0, in + start + FRAME_STRIDE*y_bot_mod); - - // correct vignetting - #if VIGNETTING - int gx = (gid_x*2 - RGB_WIDTH/2); - int gy = (gid_y*2 - RGB_HEIGHT/2); - const float gain = get_vignetting_s(gx*gx + gy*gy); - #else - const float gain = 1.0; - #endif - - // process them to floats - float4 va = val4_from_12(dat[0], gain); - float4 vb = val4_from_12(dat[1], gain); - float4 vc = val4_from_12(dat[2], gain); - float4 vd = val4_from_12(dat[3], gain); - - if (gid_x == 0) { - va.s0 = va.s2; - vb.s0 = vb.s2; - vc.s0 = vc.s2; - vd.s0 = vd.s2; - } else if (gid_x == RGB_WIDTH/2 - 1) { - va.s3 = va.s1; - vb.s3 = vb.s1; - vc.s3 = vc.s1; - vd.s3 = vd.s1; - } - - // a simplified version of https://opensignalprocessingjournal.com/contents/volumes/V6/TOSIGPJ-6-1/TOSIGPJ-6-1.pdf - const float k01 = get_k(va.s0, vb.s1, va.s2, vb.s1); - const float k02 = get_k(va.s2, vb.s1, vc.s2, vb.s1); - const float k03 = get_k(vc.s0, vb.s1, vc.s2, vb.s1); - const float k04 = get_k(va.s0, vb.s1, vc.s0, vb.s1); - rgb.x = (k02*vb.s2+k04*vb.s0)/(k02+k04); // R_G1 - rgb.y = vb.s1; // G1(R) - rgb.z = (k01*va.s1+k03*vc.s1)/(k01+k03); // B_G1 - rgb_out[0] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - - const float k11 = get_k(va.s1, vc.s1, va.s3, vc.s3); - const float k12 = get_k(va.s2, vb.s1, vb.s3, vc.s2); - const float k13 = get_k(va.s1, va.s3, vc.s1, vc.s3); - const float k14 = get_k(va.s2, vb.s3, vc.s2, vb.s1); - rgb.x = vb.s2; // R - rgb.y = (k11*(va.s2+vc.s2)*0.5+k13*(vb.s3+vb.s1)*0.5)/(k11+k13); // G_R - rgb.z = (k12*(va.s3+vc.s1)*0.5+k14*(va.s1+vc.s3)*0.5)/(k12+k14); // B_R - rgb_out[1] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - - const float k21 = get_k(vb.s0, vd.s0, vb.s2, vd.s2); - const float k22 = get_k(vb.s1, vc.s0, vc.s2, vd.s1); - const float k23 = get_k(vb.s0, vb.s2, vd.s0, vd.s2); - const float k24 = get_k(vb.s1, vc.s2, vd.s1, vc.s0); - rgb.x = (k22*(vb.s2+vd.s0)*0.5+k24*(vb.s0+vd.s2)*0.5)/(k22+k24); // R_B - rgb.y = (k21*(vb.s1+vd.s1)*0.5+k23*(vc.s2+vc.s0)*0.5)/(k21+k23); // G_B - rgb.z = vc.s1; // B - rgb_out[2] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - - const float k31 = get_k(vb.s1, vc.s2, vb.s3, vc.s2); - const float k32 = get_k(vb.s3, vc.s2, vd.s3, vc.s2); - const float k33 = get_k(vd.s1, vc.s2, vd.s3, vc.s2); - const float k34 = get_k(vb.s1, vc.s2, vd.s1, vc.s2); - rgb.x = (k31*vb.s2+k33*vd.s2)/(k31+k33); // R_G2 - rgb.y = vc.s2; // G2(B) - rgb.z = (k32*vc.s3+k34*vc.s1)/(k32+k34); // B_G2 - rgb_out[3] = convert_uchar3_sat(color_correct(clamp(rgb, 0.0, 1.0)) * 255.0); - - // write ys - uchar2 yy = (uchar2)( - RGB_TO_Y(rgb_out[0].s0, rgb_out[0].s1, rgb_out[0].s2), - RGB_TO_Y(rgb_out[1].s0, rgb_out[1].s1, rgb_out[1].s2) - ); - vstore2(yy, 0, out + mad24(gid_y * 2, YUV_STRIDE, gid_x * 2)); - yy = (uchar2)( - RGB_TO_Y(rgb_out[2].s0, rgb_out[2].s1, rgb_out[2].s2), - RGB_TO_Y(rgb_out[3].s0, rgb_out[3].s1, rgb_out[3].s2) - ); - vstore2(yy, 0, out + mad24(gid_y * 2 + 1, YUV_STRIDE, gid_x * 2)); - - // write uvs - const short ar = AVERAGE(rgb_out[0].s0, rgb_out[1].s0, rgb_out[2].s0, rgb_out[3].s0); - const short ag = AVERAGE(rgb_out[0].s1, rgb_out[1].s1, rgb_out[2].s1, rgb_out[3].s1); - const short ab = AVERAGE(rgb_out[0].s2, rgb_out[1].s2, rgb_out[2].s2, rgb_out[3].s2); - uchar2 uv = (uchar2)( - RGB_TO_U(ar, ag, ab), - RGB_TO_V(ar, ag, ab) - ); - vstore2(uv, 0, out + UV_OFFSET + mad24(gid_y, YUV_STRIDE, gid_x * 2)); +float3 apply_gamma(float3 rgb, int expo_time) { + return -0.507089*exp(-12.54124638*rgb) + 0.9655*powr(rgb, 0.5) - 0.472597*rgb + 0.507089; } + +#endif \ No newline at end of file diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 4e2194d914..add514b117 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -12,10 +12,6 @@ #include "system/camerad/sensors/os04c10_registers.h" #define ANALOG_GAIN_MAX_CNT 55 -const size_t FRAME_WIDTH = 1928; -const size_t FRAME_HEIGHT = 1208; -const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) - class SensorInfo { public: @@ -26,12 +22,14 @@ public: virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {} cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN; + float pixel_size_mm; uint32_t frame_width, frame_height; uint32_t frame_stride; uint32_t frame_offset = 0; uint32_t extra_height = 0; int registers_offset = -1; int stats_offset = -1; + int hdr_offset = -1; int exposure_time_min; int exposure_time_max; diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py index 8c1b6084c7..4ba38c1df4 100755 --- a/system/camerad/snapshot/snapshot.py +++ b/system/camerad/snapshot/snapshot.py @@ -6,12 +6,12 @@ import numpy as np from PIL import Image import cereal.messaging as messaging -from cereal.visionipc import VisionIpcClient, VisionStreamType +from msgq.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.system.hardware import PC from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert -from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes VISION_STREAMS = { @@ -83,7 +83,7 @@ def snapshot(): front_camera_allowed = params.get_bool("RecordFront") params.put_bool("IsTakingSnapshot", True) set_offroad_alert("Offroad_IsTakingSnapshot", True) - time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start + time.sleep(2.0) # Give hardwared time to read the param, or if just started give camerad time to start # Check if camerad is already started try: diff --git a/system/camerad/test/.gitignore b/system/camerad/test/.gitignore new file mode 100644 index 0000000000..d67473ebcd --- /dev/null +++ b/system/camerad/test/.gitignore @@ -0,0 +1,2 @@ +jpegs/ +test_ae_gray diff --git a/system/camerad/test/check_skips.py b/system/camerad/test/check_skips.py new file mode 100755 index 0000000000..0814ce44ff --- /dev/null +++ b/system/camerad/test/check_skips.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 +# type: ignore +import cereal.messaging as messaging + +all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState'] +prev_id = [None,None,None] +this_id = [None,None,None] +dt = [None,None,None] +num_skipped = [0,0,0] + +if __name__ == "__main__": + sm = messaging.SubMaster(all_sockets) + while True: + sm.update() + + for i in range(len(all_sockets)): + if not sm.updated[all_sockets[i]]: + continue + this_id[i] = sm[all_sockets[i]].frameId + if prev_id[i] is None: + prev_id[i] = this_id[i] + continue + dt[i] = this_id[i] - prev_id[i] + if dt[i] != 1: + num_skipped[i] += dt[i] - 1 + print(all_sockets[i] ,dt[i] - 1, num_skipped[i]) + prev_id[i] = this_id[i] diff --git a/system/camerad/test/get_thumbnails_for_segment.py b/system/camerad/test/get_thumbnails_for_segment.py new file mode 100755 index 0000000000..21409f398d --- /dev/null +++ b/system/camerad/test/get_thumbnails_for_segment.py @@ -0,0 +1,24 @@ +#!/usr/bin/env python3 +import argparse +import os +from tqdm import tqdm + +from openpilot.tools.lib.logreader import LogReader + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("route", help="The route name") + args = parser.parse_args() + + out_path = os.path.join("jpegs", f"{args.route.replace('|', '_').replace('/', '_')}") + os.makedirs(out_path, exist_ok=True) + + lr = LogReader(args.route) + + for msg in tqdm(lr): + if msg.which() == 'thumbnail': + with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.thumbnail.thumbnail) + elif msg.which() == 'navThumbnail': + with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f: + f.write(msg.navThumbnail.thumbnail) diff --git a/system/camerad/test/stress_restart.sh b/system/camerad/test/stress_restart.sh new file mode 100755 index 0000000000..0445dcba79 --- /dev/null +++ b/system/camerad/test/stress_restart.sh @@ -0,0 +1,9 @@ +#!/bin/sh +cd .. +while :; do + ./camerad & + pid="$!" + sleep 2 + kill -2 $pid + wait $pid +done diff --git a/system/camerad/test/test_ae_gray.cc b/system/camerad/test/test_ae_gray.cc new file mode 100644 index 0000000000..be9a0cc59f --- /dev/null +++ b/system/camerad/test/test_ae_gray.cc @@ -0,0 +1,84 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" + +#include + +#include +#include + +#include "common/util.h" +#include "system/camerad/cameras/camera_common.h" + +#define W 240 +#define H 160 + + +#define TONE_SPLITS 3 + +float gts[TONE_SPLITS * TONE_SPLITS * TONE_SPLITS * TONE_SPLITS] = { + 0.917969, 0.917969, 0.375000, 0.917969, 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.917969, + 0.375000, 0.375000, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.917969, 0.375000, 0.375000, + 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.187500, 0.093750, 0.093750, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, 0.093750, + 0.093750, 0.093750, 0.093750, 0.093750, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, + 0.000000}; + + +TEST_CASE("camera.test_set_exposure_target") { + // set up fake camerabuf + CameraBuf cb = {}; + VisionBuf vb = {}; + uint8_t * fb_y = new uint8_t[W*H]; + vb.y = fb_y; + cb.cur_yuv_buf = &vb; + cb.rgb_width = W; + cb.rgb_height = H; + Rect rect = {0, 0, W-1, H-1}; + + printf("AE test patterns %dx%d\n", cb.rgb_width, cb.rgb_height); + + // mix of 5 tones + uint8_t l[5] = {0, 24, 48, 96, 235}; // 235 is yuv max + + bool passed = true; + float rtol = 0.05; + // generate pattern and calculate EV + int cnt = 0; + for (int i_0=0; i_0 rtol*evgt) { + passed = false; + } + + // report + printf("%d/%d/%d/%d/%d: ev %f, gt %f, err %f\n", h_0, h_1, h_2, h_3, h_4, ev, evgt, fabs(ev - evgt) / (evgt != 0 ? evgt : 0.00001f)); + cnt++; + } + } + } + } + assert(passed); + + delete[] fb_y; +} diff --git a/system/camerad/test/test_camerad.py b/system/camerad/test/test_camerad.py new file mode 100644 index 0000000000..ada9594895 --- /dev/null +++ b/system/camerad/test/test_camerad.py @@ -0,0 +1,82 @@ +import pytest +import time +import numpy as np +from flaky import flaky +from collections import defaultdict + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.system.manager.process_config import managed_processes + +TEST_TIMESPAN = 30 +LAG_FRAME_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 0.5, # ARs use synced pulses for frame starts + log.FrameData.ImageSensor.ox03c10: 1.1} # OXs react to out-of-sync at next frame +FRAME_DELTA_TOLERANCE = {log.FrameData.ImageSensor.ar0231: 1.0, + log.FrameData.ImageSensor.ox03c10: 1.0} + +CAMERAS = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState') + +# TODO: this shouldn't be needed +@flaky(max_runs=3) +@pytest.mark.tici +class TestCamerad: + def setup_method(self): + # run camerad and record logs + managed_processes['camerad'].start() + time.sleep(3) + socks = {c: messaging.sub_sock(c, conflate=False, timeout=100) for c in CAMERAS} + + self.logs = defaultdict(list) + start_time = time.monotonic() + while time.monotonic()- start_time < TEST_TIMESPAN: + for cam, s in socks.items(): + self.logs[cam] += messaging.drain_sock(s) + time.sleep(0.2) + managed_processes['camerad'].stop() + + self.log_by_frame_id = defaultdict(list) + self.sensor_type = None + for cam, msgs in self.logs.items(): + if self.sensor_type is None: + self.sensor_type = getattr(msgs[0], msgs[0].which()).sensor.raw + expected_frames = SERVICE_LIST[cam].frequency * TEST_TIMESPAN + assert expected_frames*0.95 < len(msgs) < expected_frames*1.05, f"unexpected frame count {cam}: {expected_frames=}, got {len(msgs)}" + + dts = np.abs(np.diff([getattr(m, m.which()).timestampSof/1e6 for m in msgs]) - 1000/SERVICE_LIST[cam].frequency) + assert (dts < FRAME_DELTA_TOLERANCE[self.sensor_type]).all(), f"{cam} dts(ms) out of spec: max diff {dts.max()}, 99 percentile {np.percentile(dts, 99)}" + + for m in msgs: + self.log_by_frame_id[getattr(m, m.which()).frameId].append(m) + + # strip beginning and end + for _ in range(3): + mn, mx = min(self.log_by_frame_id.keys()), max(self.log_by_frame_id.keys()) + del self.log_by_frame_id[mn] + del self.log_by_frame_id[mx] + + def test_frame_skips(self): + skips = {} + frame_ids = self.log_by_frame_id.keys() + for frame_id in range(min(frame_ids), max(frame_ids)): + seen_cams = [msg.which() for msg in self.log_by_frame_id[frame_id]] + skip_cams = set(CAMERAS) - set(seen_cams) + if len(skip_cams): + skips[frame_id] = skip_cams + assert len(skips) == 0, f"Found frame skips, missing cameras for the following frames: {skips}" + + def test_frame_sync(self): + frame_times = {frame_id: [getattr(m, m.which()).timestampSof for m in msgs] for frame_id, msgs in self.log_by_frame_id.items()} + diffs = {frame_id: (max(ts) - min(ts))/1e6 for frame_id, ts in frame_times.items()} + + def get_desc(fid, diff): + cam_times = [(m.which(), getattr(m, m.which()).timestampSof/1e6) for m in self.log_by_frame_id[fid]] + return (diff, cam_times) + laggy_frames = {k: get_desc(k, v) for k, v in diffs.items() if v > LAG_FRAME_TOLERANCE[self.sensor_type]} + + def in_tol(diff): + return 50 - LAG_FRAME_TOLERANCE[self.sensor_type] < diff and diff < 50 + LAG_FRAME_TOLERANCE[self.sensor_type] + if len(laggy_frames) != 0 and all( in_tol(laggy_frames[lf][0]) for lf in laggy_frames): + print("TODO: handle camera out of sync") + else: + assert len(laggy_frames) == 0, f"Frames not synced properly: {laggy_frames=}" diff --git a/system/camerad/test/test_exposure.py b/system/camerad/test/test_exposure.py new file mode 100644 index 0000000000..dbe6f3d880 --- /dev/null +++ b/system/camerad/test/test_exposure.py @@ -0,0 +1,50 @@ +import time +import numpy as np + +from openpilot.selfdrive.test.helpers import with_processes, phone_only +from openpilot.system.camerad.snapshot.snapshot import get_snapshots + +TEST_TIME = 45 +REPEAT = 5 + +class TestCamerad: + @classmethod + def setup_class(cls): + pass + + def _numpy_rgb2gray(self, im): + ret = np.clip(im[:,:,2] * 0.114 + im[:,:,1] * 0.587 + im[:,:,0] * 0.299, 0, 255).astype(np.uint8) + return ret + + def _is_exposure_okay(self, i, med_mean=None): + if med_mean is None: + med_mean = np.array([[0.2,0.4],[0.2,0.6]]) + h, w = i.shape[:2] + i = i[h//10:9*h//10,w//10:9*w//10] + med_ex, mean_ex = med_mean + i = self._numpy_rgb2gray(i) + i_median = np.median(i) / 255. + i_mean = np.mean(i) / 255. + print([i_median, i_mean]) + return med_ex[0] < i_median < med_ex[1] and mean_ex[0] < i_mean < mean_ex[1] + + @phone_only + @with_processes(['camerad']) + def test_camera_operation(self): + passed = 0 + start = time.time() + while time.time() - start < TEST_TIME and passed < REPEAT: + rpic, dpic = get_snapshots(frame="roadCameraState", front_frame="driverCameraState") + wpic, _ = get_snapshots(frame="wideRoadCameraState") + + res = self._is_exposure_okay(rpic) + res = res and self._is_exposure_okay(dpic) + res = res and self._is_exposure_okay(wpic) + + if passed > 0 and not res: + passed = -passed # fails test if any failure after first sus + break + + passed += int(res) + time.sleep(2) + assert passed >= REPEAT diff --git a/system/hardware/.gitignore b/system/hardware/.gitignore new file mode 100644 index 0000000000..980f09abfa --- /dev/null +++ b/system/hardware/.gitignore @@ -0,0 +1 @@ +eon/rat diff --git a/system/hardware/base.h b/system/hardware/base.h index dc2282a93a..ca24633a18 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -5,7 +5,7 @@ #include #include -#include "cereal/messaging/messaging.h" +#include "cereal/gen/cpp/log.capnp.h" // no-op base hw class class HardwareNone { diff --git a/system/hardware/base.py b/system/hardware/base.py index 7434bb61e8..6cdb4a4d64 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -1,6 +1,5 @@ from abc import abstractmethod, ABC from collections import namedtuple -from typing import Dict from cereal import log @@ -10,7 +9,7 @@ NetworkType = log.DeviceState.NetworkType class HardwareBase(ABC): @staticmethod - def get_cmdline() -> Dict[str, str]: + def get_cmdline() -> dict[str, str]: with open('/proc/cmdline') as f: cmdline = f.read() return {kv[0]: kv[1] for kv in [s.split('=') for s in cmdline.split(' ')] if len(kv) == 2} diff --git a/selfdrive/thermald/fan_controller.py b/system/hardware/fan_controller.py similarity index 94% rename from selfdrive/thermald/fan_controller.py rename to system/hardware/fan_controller.py index 19c3292ce2..f32133f6bf 100755 --- a/selfdrive/thermald/fan_controller.py +++ b/system/hardware/fan_controller.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from abc import ABC, abstractmethod -from openpilot.common.realtime import DT_TRML +from openpilot.common.realtime import DT_HW from openpilot.common.numpy_fast import interp from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.pid import PIDController @@ -18,7 +18,7 @@ class TiciFanController(BaseFanController): cloudlog.info("Setting up TICI fan handler") self.last_ignition = False - self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_TRML)) + self.controller = PIDController(k_p=0, k_i=4e-3, k_f=1, rate=(1 / DT_HW)) def update(self, cur_temp: float, ignition: bool) -> int: self.controller.neg_limit = -(100 if ignition else 30) diff --git a/selfdrive/thermald/thermald.py b/system/hardware/hardwared.py similarity index 93% rename from selfdrive/thermald/thermald.py rename to system/hardware/hardwared.py index e868f2ffdd..e3a4c81711 100755 --- a/selfdrive/thermald/thermald.py +++ b/system/hardware/hardwared.py @@ -6,7 +6,6 @@ import threading import time from collections import OrderedDict, namedtuple from pathlib import Path -from typing import Dict, Optional, Tuple import psutil @@ -16,14 +15,14 @@ from cereal.services import SERVICE_LIST from openpilot.common.dict_helpers import strip_deprecated_keys from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params -from openpilot.common.realtime import DT_TRML +from openpilot.common.realtime import DT_HW from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, TICI, AGNOS from openpilot.system.loggerd.config import get_available_percent -from openpilot.selfdrive.statsd import statlog +from openpilot.system.statsd import statlog from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.thermald.power_monitoring import PowerMonitoring -from openpilot.selfdrive.thermald.fan_controller import TiciFanController +from openpilot.system.hardware.power_monitoring import PowerMonitoring +from openpilot.system.hardware.fan_controller import TiciFanController from openpilot.system.version import terms_version, training_version ThermalStatus = log.DeviceState.ThermalStatus @@ -50,9 +49,9 @@ THERMAL_BANDS = OrderedDict({ # Override to highest thermal band when offroad and above this temp OFFROAD_DANGER_TEMP = 75 -prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {} +prev_offroad_states: dict[str, tuple[bool, str | None]] = {} -tz_by_type: Optional[Dict[str, int]] = None +tz_by_type: dict[str, int] | None = None def populate_tz_by_type(): global tz_by_type tz_by_type = {} @@ -87,7 +86,7 @@ def read_thermal(thermal_config): return dat -def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: Optional[str]=None): +def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_text: str | None=None): if prev_offroad_states.get(offroad_alert, None) == (show_alert, extra_text): return prev_offroad_states[offroad_alert] = (show_alert, extra_text) @@ -107,7 +106,7 @@ def hw_state_thread(end_event, hw_queue): while not end_event.is_set(): # these are expensive calls. update every 10s - if (count % int(10. / DT_TRML)) == 0: + if (count % int(10. / DT_HW)) == 0: try: network_type = HARDWARE.get_network_type() modem_temps = HARDWARE.get_modem_temperatures() @@ -160,25 +159,25 @@ def hw_state_thread(end_event, hw_queue): cloudlog.exception("Error getting hardware state") count += 1 - time.sleep(DT_TRML) + time.sleep(DT_HW) -def thermald_thread(end_event, hw_queue) -> None: +def hardware_thread(end_event, hw_queue) -> None: pm = messaging.PubMaster(['deviceState']) sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll="pandaStates") count = 0 - onroad_conditions: Dict[str, bool] = { + onroad_conditions: dict[str, bool] = { "ignition": False, } - startup_conditions: Dict[str, bool] = {} - startup_conditions_prev: Dict[str, bool] = {} + startup_conditions: dict[str, bool] = {} + startup_conditions_prev: dict[str, bool] = {} - off_ts: Optional[float] = None - started_ts: Optional[float] = None + off_ts: float | None = None + started_ts: float | None = None started_seen = False - startup_blocked_ts: Optional[float] = None + startup_blocked_ts: float | None = None thermal_status = ThermalStatus.yellow last_hw_state = HardwareState( @@ -191,8 +190,8 @@ def thermald_thread(end_event, hw_queue) -> None: modem_temps=[], ) - all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML, initialized=False) - offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML, initialized=False) + all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False) + offroad_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False) should_start_prev = False in_car = False engaged_prev = False @@ -208,16 +207,10 @@ def thermald_thread(end_event, hw_queue) -> None: while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) - # Run at 2Hz - if sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_TRML) != 0: - continue - pandaStates = sm['pandaStates'] peripheralState = sm['peripheralState'] peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown - msg = read_thermal(thermal_config) - if sm.updated['pandaStates'] and len(pandaStates) > 0: # Set ignition based on any panda connected @@ -237,6 +230,14 @@ def thermald_thread(end_event, hw_queue) -> None: onroad_conditions["ignition"] = False cloudlog.error("panda timed out onroad") + # Run at 2Hz, plus rising edge of ignition + ign_edge = started_ts is None and onroad_conditions["ignition"] + if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge: + continue + + msg = read_thermal(thermal_config) + msg.deviceState.deviceType = HARDWARE.get_device_type() + try: last_hw_state = hw_queue.get_nowait() except queue.Empty: @@ -348,7 +349,7 @@ def thermald_thread(end_event, hw_queue) -> None: try: with open('/dev/kmsg', 'w') as kmsg: - kmsg.write(f"<3>[thermald] engaged: {engaged}\n") + kmsg.write(f"<3>[hardware] engaged: {engaged}\n") except Exception: pass @@ -422,7 +423,7 @@ def thermald_thread(end_event, hw_queue) -> None: # report to server once every 10 minutes rising_edge_started = should_start and not should_start_prev - if rising_edge_started or (count % int(600. / DT_TRML)) == 0: + if rising_edge_started or (count % int(600. / DT_HW)) == 0: dat = { 'count': count, 'pandaStates': [strip_deprecated_keys(p.to_dict()) for p in pandaStates], @@ -451,7 +452,7 @@ def main(): threads = [ threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), - threading.Thread(target=thermald_thread, args=(end_event, hw_queue)), + threading.Thread(target=hardware_thread, args=(end_event, hw_queue)), ] for t in threads: diff --git a/selfdrive/thermald/power_monitoring.py b/system/hardware/power_monitoring.py similarity index 96% rename from selfdrive/thermald/power_monitoring.py rename to system/hardware/power_monitoring.py index f74118b568..5a94625b48 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/system/hardware/power_monitoring.py @@ -1,11 +1,10 @@ import time import threading -from typing import Optional from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.statsd import statlog +from openpilot.system.statsd import statlog CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1)) @@ -38,7 +37,7 @@ class PowerMonitoring: self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) # Calculation tick - def calculate(self, voltage: Optional[int], ignition: bool): + def calculate(self, voltage: int | None, ignition: bool): try: now = time.monotonic() @@ -108,7 +107,7 @@ class PowerMonitoring: return int(self.car_battery_capacity_uWh) # See if we need to shutdown - def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: Optional[float], started_seen: bool): + def should_shutdown(self, ignition: bool, in_car: bool, offroad_timestamp: float | None, started_seen: bool): if offroad_timestamp is None: return False diff --git a/system/hardware/tests/__init__.py b/system/hardware/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/hardware/tests/test_fan_controller.py b/system/hardware/tests/test_fan_controller.py new file mode 100644 index 0000000000..002c1edfda --- /dev/null +++ b/system/hardware/tests/test_fan_controller.py @@ -0,0 +1,50 @@ +import pytest + +from openpilot.system.hardware.fan_controller import TiciFanController + +ALL_CONTROLLERS = [TiciFanController] + +def patched_controller(mocker, controller_class): + mocker.patch("os.system", new=mocker.Mock()) + return controller_class() + +class TestFanController: + def wind_up(self, controller, ignition=True): + for _ in range(1000): + controller.update(100, ignition) + + def wind_down(self, controller, ignition=False): + for _ in range(1000): + controller.update(10, ignition) + + @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) + def test_hot_onroad(self, mocker, controller_class): + controller = patched_controller(mocker, controller_class) + self.wind_up(controller) + assert controller.update(100, True) >= 70 + + @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) + def test_offroad_limits(self, mocker, controller_class): + controller = patched_controller(mocker, controller_class) + self.wind_up(controller) + assert controller.update(100, False) <= 30 + + @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) + def test_no_fan_wear(self, mocker, controller_class): + controller = patched_controller(mocker, controller_class) + self.wind_down(controller) + assert controller.update(10, False) == 0 + + @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) + def test_limited(self, mocker, controller_class): + controller = patched_controller(mocker, controller_class) + self.wind_up(controller, True) + assert controller.update(100, True) == 100 + + @pytest.mark.parametrize("controller_class", ALL_CONTROLLERS) + def test_windup_speed(self, mocker, controller_class): + controller = patched_controller(mocker, controller_class) + self.wind_down(controller, True) + for _ in range(10): + controller.update(90, True) + assert controller.update(90, True) >= 60 diff --git a/system/hardware/tests/test_power_monitoring.py b/system/hardware/tests/test_power_monitoring.py new file mode 100644 index 0000000000..1dff6c6c5f --- /dev/null +++ b/system/hardware/tests/test_power_monitoring.py @@ -0,0 +1,199 @@ +import pytest + +from openpilot.common.params import Params +from openpilot.system.hardware.power_monitoring import PowerMonitoring, CAR_BATTERY_CAPACITY_uWh, \ + CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING, DELAY_SHUTDOWN_TIME_S + +# Create fake time +ssb = 0. +def mock_time_monotonic(): + global ssb + ssb += 1. + return ssb + +TEST_DURATION_S = 50 +GOOD_VOLTAGE = 12 * 1e3 +VOLTAGE_BELOW_PAUSE_CHARGING = (VBATT_PAUSE_CHARGING - 1) * 1e3 + +def pm_patch(mocker, name, value, constant=False): + if constant: + mocker.patch(f"openpilot.system.hardware.power_monitoring.{name}", value) + else: + mocker.patch(f"openpilot.system.hardware.power_monitoring.{name}", return_value=value) + + +@pytest.fixture(autouse=True) +def mock_time(mocker): + mocker.patch("time.monotonic", mock_time_monotonic) + + +class TestPowerMonitoring: + def setup_method(self): + self.params = Params() + + # Test to see that it doesn't do anything when pandaState is None + def test_panda_state_present(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(None, None) + assert pm.get_power_used() == 0 + assert pm.get_car_battery_capacity() == (CAR_BATTERY_CAPACITY_uWh / 10) + + # Test to see that it doesn't integrate offroad when ignition is True + def test_offroad_ignition(self): + pm = PowerMonitoring() + for _ in range(10): + pm.calculate(GOOD_VOLTAGE, True) + assert pm.get_power_used() == 0 + + # Test to see that it integrates with discharging battery + def test_offroad_integration_discharging(self, mocker): + POWER_DRAW = 4 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + expected_power_usage = ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + assert abs(pm.get_power_used() - expected_power_usage) < 10 + + # Test to check positive integration of car_battery_capacity + def test_car_battery_integration_onroad(self, mocker): + POWER_DRAW = 4 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, True) + expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) + assert abs(pm.get_car_battery_capacity() - expected_capacity) < 10 + + # Test to check positive integration upper limit + def test_car_battery_integration_upper_limit(self, mocker): + POWER_DRAW = 4 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, True) + estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) + assert abs(pm.get_car_battery_capacity() - estimated_capacity) < 10 + + # Test to check negative integration of car_battery_capacity + def test_car_battery_integration_offroad(self, mocker): + POWER_DRAW = 4 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * POWER_DRAW * 1e6) + assert abs(pm.get_car_battery_capacity() - expected_capacity) < 10 + + # Test to check negative integration lower limit + def test_car_battery_integration_lower_limit(self, mocker): + POWER_DRAW = 4 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 1000 + for _ in range(TEST_DURATION_S + 1): + pm.calculate(GOOD_VOLTAGE, False) + estimated_capacity = 0 - ((1/3600) * POWER_DRAW * 1e6) + assert abs(pm.get_car_battery_capacity() - estimated_capacity) < 10 + + # Test to check policy of stopping charging after MAX_TIME_OFFROAD_S + def test_max_time_offroad(self, mocker): + MOCKED_MAX_OFFROAD_TIME = 3600 + POWER_DRAW = 0 # To stop shutting down for other reasons + pm_patch(mocker, "MAX_TIME_OFFROAD_S", MOCKED_MAX_OFFROAD_TIME, constant=True) + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + start_time = ssb + ignition = False + while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: + pm.calculate(GOOD_VOLTAGE, ignition) + if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: + assert not pm.should_shutdown(ignition, True, start_time, False) + assert pm.should_shutdown(ignition, True, start_time, False) + + def test_car_voltage(self, mocker): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 350 + VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S = 50 + pm_patch(mocker, "VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S", VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S, constant=True) + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + start_time = ssb + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + assert pm.should_shutdown(ignition, True, start_time, True) == \ + (pm.car_voltage_mV < VBATT_PAUSE_CHARGING * 1e3 and \ + (ssb - start_time) > VOLTAGE_SHUTDOWN_MIN_OFFROAD_TIME_S and \ + (ssb - start_time) > DELAY_SHUTDOWN_TIME_S) + assert pm.should_shutdown(ignition, True, start_time, True) + + # Test to check policy of not stopping charging when DisablePowerDown is set + def test_disable_power_down(self, mocker): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + self.params.put_bool("DisablePowerDown", True) + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = False + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + assert not pm.should_shutdown(ignition, True, ssb, False) + assert not pm.should_shutdown(ignition, True, ssb, False) + + # Test to check policy of not stopping charging when ignition + def test_ignition(self, mocker): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + ignition = True + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + assert not pm.should_shutdown(ignition, True, ssb, False) + assert not pm.should_shutdown(ignition, True, ssb, False) + + # Test to check policy of not stopping charging when harness is not connected + def test_harness_connection(self, mocker): + POWER_DRAW = 0 # To stop shutting down for other reasons + TEST_TIME = 100 + pm_patch(mocker, "HARDWARE.get_current_power_draw", POWER_DRAW) + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh + + ignition = False + for i in range(TEST_TIME): + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + if i % 10 == 0: + assert not pm.should_shutdown(ignition, False, ssb, False) + assert not pm.should_shutdown(ignition, False, ssb, False) + + def test_delay_shutdown_time(self): + pm = PowerMonitoring() + pm.car_battery_capacity_uWh = 0 + ignition = False + in_car = True + offroad_timestamp = ssb + started_seen = True + pm.calculate(VOLTAGE_BELOW_PAUSE_CHARGING, ignition) + + while ssb < offroad_timestamp + DELAY_SHUTDOWN_TIME_S: + assert not pm.should_shutdown(ignition, in_car, + offroad_timestamp, + started_seen), \ + f"Should not shutdown before {DELAY_SHUTDOWN_TIME_S} seconds offroad time" + assert pm.should_shutdown(ignition, in_car, + offroad_timestamp, + started_seen), \ + f"Should shutdown after {DELAY_SHUTDOWN_TIME_S} seconds offroad time" diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index e69842cfec..b1862bdef9 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,10 +1,10 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-f0de74e139b8b99224738d4e72a5b1831758f20b09ff6bb28f3aaaae1c4c1ebe.img.xz", - "hash": "f0de74e139b8b99224738d4e72a5b1831758f20b09ff6bb28f3aaaae1c4c1ebe", - "hash_raw": "f0de74e139b8b99224738d4e72a5b1831758f20b09ff6bb28f3aaaae1c4c1ebe", - "size": 15636480, + "url": "https://commadist.azureedge.net/agnosupdate/boot-5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f.img.xz", + "hash": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f", + "hash_raw": "5674ea6767e7198cf1e7def3de66a57061f001ed76d43dc4b4f84de545c53c6f", + "size": 16029696, "sparse": false, "full_check": true, "has_ab": true @@ -61,17 +61,17 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-0f69173d5f3058f7197c139442a6556be59e52f15402a263215a329ba5ec41e2.img.xz", - "hash": "4858385ba6284bcaa179ab77ac4263486e4d8670df921e4ac400464dc1dde59c", - "hash_raw": "0f69173d5f3058f7197c139442a6556be59e52f15402a263215a329ba5ec41e2", + "url": "https://commadist.azureedge.net/agnosupdate/system-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz", + "hash": "328e90c62068222dfd98f71dd3f6251fcb962f082b49c6be66ab2699f5db6f4f", + "hash_raw": "1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a", "size": 10737418240, "sparse": true, "full_check": false, "has_ab": true, "alt": { - "hash": "42658a6fff660d9b6abb9cb9fbb3481071259c9a9598718af6b1edff2b556009", - "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-0f69173d5f3058f7197c139442a6556be59e52f15402a263215a329ba5ec41e2.img.xz", - "size": 4548292756 + "hash": "bc11d2148f29862ee1326aca2af1cf6bbf5fed831e3f8f6b8f7a0f110dfe8d26", + "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-1badfe72851628d6cf9200a53a6151bb4e797b49c717141409fc57138eae388a.img.xz", + "size": 4548070000 } } ] \ No newline at end of file diff --git a/system/hardware/tici/agnos.py b/system/hardware/tici/agnos.py index 342281b0f8..7e3536f775 100755 --- a/system/hardware/tici/agnos.py +++ b/system/hardware/tici/agnos.py @@ -6,15 +6,17 @@ import os import struct import subprocess import time -from typing import Dict, Generator, List, Tuple, Union +from collections.abc import Generator import requests -import openpilot.system.hardware.tici.casync as casync +import openpilot.system.updated.casync.casync as casync SPARSE_CHUNK_FMT = struct.Struct('H2xI4x') CAIBX_URL = "https://commadist.azureedge.net/agnosupdate/" +AGNOS_MANIFEST_FILE = "system/hardware/tici/agnos.json" + class StreamingDecompressor: def __init__(self, url: str) -> None: @@ -117,7 +119,7 @@ def get_raw_hash(path: str, partition_size: int) -> str: return raw_hash.hexdigest().lower() -def verify_partition(target_slot_number: int, partition: Dict[str, Union[str, int]], force_full_check: bool = False) -> bool: +def verify_partition(target_slot_number: int, partition: dict[str, str | int], force_full_check: bool = False) -> bool: full_check = partition['full_check'] or force_full_check path = get_partition_path(target_slot_number, partition) @@ -184,7 +186,7 @@ def extract_casync_image(target_slot_number: int, partition: dict, cloudlog): target = casync.parse_caibx(partition['casync_caibx']) - sources: List[Tuple[str, casync.ChunkReader, casync.ChunkDict]] = [] + sources: list[tuple[str, casync.ChunkReader, casync.ChunkDict]] = [] # First source is the current partition. try: diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py index e003f131cc..af82067467 100755 --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -2,7 +2,6 @@ import time from smbus2 import SMBus from collections import namedtuple -from typing import List # https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf @@ -110,7 +109,7 @@ class Amplifier: def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig: return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000) - def _set_configs(self, configs: List[AmpConfig]) -> None: + def _set_configs(self, configs: list[AmpConfig]) -> None: with SMBus(self.AMP_I2C_BUS) as bus: for config in configs: if self.debug: @@ -123,7 +122,7 @@ class Amplifier: if self.debug: print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") - def set_configs(self, configs: List[AmpConfig]) -> bool: + def set_configs(self, configs: list[AmpConfig]) -> bool: # retry in case panda is using the amp tries = 15 for i in range(15): diff --git a/system/hardware/tici/esim.py b/system/hardware/tici/esim.py new file mode 100755 index 0000000000..df76c1a5fd --- /dev/null +++ b/system/hardware/tici/esim.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +import os +import math +import time +import binascii +import requests +import serial +import subprocess + + +def post(url, payload): + print() + print("POST to", url) + r = requests.post( + url, + data=payload, + verify=False, + headers={ + "Content-Type": "application/json", + "X-Admin-Protocol": "gsma/rsp/v2.2.0", + "charset": "utf-8", + "User-Agent": "gsma-rsp-lpad", + }, + ) + print("resp", r) + print("resp text", repr(r.text)) + print() + r.raise_for_status() + + ret = f"HTTP/1.1 {r.status_code}" + ret += ''.join(f"{k}: {v}" for k, v in r.headers.items() if k != 'Connection') + return ret.encode() + r.content + + +class LPA: + def __init__(self): + self.dev = serial.Serial('/dev/ttyUSB2', baudrate=57600, timeout=1, bytesize=8) + self.dev.reset_input_buffer() + self.dev.reset_output_buffer() + assert "OK" in self.at("AT") + + def at(self, cmd): + print(f"==> {cmd}") + self.dev.write(cmd.encode() + b'\r\n') + + r = b"" + cnt = 0 + while b"OK" not in r and b"ERROR" not in r and cnt < 20: + r += self.dev.read(8192).strip() + cnt += 1 + r = r.decode() + print(f"<== {repr(r)}") + return r + + def download_ota(self, qr): + return self.at(f'AT+QESIM="ota","{qr}"') + + def download(self, qr): + smdp = qr.split('$')[1] + out = self.at(f'AT+QESIM="download","{qr}"') + for _ in range(5): + line = out.split("+QESIM: ")[1].split("\r\n\r\nOK")[0] + + parts = [x.strip().strip('"') for x in line.split(',', maxsplit=4)] + print(repr(parts)) + trans, ret, url, payloadlen, payload = parts + assert trans == "trans" and ret == "0" + assert len(payload) == int(payloadlen) + + r = post(f"https://{smdp}/{url}", payload) + to_send = binascii.hexlify(r).decode() + + chunk_len = 1400 + for i in range(math.ceil(len(to_send) / chunk_len)): + state = 1 if (i+1)*chunk_len < len(to_send) else 0 + data = to_send[i * chunk_len : (i+1)*chunk_len] + out = self.at(f'AT+QESIM="trans",{len(to_send)},{state},{i},{len(data)},"{data}"') + assert "OK" in out + + if '+QESIM:"download",1' in out: + raise Exception("profile install failed") + elif '+QESIM:"download",0' in out: + print("done, successfully loaded") + break + + def enable(self, iccid): + self.at(f'AT+QESIM="enable","{iccid}"') + + def disable(self, iccid): + self.at(f'AT+QESIM="disable","{iccid}"') + + def delete(self, iccid): + self.at(f'AT+QESIM="delete","{iccid}"') + + def list_profiles(self): + out = self.at('AT+QESIM="list"') + return out.strip().splitlines()[1:] + + +if __name__ == "__main__": + import sys + + if "RESTART" in os.environ: + subprocess.check_call("sudo systemctl stop ModemManager", shell=True) + subprocess.check_call("/usr/comma/lte/lte.sh stop_blocking", shell=True) + subprocess.check_call("/usr/comma/lte/lte.sh start", shell=True) + while not os.path.exists('/dev/ttyUSB2'): + time.sleep(1) + time.sleep(3) + + lpa = LPA() + print(lpa.list_profiles()) + if len(sys.argv) > 1: + lpa.download(sys.argv[1]) + print(lpa.list_profiles()) diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index e553a665a8..f1d1f1e717 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -20,12 +20,12 @@ public: } static std::string get_name() { - std::string devicetree_model = util::read_file("/sys/firmware/devicetree/base/model"); - return (devicetree_model.find("tizi") != std::string::npos) ? "tizi" : "tici"; + std::string model = util::read_file("/sys/firmware/devicetree/base/model"); + return model.substr(std::string("comma ").size()); } static cereal::InitData::DeviceType get_device_type() { - return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : cereal::InitData::DeviceType::TICI; + return (get_name() == "tizi") ? cereal::InitData::DeviceType::TIZI : (get_name() == "mici" ? cereal::InitData::DeviceType::MICI : cereal::InitData::DeviceType::TICI); } static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); } diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 5bb1032ba9..a5dee88b56 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -94,11 +94,7 @@ def get_device_type(): # lru_cache and cache can cause memory leaks when used in classes with open("/sys/firmware/devicetree/base/model") as f: model = f.read().strip('\x00') - model = model.split('comma ')[-1] - # TODO: remove this with AGNOS 7+ - if model.startswith('Qualcomm'): - model = 'tici' - return model + return model.split('comma ')[-1] class Tici(HardwareBase): @cached_property @@ -116,6 +112,8 @@ class Tici(HardwareBase): @cached_property def amplifier(self): + if self.get_device_type() == "mici": + return None return Amplifier() def get_os_version(self): @@ -374,29 +372,26 @@ class Tici(HardwareBase): def set_power_save(self, powersave_enabled): # amplifier, 100mW at idle - self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) - if not powersave_enabled: - self.amplifier.initialize_configuration(self.get_device_type()) + if self.amplifier is not None: + self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) + if not powersave_enabled: + self.amplifier.initialize_configuration(self.get_device_type()) # *** CPU config *** - # offline big cluster, leave core 4 online for boardd - for i in range(5, 8): + # offline big cluster, leave core 4 online for pandad + for i in range(4, 8): val = '0' if powersave_enabled else '1' sudo_write(val, f'/sys/devices/system/cpu/cpu{i}/online') for n in ('0', '4'): + if powersave_enabled and n == '4': + continue gov = 'ondemand' if powersave_enabled else 'performance' sudo_write(gov, f'/sys/devices/system/cpu/cpufreq/policy{n}/scaling_governor') # *** IRQ config *** - # boardd core - affine_irq(4, "spi_geni") # SPI - affine_irq(4, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB) - if "tici" in self.get_device_type(): - affine_irq(4, "xhci-hcd:usb1") # internal panda USB (also modem) - # GPU affine_irq(5, "kgsl-3d0") @@ -414,9 +409,10 @@ class Tici(HardwareBase): return 0 def initialize_hardware(self): - self.amplifier.initialize_configuration(self.get_device_type()) + if self.amplifier is not None: + self.amplifier.initialize_configuration(self.get_device_type()) - # Allow thermald to write engagement status to kmsg + # Allow hardwared to write engagement status to kmsg os.system("sudo chmod a+w /dev/kmsg") # Ensure fan gpio is enabled so fan runs until shutdown, also turned on at boot by the ABL @@ -453,6 +449,18 @@ class Tici(HardwareBase): sudo_write("N", "/sys/kernel/debug/msm_vidc/clock_scaling") sudo_write("Y", "/sys/kernel/debug/msm_vidc/disable_thermal_mitigation") + # pandad core + affine_irq(3, "spi_geni") # SPI + if "tici" in self.get_device_type(): + affine_irq(3, "xhci-hcd:usb3") # aux panda USB (or potentially anything else on USB) + affine_irq(3, "xhci-hcd:usb1") # internal panda USB (also modem) + try: + pid = subprocess.check_output(["pgrep", "-f", "spi0"], encoding='utf8').strip() + subprocess.call(["sudo", "chrt", "-f", "-p", "1", pid]) + subprocess.call(["sudo", "taskset", "-pc", "3", pid]) + except subprocess.CalledProcessException as e: + print(str(e)) + def configure_modem(self): sim_id = self.get_sim_info().get('sim_id', '') @@ -468,8 +476,9 @@ class Tici(HardwareBase): # use sim slot 'AT^SIMSWAP=1', - # configure ECM mode - 'AT$QCPCFG=usbNet,1' + # ethernet config + 'AT$QCPCFG=usbNet,0', + 'AT$QCNETDEVCTL=3,1', ] else: cmds += [ @@ -478,6 +487,12 @@ class Tici(HardwareBase): 'AT+QNVFW="/nv/item_files/ims/IMS_enable",00', 'AT+QNVFW="/nv/item_files/modem/mmode/ue_usage_setting",01', ] + if self.get_device_type() == "tizi": + cmds += [ + # SIM hot swap + 'AT+QSIMDET=1,0', + 'AT+QSIMSTAT=1', + ] # clear out old blue prime initial APN os.system('mmcli -m any --3gpp-set-initial-eps-bearer-settings="apn="') diff --git a/system/hardware/tici/power_monitor.py b/system/hardware/tici/power_monitor.py new file mode 100755 index 0000000000..296290dae8 --- /dev/null +++ b/system/hardware/tici/power_monitor.py @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 +import sys +import time +import datetime +import numpy as np +from collections import deque + +from openpilot.common.realtime import Ratekeeper +from openpilot.common.filter_simple import FirstOrderFilter + + +def read_power(): + with open("/sys/bus/i2c/devices/0-0040/hwmon/hwmon1/power1_input") as f: + return int(f.read()) / 1e6 + +def sample_power(seconds=5) -> list[float]: + rate = 123 + rk = Ratekeeper(rate, print_delay_threshold=None) + + pwrs = [] + for _ in range(rate*seconds): + pwrs.append(read_power()) + rk.keep_time() + return pwrs + +def get_power(seconds=5): + pwrs = sample_power(seconds) + return np.mean(pwrs) + +def wait_for_power(min_pwr, max_pwr, min_secs_in_range, timeout): + start_time = time.monotonic() + pwrs = deque([min_pwr - 1.]*min_secs_in_range, maxlen=min_secs_in_range) + while (time.monotonic() - start_time < timeout): + pwrs.append(get_power(1)) + if all(min_pwr <= p <= max_pwr for p in pwrs): + break + return np.mean(pwrs) + + +if __name__ == "__main__": + duration = None + if len(sys.argv) > 1: + duration = int(sys.argv[1]) + + rate = 23 + rk = Ratekeeper(rate, print_delay_threshold=None) + fltr = FirstOrderFilter(0, 5, 1. / rate, initialized=False) + + measurements = [] + start_time = time.monotonic() + + try: + while duration is None or time.monotonic() - start_time < duration: + fltr.update(read_power()) + if rk.frame % rate == 0: + measurements.append(fltr.x) + t = datetime.timedelta(seconds=time.monotonic() - start_time) + avg = sum(measurements) / len(measurements) + print(f"Now: {fltr.x:.2f} W, Avg: {avg:.2f} W over {t}") + rk.keep_time() + except KeyboardInterrupt: + pass + + t = datetime.timedelta(seconds=time.monotonic() - start_time) + avg = sum(measurements) / len(measurements) + print(f"\nAverage power: {avg:.2f}W over {t}") diff --git a/system/hardware/tici/precise_power_measure.py b/system/hardware/tici/precise_power_measure.py new file mode 100755 index 0000000000..52fe0850ab --- /dev/null +++ b/system/hardware/tici/precise_power_measure.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python3 +import numpy as np +from openpilot.system.hardware.tici.power_monitor import sample_power + +if __name__ == '__main__': + print("measuring for 5 seconds") + for _ in range(3): + pwrs = sample_power() + print(f"mean {np.mean(pwrs):.2f} std {np.std(pwrs):.2f}") diff --git a/system/hardware/tici/restart_modem.sh b/system/hardware/tici/restart_modem.sh new file mode 100755 index 0000000000..3c67d9d21d --- /dev/null +++ b/system/hardware/tici/restart_modem.sh @@ -0,0 +1,18 @@ +#!/usr/bin/bash + +#nmcli connection modify --temporary lte gsm.home-only yes +#nmcli connection modify --temporary lte gsm.auto-config yes +#nmcli connection modify --temporary lte connection.autoconnect-retries 20 +sudo nmcli connection reload + +sudo systemctl stop ModemManager +nmcli con down lte +nmcli con down blue-prime + +# power cycle modem +/usr/comma/lte/lte.sh stop_blocking +/usr/comma/lte/lte.sh start + +sudo systemctl restart NetworkManager +#sudo systemctl restart ModemManager +sudo ModemManager --debug diff --git a/system/hardware/tici/tests/__init__.py b/system/hardware/tici/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/hardware/tici/tests/compare_casync_manifest.py b/system/hardware/tici/tests/compare_casync_manifest.py new file mode 100755 index 0000000000..7de66d91d0 --- /dev/null +++ b/system/hardware/tici/tests/compare_casync_manifest.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +import argparse +import collections +import multiprocessing +import os + +import requests +from tqdm import tqdm + +import openpilot.system.hardware.tici.casync as casync + + +def get_chunk_download_size(chunk): + sha = chunk.sha.hex() + path = os.path.join(remote_url, sha[:4], sha + ".cacnk") + if os.path.isfile(path): + return os.path.getsize(path) + else: + r = requests.head(path, timeout=10) + r.raise_for_status() + return int(r.headers['content-length']) + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description='Compute overlap between two casync manifests') + parser.add_argument('frm') + parser.add_argument('to') + args = parser.parse_args() + + frm = casync.parse_caibx(args.frm) + to = casync.parse_caibx(args.to) + remote_url = args.to.replace('.caibx', '') + + most_common = collections.Counter(t.sha for t in to).most_common(1)[0][0] + + frm_dict = casync.build_chunk_dict(frm) + + # Get content-length for each chunk + with multiprocessing.Pool() as pool: + szs = list(tqdm(pool.imap(get_chunk_download_size, to), total=len(to))) + chunk_sizes = {t.sha: sz for (t, sz) in zip(to, szs, strict=True)} + + sources: dict[str, list[int]] = { + 'seed': [], + 'remote_uncompressed': [], + 'remote_compressed': [], + } + + for chunk in to: + # Assume most common chunk is the zero chunk + if chunk.sha == most_common: + continue + + if chunk.sha in frm_dict: + sources['seed'].append(chunk.length) + else: + sources['remote_uncompressed'].append(chunk.length) + sources['remote_compressed'].append(chunk_sizes[chunk.sha]) + + print() + print("Update statistics (excluding zeros)") + print() + print("Download only with no seed:") + print(f" Remote (uncompressed)\t\t{sum(sources['seed'] + sources['remote_uncompressed']) / 1000 / 1000:.2f} MB\tn = {len(to)}") + print(f" Remote (compressed download)\t{sum(chunk_sizes.values()) / 1000 / 1000:.2f} MB\tn = {len(to)}") + print() + print("Upgrade with seed partition:") + print(f" Seed (uncompressed)\t\t{sum(sources['seed']) / 1000 / 1000:.2f} MB\t\t\t\tn = {len(sources['seed'])}") + sz, n = sum(sources['remote_uncompressed']), len(sources['remote_uncompressed']) + print(f" Remote (uncompressed)\t\t{sz / 1000 / 1000:.2f} MB\t(avg {sz / 1000 / 1000 / n:4f} MB)\tn = {n}") + sz, n = sum(sources['remote_compressed']), len(sources['remote_compressed']) + print(f" Remote (compressed download)\t{sz / 1000 / 1000:.2f} MB\t(avg {sz / 1000 / 1000 / n:4f} MB)\tn = {n}") diff --git a/system/hardware/tici/tests/test_agnos_updater.py b/system/hardware/tici/tests/test_agnos_updater.py new file mode 100644 index 0000000000..a1bbd363fd --- /dev/null +++ b/system/hardware/tici/tests/test_agnos_updater.py @@ -0,0 +1,20 @@ +import json +import os +import requests + +TEST_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__))) +MANIFEST = os.path.join(TEST_DIR, "../agnos.json") + + +class TestAgnosUpdater: + + def test_manifest(self): + with open(MANIFEST) as f: + m = json.load(f) + + for img in m: + r = requests.head(img['url'], timeout=10) + r.raise_for_status() + assert r.headers['Content-Type'] == "application/x-xz" + if not img['sparse']: + assert img['hash'] == img['hash_raw'] diff --git a/system/hardware/tici/tests/test_amplifier.py b/system/hardware/tici/tests/test_amplifier.py new file mode 100644 index 0000000000..3f75436db1 --- /dev/null +++ b/system/hardware/tici/tests/test_amplifier.py @@ -0,0 +1,70 @@ +import pytest +import time +import random +import subprocess + +from panda import Panda +from openpilot.system.hardware import TICI, HARDWARE +from openpilot.system.hardware.tici.hardware import Tici +from openpilot.system.hardware.tici.amplifier import Amplifier + + +class TestAmplifier: + + @classmethod + def setup_class(cls): + if not TICI: + pytest.skip() + + def setup_method(self): + # clear dmesg + subprocess.check_call("sudo dmesg -C", shell=True) + + HARDWARE.reset_internal_panda() + Panda.wait_for_panda(None, 30) + self.panda = Panda() + + def teardown_method(self): + HARDWARE.reset_internal_panda() + + def _check_for_i2c_errors(self, expected): + dmesg = subprocess.check_output("dmesg", shell=True, encoding='utf8') + i2c_lines = [l for l in dmesg.strip().splitlines() if 'i2c_geni a88000.i2c' in l] + i2c_str = '\n'.join(i2c_lines) + + if not expected: + return len(i2c_lines) == 0 + else: + return "i2c error :-107" in i2c_str or "Bus arbitration lost" in i2c_str + + def test_init(self): + amp = Amplifier(debug=True) + r = amp.initialize_configuration(Tici().get_device_type()) + assert r + assert self._check_for_i2c_errors(False) + + def test_shutdown(self): + amp = Amplifier(debug=True) + for _ in range(10): + r = amp.set_global_shutdown(True) + r = amp.set_global_shutdown(False) + # amp config should be successful, with no i2c errors + assert r + assert self._check_for_i2c_errors(False) + + def test_init_while_siren_play(self): + for _ in range(10): + self.panda.set_siren(False) + time.sleep(0.1) + + self.panda.set_siren(True) + time.sleep(random.randint(0, 5)) + + amp = Amplifier(debug=True) + r = amp.initialize_configuration(Tici().get_device_type()) + assert r + + if self._check_for_i2c_errors(True): + break + else: + pytest.fail("didn't hit any i2c errors") diff --git a/system/hardware/tici/tests/test_hardware.py b/system/hardware/tici/tests/test_hardware.py new file mode 100644 index 0000000000..75f53e7cdb --- /dev/null +++ b/system/hardware/tici/tests/test_hardware.py @@ -0,0 +1,26 @@ +import pytest +import time +import numpy as np + +from openpilot.system.hardware.tici.hardware import Tici + +HARDWARE = Tici() + + +@pytest.mark.tici +class TestHardware: + + def test_power_save_time(self): + ts = {True: [], False: []} + for _ in range(5): + for on in (True, False): + st = time.monotonic() + HARDWARE.set_power_save(on) + ts[on].append(time.monotonic() - st) + + # disabling power save is the main time-critical one + assert 0.1 < np.mean(ts[False]) < 0.15 + assert max(ts[False]) < 0.2 + + assert 0.1 < np.mean(ts[True]) < 0.35 + assert max(ts[True]) < 0.4 diff --git a/system/hardware/tici/tests/test_power_draw.py b/system/hardware/tici/tests/test_power_draw.py new file mode 100644 index 0000000000..866f7d1188 --- /dev/null +++ b/system/hardware/tici/tests/test_power_draw.py @@ -0,0 +1,127 @@ +from collections import defaultdict, deque +import pytest +import time +import numpy as np +from dataclasses import dataclass +from tabulate import tabulate + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.mock import mock_messages +from openpilot.selfdrive.car.car_helpers import write_car_param +from openpilot.system.hardware.tici.power_monitor import get_power +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.manager.manager import manager_cleanup + +SAMPLE_TIME = 8 # seconds to sample power +MAX_WARMUP_TIME = 30 # seconds to wait for SAMPLE_TIME consecutive valid samples + +@dataclass +class Proc: + procs: list[str] + power: float + msgs: list[str] + rtol: float = 0.05 + atol: float = 0.12 + + @property + def name(self): + return '+'.join(self.procs) + + +PROCS = [ + Proc(['camerad'], 2.1, msgs=['roadCameraState', 'wideRoadCameraState', 'driverCameraState']), + Proc(['modeld'], 1.12, atol=0.2, msgs=['modelV2']), + Proc(['dmonitoringmodeld'], 0.4, msgs=['driverStateV2']), + Proc(['encoderd'], 0.23, msgs=[]), +] + + +@pytest.mark.tici +class TestPowerDraw: + + def setup_method(self): + write_car_param() + + # wait a bit for power save to disable + time.sleep(5) + + def teardown_method(self): + manager_cleanup() + + def get_expected_messages(self, proc): + return int(sum(SAMPLE_TIME * SERVICE_LIST[msg].frequency for msg in proc.msgs)) + + def valid_msg_count(self, proc, msg_counts): + msgs_received = sum(msg_counts[msg] for msg in proc.msgs) + msgs_expected = self.get_expected_messages(proc) + return np.core.numeric.isclose(msgs_expected, msgs_received, rtol=.02, atol=2) + + def valid_power_draw(self, proc, used): + return np.core.numeric.isclose(used, proc.power, rtol=proc.rtol, atol=proc.atol) + + def tabulate_msg_counts(self, msgs_and_power): + msg_counts = defaultdict(int) + for _, counts in msgs_and_power: + for msg, count in counts.items(): + msg_counts[msg] += count + return msg_counts + + def get_power_with_warmup_for_target(self, proc, prev): + socks = {msg: messaging.sub_sock(msg) for msg in proc.msgs} + for sock in socks.values(): + messaging.drain_sock_raw(sock) + + msgs_and_power = deque([], maxlen=SAMPLE_TIME) + + start_time = time.monotonic() + + while (time.monotonic() - start_time) < MAX_WARMUP_TIME: + power = get_power(1) + iteration_msg_counts = {} + for msg,sock in socks.items(): + iteration_msg_counts[msg] = len(messaging.drain_sock_raw(sock)) + msgs_and_power.append((power, iteration_msg_counts)) + + if len(msgs_and_power) < SAMPLE_TIME: + continue + + msg_counts = self.tabulate_msg_counts(msgs_and_power) + now = np.mean([m[0] for m in msgs_and_power]) + + if self.valid_msg_count(proc, msg_counts) and self.valid_power_draw(proc, now - prev): + break + + return now, msg_counts, time.monotonic() - start_time - SAMPLE_TIME + + @mock_messages(['liveLocationKalman']) + def test_camera_procs(self, subtests): + baseline = get_power() + + prev = baseline + used = {} + warmup_time = {} + msg_counts = {} + + for proc in PROCS: + for p in proc.procs: + managed_processes[p].start() + now, local_msg_counts, warmup_time[proc.name] = self.get_power_with_warmup_for_target(proc, prev) + msg_counts.update(local_msg_counts) + + used[proc.name] = now - prev + prev = now + + manager_cleanup() + + tab = [['process', 'expected (W)', 'measured (W)', '# msgs expected', '# msgs received', "warmup time (s)"]] + for proc in PROCS: + cur = used[proc.name] + expected = proc.power + msgs_received = sum(msg_counts[msg] for msg in proc.msgs) + tab.append([proc.name, round(expected, 2), round(cur, 2), self.get_expected_messages(proc), msgs_received, round(warmup_time[proc.name], 2)]) + with subtests.test(proc=proc.name): + assert self.valid_msg_count(proc, msg_counts), f"expected {self.get_expected_messages(proc)} msgs, got {msgs_received} msgs" + assert self.valid_power_draw(proc, cur), f"expected {expected:.2f}W, got {cur:.2f}W" + print(tabulate(tab)) + print(f"Baseline {baseline:.2f}W\n") diff --git a/system/logcatd/SConscript b/system/logcatd/SConscript index 6bd7c6ff3e..ac2a79a1f2 100644 --- a/system/logcatd/SConscript +++ b/system/logcatd/SConscript @@ -1,3 +1,3 @@ -Import('env', 'cereal', 'messaging', 'common') +Import('env', 'messaging', 'common') -env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[cereal, messaging, common, 'zmq', 'capnp', 'kj', 'systemd', 'json11']) +env.Program('logcatd', 'logcatd_systemd.cc', LIBS=[messaging, common, 'systemd', 'json11']) diff --git a/system/loggerd/README.md b/system/loggerd/README.md new file mode 100644 index 0000000000..714e4242a0 --- /dev/null +++ b/system/loggerd/README.md @@ -0,0 +1,30 @@ +# loggerd + +openpilot records routes in one minute chunks called segments. A route starts on the rising edge of ignition and ends on the falling edge. + +Check out our [python library](https://github.com/commaai/openpilot/blob/master/tools/lib/logreader.py) for reading openpilot logs. Also checkout our [tools](https://github.com/commaai/openpilot/tree/master/tools) to replay and view your data. These are the same tools we use to debug and develop openpilot. + +## log types + +For each segment, openpilot records the following log types: + +## rlog.bz2 + +rlogs contain all the messages passed amongst openpilot's processes. See [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for a list of all the logged services. They're a bzip2 archive of the serialized capnproto messages. + +## {f,e,d}camera.hevc + +Each camera stream is H.265 encoded and written to its respective file. +* fcamera.hevc is the road camera +* ecamera.hevc is the wide road camera +* dcamera.hevc is the driver camera + +## qlog.bz2 & qcamera.ts + +qlogs are a decimated subset of the rlogs. Check out [cereal/services.py](https://github.com/commaai/cereal/blob/master/services.py) for the decimation. + + +qcameras are H.264 encoded, lower res versions of the fcamera.hevc. The video shown in [comma connect](https://connect.comma.ai/) is from the qcameras. + + +qlogs and qcameras are designed to be small enough to upload instantly on slow internet and store forever, yet useful enough for most analysis and debugging. diff --git a/system/loggerd/SConscript b/system/loggerd/SConscript index d4f52fb5f1..196d18476a 100644 --- a/system/loggerd/SConscript +++ b/system/loggerd/SConscript @@ -1,9 +1,8 @@ -Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc') +Import('env', 'arch', 'messaging', 'common', 'visionipc') -libs = [common, cereal, messaging, visionipc, - 'zmq', 'capnp', 'kj', 'z', - 'avformat', 'avcodec', 'swscale', 'avutil', - 'yuv', 'OpenCL', 'pthread'] +libs = [common, messaging, visionipc, + 'z', 'avformat', 'avcodec', 'swscale', + 'avutil', 'yuv', 'OpenCL', 'pthread'] src = ['logger.cc', 'video_writer.cc', 'encoder/encoder.cc', 'encoder/v4l_encoder.cc'] if arch != "larch64": diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py index 868340150a..2f0b96c90e 100755 --- a/system/loggerd/deleter.py +++ b/system/loggerd/deleter.py @@ -2,7 +2,6 @@ import os import shutil import threading -from typing import List from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import cloudlog from openpilot.system.loggerd.config import get_available_bytes, get_available_percent @@ -23,7 +22,7 @@ def has_preserve_xattr(d: str) -> bool: return getxattr(os.path.join(Paths.log_root(), d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE -def get_preserved_segments(dirs_by_creation: List[str]) -> List[str]: +def get_preserved_segments(dirs_by_creation: list[str]) -> list[str]: preserved = [] for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))): if n == PRESERVE_COUNT: diff --git a/system/loggerd/encoder/encoder.cc b/system/loggerd/encoder/encoder.cc index c4bd91bcf7..313a0f57a1 100644 --- a/system/loggerd/encoder/encoder.cc +++ b/system/loggerd/encoder/encoder.cc @@ -6,7 +6,12 @@ VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in out_width = encoder_info.frame_width > 0 ? encoder_info.frame_width : in_width; out_height = encoder_info.frame_height > 0 ? encoder_info.frame_height : in_height; - pm.reset(new PubMaster({encoder_info.publish_name})); + + std::vector pubs = {encoder_info.publish_name}; + if (encoder_info.thumbnail_name != NULL) { + pubs.push_back(encoder_info.thumbnail_name); + } + pm.reset(new PubMaster(pubs)); } void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, @@ -40,4 +45,15 @@ void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t kj::ArrayOutputStream output_stream(kj::ArrayPtr(e->msg_cache.data(), bytes_size)); capnp::writeMessage(output_stream, msg); e->pm->send(e->encoder_info.publish_name, e->msg_cache.data(), bytes_size); -} + + // Publish keyframe thumbnail + if ((flags & V4L2_BUF_FLAG_KEYFRAME) && e->encoder_info.thumbnail_name != NULL) { + MessageBuilder tm; + auto thumbnail = tm.initEvent().initThumbnail(); + thumbnail.setFrameId(extra.frame_id); + thumbnail.setTimestampEof(extra.timestamp_eof); + thumbnail.setThumbnail(dat); + thumbnail.setEncoding(cereal::Thumbnail::Encoding::KEYFRAME); + pm->send(e->encoder_info.thumbnail_name, tm); + } +} \ No newline at end of file diff --git a/system/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h index 7c203f9193..72848609ef 100644 --- a/system/loggerd/encoder/encoder.h +++ b/system/loggerd/encoder/encoder.h @@ -7,7 +7,7 @@ #include #include "cereal/messaging/messaging.h" -#include "cereal/visionipc/visionipc.h" +#include "msgq/visionipc/visionipc.h" #include "common/queue.h" #include "system/camerad/cameras/camera_common.h" #include "system/loggerd/loggerd.h" @@ -25,6 +25,8 @@ public: void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat); protected: + void publish_thumbnail(uint32_t frame_id, uint64_t timestamp_eof, kj::ArrayPtr dat); + int in_width, in_height; int out_width, out_height; const EncoderInfo encoder_info; diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc index 2bd2863126..853a17abbe 100644 --- a/system/loggerd/encoder/v4l_encoder.cc +++ b/system/loggerd/encoder/v4l_encoder.cc @@ -16,7 +16,12 @@ #define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000 #define V4L2_QCOM_BUF_FLAG_EOS 0x02000000 -// echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level +/* + kernel debugging: + echo 0xff > /sys/module/videobuf2_core/parameters/debug + echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level + echo 0xff > /sys/devices/platform/soc/aa00000.qcom,vidc/video4linux/video33/dev_debug +*/ const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0; static void checked_ioctl(int fd, unsigned long request, void *argp) { diff --git a/system/loggerd/logger.cc b/system/loggerd/logger.cc index 7a829a2f1f..8234cd84ad 100644 --- a/system/loggerd/logger.cc +++ b/system/loggerd/logger.cc @@ -89,15 +89,6 @@ kj::Array logger_build_init_data() { return capnp::messageToFlatArray(msg); } -std::string logger_get_route_name() { - char route_name[64] = {'\0'}; - time_t rawtime = time(NULL); - struct tm timeinfo; - localtime_r(&rawtime, &timeinfo); - strftime(route_name, sizeof(route_name), "%Y-%m-%d--%H-%M-%S", &timeinfo); - return route_name; -} - std::string logger_get_identifier(std::string key) { // a log identifier is a 32 bit counter, plus a 10 character unique ID. // e.g. 000001a3--c20ba54385 @@ -105,7 +96,7 @@ std::string logger_get_identifier(std::string key) { Params params; uint32_t cnt; try { - cnt = std::stol(params.get(key)); + cnt = std::stoul(params.get(key)); } catch (std::exception &e) { cnt = 0; } @@ -122,16 +113,16 @@ std::string logger_get_identifier(std::string key) { return util::string_format("%08x--%s", cnt, ss.str().c_str()); } -static void log_sentinel(LoggerState *log, SentinelType type, int eixt_signal = 0) { +static void log_sentinel(LoggerState *log, SentinelType type, int exit_signal = 0) { MessageBuilder msg; auto sen = msg.initEvent().initSentinel(); sen.setType(type); - sen.setSignal(eixt_signal); + sen.setSignal(exit_signal); log->write(msg.toBytes(), true); } LoggerState::LoggerState(const std::string &log_root) { - route_name = logger_get_route_name(); + route_name = logger_get_identifier("RouteCount"); route_path = log_root + "/" + route_name; init_data = logger_build_init_data(); } diff --git a/system/loggerd/logger.h b/system/loggerd/logger.h index dd3bee150c..7a8490d57a 100644 --- a/system/loggerd/logger.h +++ b/system/loggerd/logger.h @@ -52,5 +52,4 @@ protected: }; kj::Array logger_build_init_data(); -std::string logger_get_route_name(); std::string logger_get_identifier(std::string key); diff --git a/system/loggerd/loggerd.cc b/system/loggerd/loggerd.cc index 3c0ffc1667..a324479fb5 100644 --- a/system/loggerd/loggerd.cc +++ b/system/loggerd/loggerd.cc @@ -215,7 +215,7 @@ void loggerd_thread() { const bool encoder = util::ends_with(it.name, "EncodeData"); const bool livestream_encoder = util::starts_with(it.name, "livestream"); if (!it.should_log && (!encoder || livestream_encoder)) continue; - LOGD("logging %s (on port %d)", it.name.c_str(), it.port); + LOGD("logging %s", it.name.c_str()); SubSocket * sock = SubSocket::create(ctx.get(), it.name); assert(sock != NULL); diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index ea288f4861..7c80ba51a2 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -4,7 +4,7 @@ #include "cereal/messaging/messaging.h" #include "cereal/services.h" -#include "cereal/visionipc/visionipc_client.h" +#include "msgq/visionipc/visionipc_client.h" #include "system/camerad/cameras/camera_common.h" #include "system/hardware/hw.h" #include "common/params.h" @@ -33,6 +33,7 @@ constexpr char PRESERVE_ATTR_VALUE = '1'; class EncoderInfo { public: const char *publish_name; + const char *thumbnail_name = NULL; const char *filename = NULL; bool record = true; int frame_width = -1; @@ -76,6 +77,7 @@ const EncoderInfo main_driver_encoder_info = { const EncoderInfo stream_road_encoder_info = { .publish_name = "livestreamRoadEncodeData", + //.thumbnail_name = "thumbnail", .encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .record = false, .bitrate = LIVESTREAM_BITRATE, diff --git a/system/loggerd/tests/__init__.py b/system/loggerd/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/loggerd/tests/loggerd_tests_common.py b/system/loggerd/tests/loggerd_tests_common.py new file mode 100644 index 0000000000..15fd997eb8 --- /dev/null +++ b/system/loggerd/tests/loggerd_tests_common.py @@ -0,0 +1,90 @@ +import os +import random +from pathlib import Path + + +import openpilot.system.loggerd.deleter as deleter +import openpilot.system.loggerd.uploader as uploader +from openpilot.common.params import Params +from openpilot.system.hardware.hw import Paths +from openpilot.system.loggerd.xattr_cache import setxattr + + +def create_random_file(file_path: Path, size_mb: float, lock: bool = False, upload_xattr: bytes = None) -> None: + file_path.parent.mkdir(parents=True, exist_ok=True) + + if lock: + lock_path = str(file_path) + ".lock" + os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL)) + + chunks = 128 + chunk_bytes = int(size_mb * 1024 * 1024 / chunks) + data = os.urandom(chunk_bytes) + + with open(file_path, "wb") as f: + for _ in range(chunks): + f.write(data) + + if upload_xattr is not None: + setxattr(str(file_path), uploader.UPLOAD_ATTR_NAME, upload_xattr) + +class MockResponse: + def __init__(self, text, status_code): + self.text = text + self.status_code = status_code + +class MockApi: + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}', 200) + + def get_token(self): + return "fake-token" + +class MockApiIgnore: + def __init__(self, dongle_id): + pass + + def get(self, *args, **kwargs): + return MockResponse('', 412) + + def get_token(self): + return "fake-token" + +class UploaderTestCase: + f_type = "UNKNOWN" + + root: Path + seg_num: int + seg_format: str + seg_format2: str + seg_dir: str + + def set_ignore(self): + uploader.Api = MockApiIgnore + + def setup_method(self): + uploader.Api = MockApi + uploader.fake_upload = True + uploader.force_wifi = True + uploader.allow_sleep = False + self.seg_num = random.randint(1, 300) + self.seg_format = "00000004--0ac3964c96--{}" + self.seg_format2 = "00000005--4c4e99b08b--{}" + self.seg_dir = self.seg_format.format(self.seg_num) + + self.params = Params() + self.params.put("IsOffroad", "1") + self.params.put("DongleId", "0000000000000000") + + def make_file_with_data(self, f_dir: str, fn: str, size_mb: float = .1, lock: bool = False, + upload_xattr: bytes = None, preserve_xattr: bytes = None) -> Path: + file_path = Path(Paths.log_root()) / f_dir / fn + create_random_file(file_path, size_mb, lock, upload_xattr) + + if preserve_xattr is not None: + setxattr(str(file_path.parent), deleter.PRESERVE_ATTR_NAME, preserve_xattr) + + return file_path diff --git a/system/loggerd/tests/test_deleter.py b/system/loggerd/tests/test_deleter.py new file mode 100644 index 0000000000..6222ea253b --- /dev/null +++ b/system/loggerd/tests/test_deleter.py @@ -0,0 +1,117 @@ +import time +import threading +from collections import namedtuple +from pathlib import Path +from collections.abc import Sequence + +import openpilot.system.loggerd.deleter as deleter +from openpilot.common.timeout import Timeout, TimeoutException +from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase + +Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize']) + + +class TestDeleter(UploaderTestCase): + def fake_statvfs(self, d): + return self.fake_stats + + def setup_method(self): + self.f_type = "fcamera.hevc" + super().setup_method() + self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096) + deleter.os.statvfs = self.fake_statvfs + + def start_thread(self): + self.end_event = threading.Event() + self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event]) + self.del_thread.daemon = True + self.del_thread.start() + + def join_thread(self): + self.end_event.set() + self.del_thread.join() + + def test_delete(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1) + + self.start_thread() + + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while f_path.exists(): + time.sleep(0.01) + finally: + self.join_thread() + + def assertDeleteOrder(self, f_paths: Sequence[Path], timeout: int = 5) -> None: + deleted_order = [] + + self.start_thread() + try: + with Timeout(timeout, "Timeout waiting for files to be deleted"): + while True: + for f in f_paths: + if not f.exists() and f not in deleted_order: + deleted_order.append(f) + if len(deleted_order) == len(f_paths): + break + time.sleep(0.01) + except TimeoutException: + print("Not deleted:", [f for f in f_paths if f not in deleted_order]) + raise + finally: + self.join_thread() + + assert deleted_order == f_paths, "Files not deleted in expected order" + + def test_delete_order(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(1), self.f_type), + self.make_file_with_data(self.seg_format2.format(0), self.f_type), + ]) + + def test_delete_many_preserved(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(1), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), + self.make_file_with_data(self.seg_format.format(2), self.f_type), + ] + [ + self.make_file_with_data(self.seg_format2.format(i), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE) + for i in range(5) + ]) + + def test_delete_last(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(1), self.f_type), + self.make_file_with_data(self.seg_format2.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(0), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), + self.make_file_with_data("boot", self.seg_format[:-4]), + self.make_file_with_data("crash", self.seg_format2[:-4]), + ]) + + def test_no_delete_when_available_space(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type) + + block_size = 4096 + available = (10 * 1024 * 1024 * 1024) / block_size # 10GB free + self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size) + + self.start_thread() + start_time = time.monotonic() + while f_path.exists() and time.monotonic() - start_time < 2: + time.sleep(0.01) + self.join_thread() + + assert f_path.exists(), "File deleted with available space" + + def test_no_delete_with_lock_file(self): + f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) + + self.start_thread() + start_time = time.monotonic() + while f_path.exists() and time.monotonic() - start_time < 2: + time.sleep(0.01) + self.join_thread() + + assert f_path.exists(), "File deleted when locked" diff --git a/system/loggerd/tests/test_encoder.py b/system/loggerd/tests/test_encoder.py new file mode 100644 index 0000000000..75862a9d45 --- /dev/null +++ b/system/loggerd/tests/test_encoder.py @@ -0,0 +1,150 @@ +import math +import os +import pytest +import random +import shutil +import subprocess +import time +from pathlib import Path + +from parameterized import parameterized +from tqdm import trange + +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.system.hardware import TICI +from openpilot.system.manager.process_config import managed_processes +from openpilot.tools.lib.logreader import LogReader +from openpilot.system.hardware.hw import Paths + +SEGMENT_LENGTH = 2 +FULL_SIZE = 2507572 +CAMERAS = [ + ("fcamera.hevc", 20, FULL_SIZE, "roadEncodeIdx"), + ("dcamera.hevc", 20, FULL_SIZE, "driverEncodeIdx"), + ("ecamera.hevc", 20, FULL_SIZE, "wideRoadEncodeIdx"), + ("qcamera.ts", 20, 130000, None), +] + +# we check frame count, so we don't have to be too strict on size +FILE_SIZE_TOLERANCE = 0.7 + + +@pytest.mark.tici # TODO: all of loggerd should work on PC +class TestEncoder: + + def setup_method(self): + self._clear_logs() + os.environ["LOGGERD_TEST"] = "1" + os.environ["LOGGERD_SEGMENT_LENGTH"] = str(SEGMENT_LENGTH) + + def teardown_method(self): + self._clear_logs() + + def _clear_logs(self): + if os.path.exists(Paths.log_root()): + shutil.rmtree(Paths.log_root()) + + def _get_latest_segment_path(self): + last_route = sorted(Path(Paths.log_root()).iterdir())[-1] + return os.path.join(Paths.log_root(), last_route) + + # TODO: this should run faster than real time + @parameterized.expand([(True, ), (False, )]) + def test_log_rotation(self, record_front): + Params().put_bool("RecordFront", record_front) + + managed_processes['sensord'].start() + managed_processes['loggerd'].start() + managed_processes['encoderd'].start() + + time.sleep(1.0) + managed_processes['camerad'].start() + + num_segments = int(os.getenv("SEGMENTS", random.randint(10, 15))) + + # wait for loggerd to make the dir for first segment + route_prefix_path = None + with Timeout(int(SEGMENT_LENGTH*3)): + while route_prefix_path is None: + try: + route_prefix_path = self._get_latest_segment_path().rsplit("--", 1)[0] + except Exception: + time.sleep(0.1) + + def check_seg(i): + # check each camera file size + counts = [] + first_frames = [] + for camera, fps, size, encode_idx_name in CAMERAS: + if not record_front and "dcamera" in camera: + continue + + file_path = f"{route_prefix_path}--{i}/{camera}" + + # check file exists + assert os.path.exists(file_path), f"segment #{i}: '{file_path}' missing" + + # TODO: this ffprobe call is really slow + # check frame count + cmd = f"ffprobe -v error -select_streams v:0 -count_packets -show_entries stream=nb_read_packets -of csv=p=0 {file_path}" + if TICI: + cmd = "LD_LIBRARY_PATH=/usr/local/lib " + cmd + + expected_frames = fps * SEGMENT_LENGTH + probe = subprocess.check_output(cmd, shell=True, encoding='utf8') + frame_count = int(probe.split('\n')[0].strip()) + counts.append(frame_count) + + assert frame_count == expected_frames, \ + f"segment #{i}: {camera} failed frame count check: expected {expected_frames}, got {frame_count}" + + # sanity check file size + file_size = os.path.getsize(file_path) + assert math.isclose(file_size, size, rel_tol=FILE_SIZE_TOLERANCE), \ + f"{file_path} size {file_size} isn't close to target size {size}" + + # Check encodeIdx + if encode_idx_name is not None: + rlog_path = f"{route_prefix_path}--{i}/rlog" + msgs = [m for m in LogReader(rlog_path) if m.which() == encode_idx_name] + encode_msgs = [getattr(m, encode_idx_name) for m in msgs] + + valid = [m.valid for m in msgs] + segment_idxs = [m.segmentId for m in encode_msgs] + encode_idxs = [m.encodeId for m in encode_msgs] + frame_idxs = [m.frameId for m in encode_msgs] + + # Check frame count + assert frame_count == len(segment_idxs) + assert frame_count == len(encode_idxs) + + # Check for duplicates or skips + assert 0 == segment_idxs[0] + assert len(set(segment_idxs)) == len(segment_idxs) + + assert all(valid) + + assert expected_frames * i == encode_idxs[0] + first_frames.append(frame_idxs[0]) + assert len(set(encode_idxs)) == len(encode_idxs) + + assert 1 == len(set(first_frames)) + + if TICI: + expected_frames = fps * SEGMENT_LENGTH + assert min(counts) == expected_frames + shutil.rmtree(f"{route_prefix_path}--{i}") + + try: + for i in trange(num_segments): + # poll for next segment + with Timeout(int(SEGMENT_LENGTH*10), error_msg=f"timed out waiting for segment {i}"): + while Path(f"{route_prefix_path}--{i+1}") not in Path(Paths.log_root()).iterdir(): + time.sleep(0.1) + check_seg(i) + finally: + managed_processes['loggerd'].stop() + managed_processes['encoderd'].stop() + managed_processes['camerad'].stop() + managed_processes['sensord'].stop() diff --git a/system/loggerd/tests/test_logger.cc b/system/loggerd/tests/test_logger.cc new file mode 100644 index 0000000000..2dae136e13 --- /dev/null +++ b/system/loggerd/tests/test_logger.cc @@ -0,0 +1,74 @@ +#include "catch2/catch.hpp" +#include "system/loggerd/logger.h" + +typedef cereal::Sentinel::SentinelType SentinelType; + +void verify_segment(const std::string &route_path, int segment, int max_segment, int required_event_cnt) { + const std::string segment_path = route_path + "--" + std::to_string(segment); + SentinelType begin_sentinel = segment == 0 ? SentinelType::START_OF_ROUTE : SentinelType::START_OF_SEGMENT; + SentinelType end_sentinel = segment == max_segment - 1 ? SentinelType::END_OF_ROUTE : SentinelType::END_OF_SEGMENT; + + REQUIRE(!util::file_exists(segment_path + "/rlog.lock")); + for (const char *fn : {"/rlog", "/qlog"}) { + const std::string log_file = segment_path + fn; + std::string log = util::read_file(log_file); + REQUIRE(!log.empty()); + int event_cnt = 0, i = 0; + kj::ArrayPtr words((capnp::word *)log.data(), log.size() / sizeof(capnp::word)); + while (words.size() > 0) { + try { + capnp::FlatArrayMessageReader reader(words); + auto event = reader.getRoot(); + words = kj::arrayPtr(reader.getEnd(), words.end()); + if (i == 0) { + REQUIRE(event.which() == cereal::Event::INIT_DATA); + } else if (i == 1) { + REQUIRE(event.which() == cereal::Event::SENTINEL); + REQUIRE(event.getSentinel().getType() == begin_sentinel); + REQUIRE(event.getSentinel().getSignal() == 0); + } else if (words.size() > 0) { + REQUIRE(event.which() == cereal::Event::CLOCKS); + ++event_cnt; + } else { + // the last event must be SENTINEL + REQUIRE(event.which() == cereal::Event::SENTINEL); + REQUIRE(event.getSentinel().getType() == end_sentinel); + REQUIRE(event.getSentinel().getSignal() == (end_sentinel == SentinelType::END_OF_ROUTE ? 1 : 0)); + } + ++i; + } catch (const kj::Exception &ex) { + INFO("failed parse " << i << " exception :" << ex.getDescription()); + REQUIRE(0); + break; + } + } + REQUIRE(event_cnt == required_event_cnt); + } +} + +void write_msg(LoggerState *logger) { + MessageBuilder msg; + msg.initEvent().initClocks(); + logger->write(msg.toBytes(), true); +} + +TEST_CASE("logger") { + const int segment_cnt = 100; + const std::string log_root = "/tmp/test_logger"; + system(("rm " + log_root + " -rf").c_str()); + std::string route_name; + { + LoggerState logger(log_root); + route_name = logger.routeName(); + for (int i = 0; i < segment_cnt; ++i) { + REQUIRE(logger.next()); + REQUIRE(util::file_exists(logger.segmentPath() + "/rlog.lock")); + REQUIRE(logger.segment() == i); + write_msg(&logger); + } + logger.setExitSignal(1); + } + for (int i = 0; i < segment_cnt; ++i) { + verify_segment(log_root + "/" + route_name, i, segment_cnt, 1); + } +} diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py new file mode 100644 index 0000000000..6a24540acf --- /dev/null +++ b/system/loggerd/tests/test_loggerd.py @@ -0,0 +1,284 @@ +import numpy as np +import os +import re +import random +import string +import subprocess +import time +from collections import defaultdict +from pathlib import Path +from flaky import flaky + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.common.basedir import BASEDIR +from openpilot.common.params import Params +from openpilot.common.timeout import Timeout +from openpilot.system.hardware.hw import Paths +from openpilot.system.loggerd.xattr_cache import getxattr +from openpilot.system.loggerd.deleter import PRESERVE_ATTR_NAME, PRESERVE_ATTR_VALUE +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.version import get_version +from openpilot.tools.lib.helpers import RE +from openpilot.tools.lib.logreader import LogReader +from msgq.visionipc import VisionIpcServer, VisionStreamType +from openpilot.common.transformations.camera import DEVICE_CAMERAS + +SentinelType = log.Sentinel.SentinelType + +CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in SERVICE_LIST + and SERVICE_LIST[f].should_log and "encode" not in f.lower()] + + +class TestLoggerd: + def _get_latest_log_dir(self): + log_dirs = sorted(Path(Paths.log_root()).iterdir(), key=lambda f: f.stat().st_mtime) + return log_dirs[-1] + + def _get_log_dir(self, x): + for l in x.splitlines(): + for p in l.split(' '): + path = Path(p.strip()) + if path.is_dir(): + return path + return None + + def _get_log_fn(self, x): + for l in x.splitlines(): + for p in l.split(' '): + path = Path(p.strip()) + if path.is_file(): + return path + return None + + def _gen_bootlog(self): + with Timeout(5): + out = subprocess.check_output("./bootlog", cwd=os.path.join(BASEDIR, "system/loggerd"), encoding='utf-8') + + log_fn = self._get_log_fn(out) + + # check existence + assert log_fn is not None + + return log_fn + + def _check_init_data(self, msgs): + msg = msgs[0] + assert msg.which() == 'initData' + + def _check_sentinel(self, msgs, route): + start_type = SentinelType.startOfRoute if route else SentinelType.startOfSegment + assert msgs[1].sentinel.type == start_type + + end_type = SentinelType.endOfRoute if route else SentinelType.endOfSegment + assert msgs[-1].sentinel.type == end_type + + def _publish_random_messages(self, services: list[str]) -> dict[str, list]: + pm = messaging.PubMaster(services) + + managed_processes["loggerd"].start() + for s in services: + assert pm.wait_for_readers_to_update(s, timeout=5) + + sent_msgs = defaultdict(list) + for _ in range(random.randint(2, 10) * 100): + for s in services: + try: + m = messaging.new_message(s) + except Exception: + m = messaging.new_message(s, random.randint(2, 10)) + pm.send(s, m) + sent_msgs[s].append(m) + + for s in services: + assert pm.wait_for_readers_to_update(s, timeout=5) + managed_processes["loggerd"].stop() + + return sent_msgs + + def test_init_data_values(self): + os.environ["CLEAN"] = random.choice(["0", "1"]) + + dongle = ''.join(random.choice(string.printable) for n in range(random.randint(1, 100))) + fake_params = [ + # param, initData field, value + ("DongleId", "dongleId", dongle), + ("GitCommit", "gitCommit", "commit"), + ("GitCommitDate", "gitCommitDate", "date"), + ("GitBranch", "gitBranch", "branch"), + ("GitRemote", "gitRemote", "remote"), + ] + params = Params() + for k, _, v in fake_params: + params.put(k, v) + params.put("AccessToken", "abc") + + lr = list(LogReader(str(self._gen_bootlog()))) + initData = lr[0].initData + + assert initData.dirty != bool(os.environ["CLEAN"]) + assert initData.version == get_version() + + if os.path.isfile("/proc/cmdline"): + with open("/proc/cmdline") as f: + assert list(initData.kernelArgs) == f.read().strip().split(" ") + + with open("/proc/version") as f: + assert initData.kernelVersion == f.read() + + # check params + logged_params = {entry.key: entry.value for entry in initData.params.entries} + expected_params = {k for k, _, __ in fake_params} | {'AccessToken', 'BootCount'} + assert set(logged_params.keys()) == expected_params, set(logged_params.keys()) ^ expected_params + assert logged_params['AccessToken'] == b'', f"DONT_LOG param value was logged: {repr(logged_params['AccessToken'])}" + for param_key, initData_key, v in fake_params: + assert getattr(initData, initData_key) == v + assert logged_params[param_key].decode() == v + + @flaky(max_runs=3) + def test_rotation(self): + os.environ["LOGGERD_TEST"] = "1" + Params().put("RecordFront", "1") + + d = DEVICE_CAMERAS[("tici", "ar0231")] + expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048*2346, 2048, 2048*1216), "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048*2346, 2048, 2048*1216), "driverCameraState"), + (VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] + + pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) + vipc_server = VisionIpcServer("camerad") + for stream_type, frame_spec, _ in streams: + vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec)) + vipc_server.start_listener() + + num_segs = random.randint(2, 5) + length = random.randint(1, 3) + os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length) + managed_processes["loggerd"].start() + managed_processes["encoderd"].start() + assert pm.wait_for_readers_to_update("roadCameraState", timeout=5) + + fps = 20.0 + for n in range(1, int(num_segs*length*fps)+1): + for stream_type, frame_spec, state in streams: + dat = np.empty(frame_spec[2], dtype=np.uint8) + vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) + + camera_state = messaging.new_message(state) + frame = getattr(camera_state, state) + frame.frameId = n + pm.send(state, camera_state) + + for _, _, state in streams: + assert pm.wait_for_readers_to_update(state, timeout=5, dt=0.001) + + managed_processes["loggerd"].stop() + managed_processes["encoderd"].stop() + + route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0] + for n in range(num_segs): + p = Path(f"{route_path}--{n}") + logged = {f.name for f in p.iterdir() if f.is_file()} + diff = logged ^ expected_files + assert len(diff) == 0, f"didn't get all expected files. run={_} seg={n} {route_path=}, {diff=}\n{logged=} {expected_files=}" + + def test_bootlog(self): + # generate bootlog with fake launch log + launch_log = ''.join(str(random.choice(string.printable)) for _ in range(100)) + with open("/tmp/launch_log", "w") as f: + f.write(launch_log) + + bootlog_path = self._gen_bootlog() + lr = list(LogReader(str(bootlog_path))) + + # check length + assert len(lr) == 2 # boot + initData + + self._check_init_data(lr) + + # check msgs + bootlog_msgs = [m for m in lr if m.which() == 'boot'] + assert len(bootlog_msgs) == 1 + + # sanity check values + boot = bootlog_msgs.pop().boot + assert abs(boot.wallTimeNanos - time.time_ns()) < 5*1e9 # within 5s + assert boot.launchLog == launch_log + + for fn in ["console-ramoops", "pmsg-ramoops-0"]: + path = Path(os.path.join("/sys/fs/pstore/", fn)) + if path.is_file(): + with open(path, "rb") as f: + expected_val = f.read() + bootlog_val = [e.value for e in boot.pstore.entries if e.key == fn][0] + assert expected_val == bootlog_val + + # next one should increment by one + bl1 = re.match(RE.LOG_ID_V2, bootlog_path.name) + bl2 = re.match(RE.LOG_ID_V2, self._gen_bootlog().name) + assert bl1.group('uid') != bl2.group('uid') + assert int(bl1.group('count')) == 0 and int(bl2.group('count')) == 1 + + def test_qlog(self): + qlog_services = [s for s in CEREAL_SERVICES if SERVICE_LIST[s].decimation is not None] + no_qlog_services = [s for s in CEREAL_SERVICES if SERVICE_LIST[s].decimation is None] + + services = random.sample(qlog_services, random.randint(2, min(10, len(qlog_services)))) + \ + random.sample(no_qlog_services, random.randint(2, min(10, len(no_qlog_services)))) + sent_msgs = self._publish_random_messages(services) + + qlog_path = os.path.join(self._get_latest_log_dir(), "qlog") + lr = list(LogReader(qlog_path)) + + # check initData and sentinel + self._check_init_data(lr) + self._check_sentinel(lr, True) + + recv_msgs = defaultdict(list) + for m in lr: + recv_msgs[m.which()].append(m) + + for s, msgs in sent_msgs.items(): + recv_cnt = len(recv_msgs[s]) + + if s in no_qlog_services: + # check services with no specific decimation aren't in qlog + assert recv_cnt == 0, f"got {recv_cnt} {s} msgs in qlog" + else: + # check logged message count matches decimation + expected_cnt = (len(msgs) - 1) // SERVICE_LIST[s].decimation + 1 + assert recv_cnt == expected_cnt, f"expected {expected_cnt} msgs for {s}, got {recv_cnt}" + + def test_rlog(self): + services = random.sample(CEREAL_SERVICES, random.randint(5, 10)) + sent_msgs = self._publish_random_messages(services) + + lr = list(LogReader(os.path.join(self._get_latest_log_dir(), "rlog"))) + + # check initData and sentinel + self._check_init_data(lr) + self._check_sentinel(lr, True) + + # check all messages were logged and in order + lr = lr[2:-1] # slice off initData and both sentinels + for m in lr: + sent = sent_msgs[m.which()].pop(0) + sent.clear_write_flag() + assert sent.to_bytes() == m.as_builder().to_bytes() + + def test_preserving_flagged_segments(self): + services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) | {"userFlag"} + self._publish_random_messages(services) + + segment_dir = self._get_latest_log_dir() + assert getxattr(segment_dir, PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE + + def test_not_preserving_unflagged_segments(self): + services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) - {"userFlag"} + self._publish_random_messages(services) + + segment_dir = self._get_latest_log_dir() + assert getxattr(segment_dir, PRESERVE_ATTR_NAME) is None + diff --git a/system/loggerd/tests/test_runner.cc b/system/loggerd/tests/test_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/system/loggerd/tests/test_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/system/loggerd/tests/test_uploader.py b/system/loggerd/tests/test_uploader.py new file mode 100644 index 0000000000..6591198281 --- /dev/null +++ b/system/loggerd/tests/test_uploader.py @@ -0,0 +1,184 @@ +import os +import time +import threading +import logging +import json +from pathlib import Path +from openpilot.system.hardware.hw import Paths + +from openpilot.common.swaglog import cloudlog +from openpilot.system.loggerd.uploader import main, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE + +from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase + + +class FakeLogHandler(logging.Handler): + def __init__(self): + logging.Handler.__init__(self) + self.reset() + + def reset(self): + self.upload_order = list() + self.upload_ignored = list() + + def emit(self, record): + try: + j = json.loads(record.getMessage()) + if j["event"] == "upload_success": + self.upload_order.append(j["key"]) + if j["event"] == "upload_ignored": + self.upload_ignored.append(j["key"]) + except Exception: + pass + +log_handler = FakeLogHandler() +cloudlog.addHandler(log_handler) + + +class TestUploader(UploaderTestCase): + def setup_method(self): + super().setup_method() + log_handler.reset() + + def start_thread(self): + self.end_event = threading.Event() + self.up_thread = threading.Thread(target=main, args=[self.end_event]) + self.up_thread.daemon = True + self.up_thread.start() + + def join_thread(self): + self.end_event.set() + self.up_thread.join() + + def gen_files(self, lock=False, xattr: bytes = None, boot=True) -> list[Path]: + f_paths = [] + for t in ["qlog", "rlog", "dcamera.hevc", "fcamera.hevc"]: + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock, upload_xattr=xattr)) + + if boot: + f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock, upload_xattr=xattr)) + return f_paths + + def gen_order(self, seg1: list[int], seg2: list[int], boot=True) -> list[str]: + keys = [] + if boot: + keys += [f"boot/{self.seg_format.format(i)}.bz2" for i in seg1] + keys += [f"boot/{self.seg_format2.format(i)}.bz2" for i in seg2] + keys += [f"{self.seg_format.format(i)}/qlog.bz2" for i in seg1] + keys += [f"{self.seg_format2.format(i)}/qlog.bz2" for i in seg2] + return keys + + def test_upload(self): + self.gen_files(lock=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + assert len(log_handler.upload_ignored) == 0, "Some files were ignored" + assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" + assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" + for f_path in exp_order: + assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" + + assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" + + def test_upload_with_wrong_xattr(self): + self.gen_files(lock=False, xattr=b'0') + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + assert len(log_handler.upload_ignored) == 0, "Some files were ignored" + assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" + assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" + for f_path in exp_order: + assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" + + assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" + + def test_upload_ignored(self): + self.set_ignore() + self.gen_files(lock=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + exp_order = self.gen_order([self.seg_num], []) + + assert len(log_handler.upload_order) == 0, "Some files were not ignored" + assert not len(log_handler.upload_ignored) < len(exp_order), "Some files failed to ignore" + assert not len(log_handler.upload_ignored) > len(exp_order), "Some files were ignored twice" + for f_path in exp_order: + assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not ignored" + + assert log_handler.upload_ignored == exp_order, "Files ignored in wrong order" + + def test_upload_files_in_create_order(self): + seg1_nums = [0, 1, 2, 10, 20] + for i in seg1_nums: + self.seg_dir = self.seg_format.format(i) + self.gen_files(boot=False) + seg2_nums = [5, 50, 51] + for i in seg2_nums: + self.seg_dir = self.seg_format2.format(i) + self.gen_files(boot=False) + + exp_order = self.gen_order(seg1_nums, seg2_nums, boot=False) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + assert len(log_handler.upload_ignored) == 0, "Some files were ignored" + assert not len(log_handler.upload_order) < len(exp_order), "Some files failed to upload" + assert not len(log_handler.upload_order) > len(exp_order), "Some files were uploaded twice" + for f_path in exp_order: + assert os.getxattr((Path(Paths.log_root()) / f_path).with_suffix(""), UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE, "All files not uploaded" + + assert log_handler.upload_order == exp_order, "Files uploaded in wrong order" + + def test_no_upload_with_lock_file(self): + self.start_thread() + + time.sleep(0.25) + f_paths = self.gen_files(lock=True, boot=False) + + # allow enough time that files should have been uploaded if they would be uploaded + time.sleep(5) + self.join_thread() + + for f_path in f_paths: + fn = f_path.with_suffix(f_path.suffix.replace(".bz2", "")) + uploaded = UPLOAD_ATTR_NAME in os.listxattr(fn) and os.getxattr(fn, UPLOAD_ATTR_NAME) == UPLOAD_ATTR_VALUE + assert not uploaded, "File upload when locked" + + def test_no_upload_with_xattr(self): + self.gen_files(lock=False, xattr=UPLOAD_ATTR_VALUE) + + self.start_thread() + # allow enough time that files could upload twice if there is a bug in the logic + time.sleep(5) + self.join_thread() + + assert len(log_handler.upload_order) == 0, "File uploaded again" + + def test_clear_locks_on_startup(self): + f_paths = self.gen_files(lock=True, boot=False) + self.start_thread() + time.sleep(1) + self.join_thread() + + for f_path in f_paths: + lock_path = f_path.with_suffix(f_path.suffix + ".lock") + assert not lock_path.is_file(), "File lock not cleared on startup" diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 5c8f25368c..832a227798 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -9,7 +9,8 @@ import threading import time import traceback import datetime -from typing import BinaryIO, Iterator, List, Optional, Tuple +from typing import BinaryIO +from collections.abc import Iterator from cereal import log import cereal.messaging as messaging @@ -42,10 +43,12 @@ class FakeResponse: self.request = FakeRequest() -def get_directory_sort(d: str) -> List[str]: - return [s.rjust(10, '0') for s in d.rsplit('--', 1)] +def get_directory_sort(d: str) -> list[str]: + # ensure old format is sorted sooner + o = ["0", ] if d.startswith("2024-") else ["1", ] + return o + [s.rjust(10, '0') for s in d.rsplit('--', 1)] -def listdir_by_creation(d: str) -> List[str]: +def listdir_by_creation(d: str) -> list[str]: if not os.path.isdir(d): return [] @@ -82,7 +85,7 @@ class Uploader: self.immediate_folders = ["crash/", "boot/"] self.immediate_priority = {"qlog": 0, "qlog.bz2": 0, "qcamera.ts": 1} - def list_upload_files(self, metered: bool) -> Iterator[Tuple[str, str, str]]: + def list_upload_files(self, metered: bool) -> Iterator[tuple[str, str, str]]: r = self.params.get("AthenadRecentlyViewedRoutes", encoding="utf8") requested_routes = [] if r is None else r.split(",") @@ -121,7 +124,7 @@ class Uploader: yield name, key, fn - def next_file_to_upload(self, metered: bool) -> Optional[Tuple[str, str, str]]: + def next_file_to_upload(self, metered: bool) -> tuple[str, str, str] | None: upload_files = list(self.list_upload_files(metered)) for name, key, fn in upload_files: @@ -207,7 +210,7 @@ class Uploader: return success - def step(self, network_type: int, metered: bool) -> Optional[bool]: + def step(self, network_type: int, metered: bool) -> bool | None: d = self.next_file_to_upload(metered) if d is None: return None @@ -221,7 +224,7 @@ class Uploader: return self.upload(name, key, fn, network_type, metered) -def main(exit_event: Optional[threading.Event] = None) -> None: +def main(exit_event: threading.Event = None) -> None: if exit_event is None: exit_event = threading.Event() diff --git a/system/loggerd/xattr_cache.py b/system/loggerd/xattr_cache.py index 5feeff34d2..d3220118ac 100644 --- a/system/loggerd/xattr_cache.py +++ b/system/loggerd/xattr_cache.py @@ -1,10 +1,9 @@ import os import errno -from typing import Dict, Optional, Tuple -_cached_attributes: Dict[Tuple, Optional[bytes]] = {} +_cached_attributes: dict[tuple, bytes | None] = {} -def getxattr(path: str, attr_name: str) -> Optional[bytes]: +def getxattr(path: str, attr_name: str) -> bytes | None: key = (path, attr_name) if key not in _cached_attributes: try: diff --git a/system/manager/__init__.py b/system/manager/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/manager/build.py b/system/manager/build.py similarity index 92% rename from selfdrive/manager/build.py rename to system/manager/build.py index f2758cf32b..956336a604 100755 --- a/selfdrive/manager/build.py +++ b/system/manager/build.py @@ -2,7 +2,6 @@ import os import subprocess from pathlib import Path -from typing import List # NOTE: Do NOT import anything here that needs be built (e.g. params) from openpilot.common.basedir import BASEDIR @@ -10,12 +9,12 @@ from openpilot.common.spinner import Spinner from openpilot.common.text_window import TextWindow from openpilot.system.hardware import AGNOS from openpilot.common.swaglog import cloudlog, add_file_handler -from openpilot.system.version import is_dirty +from openpilot.system.version import get_build_metadata 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 = 2560 +TOTAL_SCONS_NODES = 2820 MAX_BUILD_PROGRESS = 100 def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: @@ -29,7 +28,7 @@ def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: # building with all cores can result in using too # much memory, so retry with less parallelism - compile_output: List[bytes] = [] + compile_output: list[bytes] = [] for n in (nproc, nproc/2, 1): compile_output.clear() scons: subprocess.Popen = subprocess.Popen(["scons", f"-j{int(n)}", "--cache-populate", *extra_args], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) @@ -87,4 +86,5 @@ def build(spinner: Spinner, dirty: bool = False, minimal: bool = False) -> None: if __name__ == "__main__": spinner = Spinner() spinner.update_progress(0, 100) - build(spinner, is_dirty(), minimal = AGNOS) + build_metadata = get_build_metadata() + build(spinner, build_metadata.openpilot.is_dirty, minimal = AGNOS) diff --git a/selfdrive/manager/helpers.py b/system/manager/helpers.py similarity index 100% rename from selfdrive/manager/helpers.py rename to system/manager/helpers.py diff --git a/selfdrive/manager/manager.py b/system/manager/manager.py similarity index 74% rename from selfdrive/manager/manager.py rename to system/manager/manager.py index bf1eeb8fd0..6d1b8d9c22 100755 --- a/selfdrive/manager/manager.py +++ b/system/manager/manager.py @@ -4,36 +4,35 @@ import os import signal import sys import traceback -from typing import List, Tuple, Union from cereal import log import cereal.messaging as messaging -import openpilot.selfdrive.sentry as sentry +import openpilot.system.sentry as sentry from openpilot.common.params import Params, ParamKeyType from openpilot.common.text_window import TextWindow from openpilot.system.hardware import HARDWARE, PC -from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog -from openpilot.selfdrive.manager.process import ensure_running -from openpilot.selfdrive.manager.process_config import managed_processes -from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID +from openpilot.system.manager.helpers import unblock_stdout, write_onroad_params, save_bootlog +from openpilot.system.manager.process import ensure_running +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.athena.registration import register, UNREGISTERED_DONGLE_ID from openpilot.common.swaglog import cloudlog, add_file_handler -from openpilot.system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ - get_normalized_origin, terms_version, training_version, \ - is_tested_branch, is_release_branch, get_commit_date +from openpilot.system.version import get_build_metadata, terms_version, training_version def manager_init() -> None: save_bootlog() + build_metadata = get_build_metadata() + params = Params() params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) - if is_release_branch(): + if build_metadata.release_channel: params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) - default_params: List[Tuple[str, Union[str, bytes]]] = [ + default_params: list[tuple[str, str | bytes]] = [ ("CompletedTrainingVersion", "0"), ("DisengageOnAccelerator", "0"), ("GsmMetered", "1"), @@ -62,15 +61,15 @@ def manager_init() -> None: print("WARNING: failed to make /dev/shm") # set version params - params.put("Version", get_version()) + params.put("Version", build_metadata.openpilot.version) params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) - params.put("GitCommit", get_commit()) - params.put("GitCommitDate", get_commit_date()) - params.put("GitBranch", get_short_branch()) - params.put("GitRemote", get_origin()) - params.put_bool("IsTestedBranch", is_tested_branch()) - params.put_bool("IsReleaseBranch", is_release_branch()) + params.put("GitCommit", build_metadata.openpilot.git_commit) + params.put("GitCommitDate", build_metadata.openpilot.git_commit_date) + params.put("GitBranch", build_metadata.channel) + params.put("GitRemote", build_metadata.openpilot.git_origin) + params.put_bool("IsTestedBranch", build_metadata.tested_channel) + params.put_bool("IsReleaseBranch", build_metadata.release_channel) # set dongle id reg_res = register(show_spinner=True) @@ -80,21 +79,21 @@ def manager_init() -> None: serial = params.get("HardwareSerial") raise Exception(f"Registration failed for device {serial}") os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog - os.environ['GIT_ORIGIN'] = get_normalized_origin() # Needed for swaglog - os.environ['GIT_BRANCH'] = get_short_branch() # Needed for swaglog - os.environ['GIT_COMMIT'] = get_commit() # Needed for swaglog + os.environ['GIT_ORIGIN'] = build_metadata.openpilot.git_normalized_origin # Needed for swaglog + os.environ['GIT_BRANCH'] = build_metadata.channel # Needed for swaglog + os.environ['GIT_COMMIT'] = build_metadata.openpilot.git_commit # Needed for swaglog - if not is_dirty(): + if not build_metadata.openpilot.is_dirty: os.environ['CLEAN'] = '1' # init logging sentry.init(sentry.SentryProject.SELFDRIVE) cloudlog.bind_global(dongle_id=dongle_id, - version=get_version(), - origin=get_normalized_origin(), - branch=get_short_branch(), - commit=get_commit(), - dirty=is_dirty(), + version=build_metadata.openpilot.version, + origin=build_metadata.openpilot.git_normalized_origin, + branch=build_metadata.channel, + commit=build_metadata.openpilot.git_commit, + dirty=build_metadata.openpilot.is_dirty, device=HARDWARE.get_device_type()) # preimport all processes @@ -121,7 +120,7 @@ def manager_thread() -> None: params = Params() - ignore: List[str] = [] + ignore: list[str] = [] if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: @@ -146,7 +145,7 @@ def manager_thread() -> None: elif not started and started_prev: params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) - # update onroad params, which drives boardd's safety setter thread + # update onroad params, which drives pandad's safety setter thread if started != started_prev: write_onroad_params(started, params) @@ -154,7 +153,7 @@ def manager_thread() -> None: ensure_running(managed_processes.values(), started, params=params, CP=sm['carParams'], not_run=ignore) - running = ' '.join("%s%s\u001b[0m" % ("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) + running = ' '.join("{}{}\u001b[0m".format("\u001b[32m" if p.proc.is_alive() else "\u001b[31m", p.name) for p in managed_processes.values() if p.proc) print(running) cloudlog.debug(running) diff --git a/selfdrive/manager/process.py b/system/manager/process.py similarity index 94% rename from selfdrive/manager/process.py rename to system/manager/process.py index ac1b4ac68c..36f299ae62 100644 --- a/selfdrive/manager/process.py +++ b/system/manager/process.py @@ -4,7 +4,7 @@ import signal import struct import time import subprocess -from typing import Optional, Callable, List, ValuesView +from collections.abc import Callable, ValuesView from abc import ABC, abstractmethod from multiprocessing import Process @@ -12,7 +12,7 @@ from setproctitle import setproctitle from cereal import car, log import cereal.messaging as messaging -import openpilot.selfdrive.sentry as sentry +import openpilot.system.sentry as sentry from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog @@ -47,7 +47,7 @@ def launcher(proc: str, name: str) -> None: raise -def nativelauncher(pargs: List[str], cwd: str, name: str) -> None: +def nativelauncher(pargs: list[str], cwd: str, name: str) -> None: os.environ['MANAGER_DAEMON'] = name # exec the process @@ -67,12 +67,12 @@ class ManagerProcess(ABC): daemon = False sigkill = False should_run: Callable[[bool, Params, car.CarParams], bool] - proc: Optional[Process] = None + proc: Process | None = None enabled = True name = "" last_watchdog_time = 0 - watchdog_max_dt: Optional[int] = None + watchdog_max_dt: int | None = None watchdog_seen = False shutting_down = False @@ -109,7 +109,7 @@ class ManagerProcess(ABC): else: self.watchdog_seen = True - def stop(self, retry: bool = True, block: bool = True, sig: Optional[signal.Signals] = None) -> Optional[int]: + def stop(self, retry: bool = True, block: bool = True, sig: signal.Signals = None) -> int | None: if self.proc is None: return None @@ -274,7 +274,7 @@ class DaemonProcess(ManagerProcess): def ensure_running(procs: ValuesView[ManagerProcess], started: bool, params=None, CP: car.CarParams=None, - not_run: Optional[List[str]]=None) -> List[ManagerProcess]: + not_run: list[str] | None=None) -> list[ManagerProcess]: if not_run is None: not_run = [] diff --git a/selfdrive/manager/process_config.py b/system/manager/process_config.py similarity index 82% rename from selfdrive/manager/process_config.py rename to system/manager/process_config.py index cb6dd8883f..ced31077c9 100644 --- a/selfdrive/manager/process_config.py +++ b/system/manager/process_config.py @@ -3,7 +3,7 @@ import os from cereal import car from openpilot.common.params import Params from openpilot.system.hardware import PC, TICI -from openpilot.selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess +from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess WEBCAM = os.getenv("USE_WEBCAM") is not None @@ -42,7 +42,7 @@ def only_offroad(started, params, CP: car.CarParams) -> bool: return not started procs = [ - DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), + DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), NativeProcess("camerad", "system/camerad", ["./camerad"], driverview), NativeProcess("logcatd", "system/logcatd", ["./logcatd"], only_onroad), @@ -56,31 +56,31 @@ procs = [ NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar), NativeProcess("loggerd", "system/loggerd", ["./loggerd"], logging), NativeProcess("modeld", "selfdrive/modeld", ["./modeld"], only_onroad), - NativeProcess("mapsd", "selfdrive/navd", ["./mapsd"], only_onroad), - PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad), NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), - NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], always_run, enabled=False), + NativeProcess("pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", only_onroad), + PythonProcess("card", "selfdrive.car.card", only_onroad), PythonProcess("deleter", "system.loggerd.deleter", always_run), PythonProcess("dmonitoringd", "selfdrive.monitoring.dmonitoringd", driverview, enabled=(not PC or WEBCAM)), PythonProcess("qcomgpsd", "system.qcomgpsd.qcomgpsd", qcomgps, enabled=TICI), + #PythonProcess("ugpsd", "system.ugpsd", only_onroad, enabled=TICI), PythonProcess("navd", "selfdrive.navd.navd", only_onroad), - PythonProcess("pandad", "selfdrive.boardd.pandad", always_run), + PythonProcess("pandad", "selfdrive.pandad.pandad", always_run), PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad), NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI), - PythonProcess("pigeond", "system.sensord.pigeond", ublox, enabled=TICI), + PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI), PythonProcess("plannerd", "selfdrive.controls.plannerd", only_onroad), PythonProcess("radard", "selfdrive.controls.radard", only_onroad), - PythonProcess("thermald", "selfdrive.thermald.thermald", always_run), - PythonProcess("tombstoned", "selfdrive.tombstoned", always_run, enabled=not PC), - PythonProcess("updated", "selfdrive.updated", only_offroad, enabled=not PC), + PythonProcess("hardwared", "system.hardware.hardwared", always_run), + PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC), + PythonProcess("updated", "system.updated.updated", only_offroad, enabled=not PC), PythonProcess("uploader", "system.loggerd.uploader", always_run), - PythonProcess("statsd", "selfdrive.statsd", always_run), + PythonProcess("statsd", "system.statsd", always_run), # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), diff --git a/system/manager/test/__init__.py b/system/manager/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/manager/test/test_manager.py b/system/manager/test/test_manager.py old mode 100755 new mode 100644 similarity index 63% rename from selfdrive/manager/test/test_manager.py rename to system/manager/test/test_manager.py index 1ae94b26a1..e5960d1113 --- a/selfdrive/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -1,17 +1,15 @@ -#!/usr/bin/env python3 import os import pytest import signal import time -import unittest from parameterized import parameterized from cereal import car from openpilot.common.params import Params -import openpilot.selfdrive.manager.manager as manager -from openpilot.selfdrive.manager.process import ensure_running -from openpilot.selfdrive.manager.process_config import managed_processes +import openpilot.system.manager.manager as manager +from openpilot.system.manager.process import ensure_running +from openpilot.system.manager.process_config import managed_processes from openpilot.system.hardware import HARDWARE os.environ['FAKEUPLOAD'] = "1" @@ -21,15 +19,15 @@ BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] @pytest.mark.tici -class TestManager(unittest.TestCase): - def setUp(self): +class TestManager: + def setup_method(self): HARDWARE.set_power_save(False) # ensure clean CarParams params = Params() params.clear_all() - def tearDown(self): + def teardown_method(self): manager.manager_cleanup() def test_manager_prepare(self): @@ -38,7 +36,7 @@ class TestManager(unittest.TestCase): def test_blacklisted_procs(self): # TODO: ensure there are blacklisted procs until we have a dedicated test - self.assertTrue(len(BLACKLIST_PROCS), "No blacklisted procs to test not_run") + assert len(BLACKLIST_PROCS), "No blacklisted procs to test not_run" @parameterized.expand([(i,) for i in range(10)]) def test_startup_time(self, index): @@ -48,8 +46,8 @@ class TestManager(unittest.TestCase): t = time.monotonic() - start assert t < MAX_STARTUP_TIME, f"startup took {t}s, expected <{MAX_STARTUP_TIME}s" - @unittest.skip("this test is flaky the way it's currently written, should be moved to test_onroad") - def test_clean_exit(self): + @pytest.mark.skip("this test is flaky the way it's currently written, should be moved to test_onroad") + def test_clean_exit(self, subtests): """ Ensure all processes exit cleanly when stopped. """ @@ -62,21 +60,17 @@ class TestManager(unittest.TestCase): time.sleep(10) for p in procs: - with self.subTest(proc=p.name): + with subtests.test(proc=p.name): state = p.get_process_state_msg() - self.assertTrue(state.running, f"{p.name} not running") + assert state.running, f"{p.name} not running" exit_code = p.stop(retry=False) - self.assertNotIn(p.name, BLACKLIST_PROCS, f"{p.name} was started") + assert p.name not in BLACKLIST_PROCS, f"{p.name} was started" - self.assertTrue(exit_code is not None, f"{p.name} failed to exit") + assert exit_code is not None, f"{p.name} failed to exit" # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] if p.sigkill: exit_codes = [-signal.SIGKILL] - self.assertIn(exit_code, exit_codes, f"{p.name} died with {exit_code}") - - -if __name__ == "__main__": - unittest.main() + assert exit_code in exit_codes, f"{p.name} died with {exit_code}" diff --git a/system/proclogd/SConscript b/system/proclogd/SConscript index 1f4b767011..9ca8e73542 100644 --- a/system/proclogd/SConscript +++ b/system/proclogd/SConscript @@ -1,5 +1,5 @@ -Import('env', 'cereal', 'messaging', 'common') -libs = [cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj', 'common', 'zmq', 'json11'] +Import('env', 'messaging', 'common') +libs = [messaging, 'pthread', 'common', 'zmq', 'json11'] env.Program('proclogd', ['main.cc', 'proclog.cc'], LIBS=libs) if GetOption('extras'): diff --git a/system/proclogd/tests/.gitignore b/system/proclogd/tests/.gitignore new file mode 100644 index 0000000000..5230b1598d --- /dev/null +++ b/system/proclogd/tests/.gitignore @@ -0,0 +1 @@ +test_proclog diff --git a/system/proclogd/tests/test_proclog.cc b/system/proclogd/tests/test_proclog.cc new file mode 100644 index 0000000000..b86229a499 --- /dev/null +++ b/system/proclogd/tests/test_proclog.cc @@ -0,0 +1,142 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" +#include "common/util.h" +#include "system/proclogd/proclog.h" + +const std::string allowed_states = "RSDTZtWXxKWPI"; + +TEST_CASE("Parser::procStat") { + SECTION("from string") { + const std::string stat_str = + "33012 (code )) S 32978 6620 6620 0 -1 4194368 2042377 0 144 0 24510 11627 0 " + "0 20 0 39 0 53077 830029824 62214 18446744073709551615 94257242783744 94257366235808 " + "140735738643248 0 0 0 0 4098 1073808632 0 0 0 17 2 0 0 2 0 0 94257370858656 94257371248232 " + "94257404952576 140735738648768 140735738648823 140735738648823 140735738650595 0"; + auto stat = Parser::procStat(stat_str); + REQUIRE(stat); + REQUIRE(stat->pid == 33012); + REQUIRE(stat->name == "code )"); + REQUIRE(stat->state == 'S'); + REQUIRE(stat->ppid == 32978); + REQUIRE(stat->utime == 24510); + REQUIRE(stat->stime == 11627); + REQUIRE(stat->cutime == 0); + REQUIRE(stat->cstime == 0); + REQUIRE(stat->priority == 20); + REQUIRE(stat->nice == 0); + REQUIRE(stat->num_threads == 39); + REQUIRE(stat->starttime == 53077); + REQUIRE(stat->vms == 830029824); + REQUIRE(stat->rss == 62214); + REQUIRE(stat->processor == 2); + } + SECTION("all processes") { + std::vector pids = Parser::pids(); + REQUIRE(pids.size() > 1); + for (int pid : pids) { + std::string stat_path = "/proc/" + std::to_string(pid) + "/stat"; + INFO(stat_path); + if (auto stat = Parser::procStat(util::read_file(stat_path))) { + REQUIRE(stat->pid == pid); + REQUIRE(allowed_states.find(stat->state) != std::string::npos); + } else { + REQUIRE(util::file_exists(stat_path) == false); + } + } + } +} + +TEST_CASE("Parser::cpuTimes") { + SECTION("from string") { + std::string stat = + "cpu 0 0 0 0 0 0 0 0 0 0\n" + "cpu0 1 2 3 4 5 6 7 8 9 10\n" + "cpu1 1 2 3 4 5 6 7 8 9 10\n"; + std::istringstream stream(stat); + auto stats = Parser::cpuTimes(stream); + REQUIRE(stats.size() == 2); + for (int i = 0; i < stats.size(); ++i) { + REQUIRE(stats[i].id == i); + REQUIRE(stats[i].utime == 1); + REQUIRE(stats[i].ntime ==2); + REQUIRE(stats[i].stime == 3); + REQUIRE(stats[i].itime == 4); + REQUIRE(stats[i].iowtime == 5); + REQUIRE(stats[i].irqtime == 6); + REQUIRE(stats[i].sirqtime == 7); + } + } + SECTION("all cpus") { + std::istringstream stream(util::read_file("/proc/stat")); + auto stats = Parser::cpuTimes(stream); + REQUIRE(stats.size() == sysconf(_SC_NPROCESSORS_ONLN)); + for (int i = 0; i < stats.size(); ++i) { + REQUIRE(stats[i].id == i); + } + } +} + +TEST_CASE("Parser::memInfo") { + SECTION("from string") { + std::istringstream stream("MemTotal: 1024 kb\nMemFree: 2048 kb\n"); + auto meminfo = Parser::memInfo(stream); + REQUIRE(meminfo["MemTotal:"] == 1024 * 1024); + REQUIRE(meminfo["MemFree:"] == 2048 * 1024); + } + SECTION("from /proc/meminfo") { + std::string require_keys[] = {"MemTotal:", "MemFree:", "MemAvailable:", "Buffers:", "Cached:", "Active:", "Inactive:", "Shmem:"}; + std::istringstream stream(util::read_file("/proc/meminfo")); + auto meminfo = Parser::memInfo(stream); + for (auto &key : require_keys) { + REQUIRE(meminfo.find(key) != meminfo.end()); + REQUIRE(meminfo[key] > 0); + } + } +} + +void test_cmdline(std::string cmdline, const std::vector requires) { + std::stringstream ss; + ss.write(&cmdline[0], cmdline.size()); + auto cmds = Parser::cmdline(ss); + REQUIRE(cmds.size() == requires.size()); + for (int i = 0; i < requires.size(); ++i) { + REQUIRE(cmds[i] == requires[i]); + } +} +TEST_CASE("Parser::cmdline") { + test_cmdline(std::string("a\0b\0c\0", 7), {"a", "b", "c"}); + test_cmdline(std::string("a\0\0c\0", 6), {"a", "c"}); + test_cmdline(std::string("a\0b\0c\0\0\0", 9), {"a", "b", "c"}); +} + +TEST_CASE("buildProcLoggerMessage") { + MessageBuilder msg; + buildProcLogMessage(msg); + + kj::Array buf = capnp::messageToFlatArray(msg); + capnp::FlatArrayMessageReader reader(buf); + auto log = reader.getRoot().getProcLog(); + REQUIRE(log.totalSize().wordCount > 0); + + // test cereal::ProcLog::CPUTimes + auto cpu_times = log.getCpuTimes(); + REQUIRE(cpu_times.size() == sysconf(_SC_NPROCESSORS_ONLN)); + REQUIRE(cpu_times[cpu_times.size() - 1].getCpuNum() == cpu_times.size() - 1); + + // test cereal::ProcLog::Mem + auto mem = log.getMem(); + REQUIRE(mem.getTotal() > 0); + REQUIRE(mem.getShared() > 0); + + // test cereal::ProcLog::Process + auto procs = log.getProcs(); + for (auto p : procs) { + REQUIRE(allowed_states.find(p.getState()) != std::string::npos); + if (p.getPid() == ::getpid()) { + REQUIRE(p.getName() == "test_proclog"); + REQUIRE(p.getState() == 'R'); + REQUIRE_THAT(p.getExe().cStr(), Catch::Matchers::Contains("test_proclog")); + REQUIRE_THAT(p.getCmdline()[0], Catch::Matchers::Contains("test_proclog")); + } + } +} diff --git a/system/qcomgpsd/nmeaport.py b/system/qcomgpsd/nmeaport.py index 231096fc5d..8b9ab51086 100644 --- a/system/qcomgpsd/nmeaport.py +++ b/system/qcomgpsd/nmeaport.py @@ -93,7 +93,7 @@ def nmea_checksum_ok(s): def process_nmea_port_messages(device:str="/dev/ttyUSB1") -> NoReturn: while True: try: - with open(device, "r") as nmeaport: + with open(device) as nmeaport: for line in nmeaport: line = line.strip() if DEBUG: @@ -127,7 +127,7 @@ def main() -> NoReturn: print("qcomgpsd is running, please kill openpilot before running this script! (aborted)") sys.exit(1) except CalledProcessError as e: - if e.returncode != 1: # 1 == no process found (boardd not running) + if e.returncode != 1: # 1 == no process found (pandad not running) raise e print("power up antenna ...") diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index e2a180df86..21c7995a77 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -10,13 +10,14 @@ import shutil import subprocess import datetime from multiprocessing import Process, Event -from typing import NoReturn, Optional +from typing import NoReturn from struct import unpack_from, calcsize, pack from cereal import log import cereal.messaging as messaging from openpilot.common.gpio import gpio_init, gpio_set from openpilot.common.retry import retry +from openpilot.common.time import system_time_valid from openpilot.system.hardware.tici.pins import GPIO from openpilot.common.swaglog import cloudlog from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv @@ -90,7 +91,7 @@ def try_setup_logs(diag, logs): return setup_logs(diag, logs) @retry(attempts=3, delay=1.0) -def at_cmd(cmd: str) -> Optional[str]: +def at_cmd(cmd: str) -> str | None: return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8') def gps_enabled() -> bool: @@ -171,8 +172,9 @@ def setup_quectel(diag: ModemDiag) -> bool: inject_assistance() os.remove(ASSIST_DATA_FILE) #at_cmd("AT+QGPSXTRADATA?") - time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S") - at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000") + if system_time_valid(): + time_str = datetime.datetime.utcnow().strftime("%Y/%m/%d,%H:%M:%S") + at_cmd(f"AT+QGPSXTRATIME=0,\"{time_str}\",1,1,1000") at_cmd("AT+QGPSCFG=\"outport\",\"usbnmea\"") at_cmd("AT+QGPS=1") @@ -205,10 +207,10 @@ def teardown_quectel(diag): try_setup_logs(diag, []) -def wait_for_modem(): +def wait_for_modem(cmd="AT+QGPS?"): cloudlog.warning("waiting for modem to come up") while True: - ret = subprocess.call("mmcli -m any --timeout 10 --command=\"AT+QGPS?\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) + ret = subprocess.call(f"mmcli -m any --timeout 10 --command=\"{cmd}\"", stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, shell=True) if ret == 0: return time.sleep(0.1) @@ -342,7 +344,7 @@ def main() -> NoReturn: gps.bearingDeg = report["q_FltHeadingRad"] * 180/math.pi # TODO needs update if there is another leap second, after june 2024? - dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.timezone.utc) + + dt_timestamp = (datetime.datetime(1980, 1, 6, 0, 0, 0, 0, datetime.UTC) + datetime.timedelta(weeks=report['w_GpsWeekNumber']) + datetime.timedelta(seconds=(1e-3*report['q_GpsFixTimeMs'] - 18))) gps.unixTimestampMillis = dt_timestamp.timestamp()*1e3 @@ -352,8 +354,8 @@ def main() -> NoReturn: gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180 gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma])) # quectel gps verticalAccuracy is clipped to 500, set invalid if so - gps.flags = 1 if gps.verticalAccuracy != 500 else 0 - if gps.flags: + gps.hasFix = gps.verticalAccuracy != 500 + if gps.hasFix: want_assistance = False stop_download_event.set() pm.send('gpsLocation', msg) diff --git a/system/qcomgpsd/tests/test_qcomgpsd.py b/system/qcomgpsd/tests/test_qcomgpsd.py old mode 100755 new mode 100644 index 6c93f7dd93..ef23737dd7 --- a/system/qcomgpsd/tests/test_qcomgpsd.py +++ b/system/qcomgpsd/tests/test_qcomgpsd.py @@ -1,38 +1,36 @@ -#!/usr/bin/env python3 import os import pytest import json import time import datetime -import unittest import subprocess import cereal.messaging as messaging from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem -from openpilot.selfdrive.manager.process_config import managed_processes +from openpilot.system.manager.process_config import managed_processes GOOD_SIGNAL = bool(int(os.getenv("GOOD_SIGNAL", '0'))) @pytest.mark.tici -class TestRawgpsd(unittest.TestCase): +class TestRawgpsd: @classmethod - def setUpClass(cls): + def setup_class(cls): os.system("sudo systemctl start systemd-resolved") os.system("sudo systemctl restart ModemManager lte") wait_for_modem() @classmethod - def tearDownClass(cls): + def teardown_class(cls): managed_processes['qcomgpsd'].stop() os.system("sudo systemctl restart systemd-resolved") os.system("sudo systemctl restart ModemManager lte") - def setUp(self): + def setup_method(self): at_cmd("AT+QGPSDEL=0") self.sm = messaging.SubMaster(['qcomGnss', 'gpsLocation', 'gnssMeasurements']) - def tearDown(self): + def teardown_method(self): managed_processes['qcomgpsd'].stop() os.system("sudo systemctl restart systemd-resolved") @@ -57,18 +55,18 @@ class TestRawgpsd(unittest.TestCase): os.system("sudo systemctl restart ModemManager") assert self._wait_for_output(30) - def test_startup_time(self): + def test_startup_time(self, subtests): for internet in (True, False): if not internet: os.system("sudo systemctl stop systemd-resolved") - with self.subTest(internet=internet): + with subtests.test(internet=internet): managed_processes['qcomgpsd'].start() assert self._wait_for_output(7) managed_processes['qcomgpsd'].stop() - def test_turns_off_gnss(self): + def test_turns_off_gnss(self, subtests): for s in (0.1, 1, 5): - with self.subTest(runtime=s): + with subtests.test(runtime=s): managed_processes['qcomgpsd'].start() time.sleep(s) managed_processes['qcomgpsd'].stop() @@ -87,7 +85,7 @@ class TestRawgpsd(unittest.TestCase): if should_be_loaded: assert valid_duration == "10080" # should be max time injected_time = datetime.datetime.strptime(injected_time_str.replace("\"", ""), "%Y/%m/%d,%H:%M:%S") - self.assertLess(abs((datetime.datetime.utcnow() - injected_time).total_seconds()), 60*60*12) + assert abs((datetime.datetime.utcnow() - injected_time).total_seconds()) < 60*60*12 else: valid_duration, injected_time_str = out.split(",", 1) injected_time_str = injected_time_str.replace('\"', '').replace('\'', '') @@ -119,6 +117,3 @@ class TestRawgpsd(unittest.TestCase): time.sleep(15) managed_processes['qcomgpsd'].stop() self.check_assistance(True) - -if __name__ == "__main__": - unittest.main(failfast=True) diff --git a/system/sensord/SConscript b/system/sensord/SConscript index 7974eb07ae..e2dfb522c6 100644 --- a/system/sensord/SConscript +++ b/system/sensord/SConscript @@ -1,4 +1,4 @@ -Import('env', 'arch', 'common', 'cereal', 'messaging') +Import('env', 'arch', 'common', 'messaging') sensors = [ 'sensors/i2c_sensor.cc', @@ -11,7 +11,7 @@ sensors = [ 'sensors/lsm6ds3_temp.cc', 'sensors/mmc5603nj_magn.cc', ] -libs = [common, cereal, messaging, 'capnp', 'zmq', 'kj', 'pthread'] +libs = [common, messaging, 'pthread'] if arch == "larch64": libs.append('i2c') env.Program('sensord', ['sensors_qcom2.cc'] + sensors, LIBS=libs) diff --git a/system/sensord/__init__.py b/system/sensord/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/sensord/tests/__init__.py b/system/sensord/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/sensord/tests/test_sensord.py b/system/sensord/tests/test_sensord.py new file mode 100644 index 0000000000..1871012dd6 --- /dev/null +++ b/system/sensord/tests/test_sensord.py @@ -0,0 +1,246 @@ +import os +import pytest +import time +import numpy as np +from collections import namedtuple, defaultdict + +import cereal.messaging as messaging +from cereal import log +from cereal.services import SERVICE_LIST +from openpilot.common.gpio import get_irqs_for_action +from openpilot.common.timeout import Timeout +from openpilot.system.manager.process_config import managed_processes + +BMX = { + ('bmx055', 'acceleration'), + ('bmx055', 'gyroUncalibrated'), + ('bmx055', 'magneticUncalibrated'), + ('bmx055', 'temperature'), +} + +LSM = { + ('lsm6ds3', 'acceleration'), + ('lsm6ds3', 'gyroUncalibrated'), + ('lsm6ds3', 'temperature'), +} +LSM_C = {(x[0]+'trc', x[1]) for x in LSM} + +MMC = { + ('mmc5603nj', 'magneticUncalibrated'), +} + +SENSOR_CONFIGURATIONS = ( + (BMX | LSM), + (MMC | LSM), + (BMX | LSM_C), + (MMC| LSM_C), +) + +Sensor = log.SensorEventData.SensorSource +SensorConfig = namedtuple('SensorConfig', ['type', 'sanity_min', 'sanity_max']) +ALL_SENSORS = { + Sensor.lsm6ds3: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.lsm6ds3trc: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("temperature", 0, 60), + }, + + Sensor.bmx055: { + SensorConfig("acceleration", 5, 15), + SensorConfig("gyroUncalibrated", 0, .2), + SensorConfig("magneticUncalibrated", 0, 300), + SensorConfig("temperature", 0, 60), + }, + + Sensor.mmc5603nj: { + SensorConfig("magneticUncalibrated", 0, 300), + } +} + + +def get_irq_count(irq: int): + with open(f"/sys/kernel/irq/{irq}/per_cpu_count") as f: + per_cpu = map(int, f.read().split(",")) + return sum(per_cpu) + +def read_sensor_events(duration_sec): + sensor_types = ['accelerometer', 'gyroscope', 'magnetometer', 'accelerometer2', + 'gyroscope2', 'temperatureSensor', 'temperatureSensor2'] + socks = {} + poller = messaging.Poller() + events = defaultdict(list) + for stype in sensor_types: + socks[stype] = messaging.sub_sock(stype, poller=poller, timeout=100) + + # wait for sensors to come up + with Timeout(int(os.environ.get("SENSOR_WAIT", "5")), "sensors didn't come up"): + while len(poller.poll(250)) == 0: + pass + time.sleep(1) + for s in socks.values(): + messaging.drain_sock_raw(s) + + st = time.monotonic() + while time.monotonic() - st < duration_sec: + for s in socks: + events[s] += messaging.drain_sock(socks[s]) + time.sleep(0.1) + + assert sum(map(len, events.values())) != 0, "No sensor events collected!" + + return {k: v for k, v in events.items() if len(v) > 0} + +@pytest.mark.tici +class TestSensord: + @classmethod + def setup_class(cls): + # enable LSM self test + os.environ["LSM_SELF_TEST"] = "1" + + # read initial sensor values every test case can use + os.system("pkill -f \\\\./sensord") + try: + managed_processes["sensord"].start() + cls.sample_secs = int(os.getenv("SAMPLE_SECS", "10")) + cls.events = read_sensor_events(cls.sample_secs) + + # determine sensord's irq + cls.sensord_irq = get_irqs_for_action("sensord")[0] + finally: + # teardown won't run if this doesn't succeed + managed_processes["sensord"].stop() + + @classmethod + def teardown_class(cls): + managed_processes["sensord"].stop() + + def teardown_method(self): + managed_processes["sensord"].stop() + + def test_sensors_present(self): + # verify correct sensors configuration + seen = set() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + seen.add((str(m.source), m.which())) + + assert seen in SENSOR_CONFIGURATIONS + + def test_lsm6ds3_timing(self, subtests): + # verify measurements are sampled and published at 104Hz + + sensor_t = { + 1: [], # accel + 5: [], # gyro + } + + for measurement in self.events['accelerometer']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for measurement in self.events['gyroscope']: + m = getattr(measurement, measurement.which()) + sensor_t[m.sensor].append(m.timestamp) + + for s, vals in sensor_t.items(): + with subtests.test(sensor=s): + assert len(vals) > 0 + tdiffs = np.diff(vals) / 1e6 # millis + + high_delay_diffs = list(filter(lambda d: d >= 20., tdiffs)) + assert len(high_delay_diffs) < 15, f"Too many large diffs: {high_delay_diffs}" + + avg_diff = sum(tdiffs)/len(tdiffs) + avg_freq = 1. / (avg_diff * 1e-3) + assert 92. < avg_freq < 114., f"avg freq {avg_freq}Hz wrong, expected 104Hz" + + stddev = np.std(tdiffs) + assert stddev < 2.0, f"Standard-dev to big {stddev}" + + def test_sensor_frequency(self, subtests): + for s, msgs in self.events.items(): + with subtests.test(sensor=s): + freq = len(msgs) / self.sample_secs + ef = SERVICE_LIST[s].frequency + assert ef*0.85 <= freq <= ef*1.15 + + def test_logmonottime_timestamp_diff(self): + # ensure diff between the message logMonotime and sample timestamp is small + + tdiffs = list() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + + # check if gyro and accel timestamps are before logMonoTime + if str(m.source).startswith("lsm6ds3") and m.which() != 'temperature': + err_msg = f"Timestamp after logMonoTime: {m.timestamp} > {measurement.logMonoTime}" + assert m.timestamp < measurement.logMonoTime, err_msg + + # negative values might occur, as non interrupt packages created + # before the sensor is read + tdiffs.append(abs(measurement.logMonoTime - m.timestamp) / 1e6) + + # some sensors have a read procedure that will introduce an expected diff on the order of 20ms + high_delay_diffs = set(filter(lambda d: d >= 25., tdiffs)) + assert len(high_delay_diffs) < 20, f"Too many measurements published: {high_delay_diffs}" + + avg_diff = round(sum(tdiffs)/len(tdiffs), 4) + assert avg_diff < 4, f"Avg packet diff: {avg_diff:.1f}ms" + + def test_sensor_values(self): + sensor_values = dict() + for etype in self.events: + for measurement in self.events[etype]: + m = getattr(measurement, measurement.which()) + key = (m.source.raw, m.which()) + values = getattr(m, m.which()) + + if hasattr(values, 'v'): + values = values.v + values = np.atleast_1d(values) + + if key in sensor_values: + sensor_values[key].append(values) + else: + sensor_values[key] = [values] + + # Sanity check sensor values + for sensor, stype in sensor_values: + for s in ALL_SENSORS[sensor]: + if s.type != stype: + continue + + key = (sensor, s.type) + mean_norm = np.mean(np.linalg.norm(sensor_values[key], axis=1)) + err_msg = f"Sensor '{sensor} {s.type}' failed sanity checks {mean_norm} is not between {s.sanity_min} and {s.sanity_max}" + assert s.sanity_min <= mean_norm <= s.sanity_max, err_msg + + def test_sensor_verify_no_interrupts_after_stop(self): + managed_processes["sensord"].start() + time.sleep(3) + + # read /proc/interrupts to verify interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + + error_msg = f"no interrupts received after sensord start!\n{state_one} {state_two}" + assert state_one != state_two, error_msg + + managed_processes["sensord"].stop() + time.sleep(1) + + # read /proc/interrupts to verify no more interrupts are received + state_one = get_irq_count(self.sensord_irq) + time.sleep(1) + state_two = get_irq_count(self.sensord_irq) + assert state_one == state_two, "Interrupts received after sensord stop!" + diff --git a/system/sensord/tests/ttff_test.py b/system/sensord/tests/ttff_test.py new file mode 100755 index 0000000000..a9cc16d707 --- /dev/null +++ b/system/sensord/tests/ttff_test.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 + +import time +import atexit + +from cereal import messaging +from openpilot.system.manager.process_config import managed_processes + +TIMEOUT = 10*60 + +def kill(): + for proc in ['ubloxd', 'pigeond']: + managed_processes[proc].stop(retry=True, block=True) + +if __name__ == "__main__": + # start ubloxd + managed_processes['ubloxd'].start() + atexit.register(kill) + + sm = messaging.SubMaster(['ubloxGnss']) + + times = [] + for i in range(20): + # start pigeond + st = time.monotonic() + managed_processes['pigeond'].start() + + # wait for a >4 satellite fix + while True: + sm.update(0) + msg = sm['ubloxGnss'] + if msg.which() == 'measurementReport' and sm.updated["ubloxGnss"]: + report = msg.measurementReport + if report.numMeas > 4: + times.append(time.monotonic() - st) + print(f"\033[94m{i}: Got a fix in {round(times[-1], 2)} seconds\033[0m") + break + + if time.monotonic() - st > TIMEOUT: + raise TimeoutError("\033[91mFailed to get a fix in {TIMEOUT} seconds!\033[0m") + + time.sleep(0.1) + + # stop pigeond + managed_processes['pigeond'].stop(retry=True, block=True) + time.sleep(20) + + print(f"\033[92mAverage TTFF: {round(sum(times) / len(times), 2)}s\033[0m") diff --git a/selfdrive/sentry.py b/system/sentry.py similarity index 76% rename from selfdrive/sentry.py rename to system/sentry.py index 5b63a9fe2d..5f4772fa71 100644 --- a/selfdrive/sentry.py +++ b/system/sentry.py @@ -4,11 +4,10 @@ from enum import Enum from sentry_sdk.integrations.threading import ThreadingIntegration from openpilot.common.params import Params -from openpilot.selfdrive.athena.registration import is_registered_device +from openpilot.system.athena.registration import is_registered_device from openpilot.system.hardware import HARDWARE, PC from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_branch, get_commit, get_origin, get_version, \ - is_comma_remote, is_dirty, is_tested_branch +from openpilot.system.version import get_build_metadata, get_version class SentryProject(Enum): @@ -43,12 +42,13 @@ def set_tag(key: str, value: str) -> None: def init(project: SentryProject) -> bool: + build_metadata = get_build_metadata() # forks like to mess with this, so double check - comma_remote = is_comma_remote() and "commaai" in get_origin() + comma_remote = build_metadata.openpilot.comma_remote and "commaai" in build_metadata.openpilot.git_origin if not comma_remote or not is_registered_device() or PC: return False - env = "release" if is_tested_branch() else "master" + env = "release" if build_metadata.tested_channel else "master" dongle_id = Params().get("DongleId", encoding='utf-8') integrations = [] @@ -63,11 +63,13 @@ def init(project: SentryProject) -> bool: max_value_length=8192, environment=env) + build_metadata = get_build_metadata() + sentry_sdk.set_user({"id": dongle_id}) - sentry_sdk.set_tag("dirty", is_dirty()) - sentry_sdk.set_tag("origin", get_origin()) - sentry_sdk.set_tag("branch", get_branch()) - sentry_sdk.set_tag("commit", get_commit()) + sentry_sdk.set_tag("dirty", build_metadata.openpilot.is_dirty) + sentry_sdk.set_tag("origin", build_metadata.openpilot.git_origin) + sentry_sdk.set_tag("branch", build_metadata.channel) + sentry_sdk.set_tag("commit", build_metadata.openpilot.git_commit) sentry_sdk.set_tag("device", HARDWARE.get_device_type()) if project == SentryProject.SELFDRIVE: diff --git a/selfdrive/statsd.py b/system/statsd.py similarity index 89% rename from selfdrive/statsd.py rename to system/statsd.py index 94572b82c7..2b5a7bb3a7 100755 --- a/selfdrive/statsd.py +++ b/system/statsd.py @@ -4,8 +4,8 @@ import zmq import time from pathlib import Path from collections import defaultdict -from datetime import datetime, timezone -from typing import NoReturn, Union, List, Dict +from datetime import datetime, UTC +from typing import NoReturn from openpilot.common.params import Params from cereal.messaging import SubMaster @@ -13,7 +13,7 @@ from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import HARDWARE from openpilot.common.file_helpers import atomic_write_in_dir -from openpilot.system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty +from openpilot.system.version import get_build_metadata from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S @@ -61,7 +61,7 @@ class StatLog: def main() -> NoReturn: dongle_id = Params().get("DongleId", encoding='utf-8') - def get_influxdb_line(measurement: str, value: Union[float, Dict[str, float]], timestamp: datetime, tags: dict) -> str: + def get_influxdb_line(measurement: str, value: float | dict[str, float], timestamp: datetime, tags: dict) -> str: res = f"{measurement}" for k, v in tags.items(): res += f",{k}={str(v)}" @@ -86,13 +86,15 @@ def main() -> NoReturn: # initialize stats directory Path(STATS_DIR).mkdir(parents=True, exist_ok=True) + build_metadata = get_build_metadata() + # initialize tags tags = { 'started': False, - 'version': get_short_version(), - 'branch': get_short_branch(), - 'dirty': is_dirty(), - 'origin': get_normalized_origin(), + 'version': build_metadata.openpilot.version, + 'branch': build_metadata.channel, + 'dirty': build_metadata.openpilot.is_dirty, + 'origin': build_metadata.openpilot.git_normalized_origin, 'deviceType': HARDWARE.get_device_type(), } @@ -102,7 +104,7 @@ def main() -> NoReturn: idx = 0 last_flush_time = time.monotonic() gauges = {} - samples: Dict[str, List[float]] = defaultdict(list) + samples: dict[str, list[float]] = defaultdict(list) try: while True: started_prev = sm['deviceState'].started @@ -131,7 +133,7 @@ def main() -> NoReturn: # 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) + current_time = datetime.utcnow().replace(tzinfo=UTC) tags['started'] = sm['deviceState'].started for key, value in gauges.items(): diff --git a/system/tests/__init__.py b/system/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/tests/test_logmessaged.py b/system/tests/test_logmessaged.py new file mode 100644 index 0000000000..3baf5300c0 --- /dev/null +++ b/system/tests/test_logmessaged.py @@ -0,0 +1,55 @@ +import glob +import os +import time + +import cereal.messaging as messaging +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.hardware.hw import Paths +from openpilot.common.swaglog import cloudlog, ipchandler + + +class TestLogmessaged: + def setup_method(self): + # clear the IPC buffer in case some other tests used cloudlog and filled it + ipchandler.close() + ipchandler.connect() + + managed_processes['logmessaged'].start() + self.sock = messaging.sub_sock("logMessage", timeout=1000, conflate=False) + self.error_sock = messaging.sub_sock("logMessage", timeout=1000, conflate=False) + + # ensure sockets are connected + time.sleep(1) + messaging.drain_sock(self.sock) + messaging.drain_sock(self.error_sock) + + def teardown_method(self): + del self.sock + del self.error_sock + managed_processes['logmessaged'].stop(block=True) + + def _get_log_files(self): + return list(glob.glob(os.path.join(Paths.swaglog_root(), "swaglog.*"))) + + def test_simple_log(self): + msgs = [f"abc {i}" for i in range(10)] + for m in msgs: + cloudlog.error(m) + time.sleep(3) + m = messaging.drain_sock(self.sock) + assert len(m) == len(msgs) + assert len(self._get_log_files()) >= 1 + + def test_big_log(self): + n = 10 + msg = "a"*3*1024*1024 + for _ in range(n): + cloudlog.info(msg) + time.sleep(3) + + msgs = messaging.drain_sock(self.sock) + assert len(msgs) == 0 + + logsize = sum([os.path.getsize(f) for f in self._get_log_files()]) + assert (n*len(msg)) < logsize < (n*(len(msg)+1024)) + diff --git a/system/timed.py b/system/timed.py index 39acb2ba12..2b9a42c455 100755 --- a/system/timed.py +++ b/system/timed.py @@ -8,6 +8,7 @@ from typing import NoReturn from timezonefinder import TimezoneFinder import cereal.messaging as messaging +from openpilot.common.time import system_time_valid from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import AGNOS @@ -56,7 +57,6 @@ def main() -> NoReturn: """ params = Params() - tf = TimezoneFinder() # Restore timezone from param tz = params.get("Timezone", encoding='utf8') @@ -70,7 +70,8 @@ def main() -> NoReturn: while True: sm.update(1000) - msg = messaging.new_message('clocks', valid=True) + msg = messaging.new_message('clocks') + msg.valid = system_time_valid() msg.clocks.wallTimeNanos = time.time_ns() pm.send('clocks', msg) diff --git a/selfdrive/tombstoned.py b/system/tombstoned.py similarity index 94% rename from selfdrive/tombstoned.py rename to system/tombstoned.py index ba3582d130..5bcced2666 100755 --- a/selfdrive/tombstoned.py +++ b/system/tombstoned.py @@ -9,10 +9,10 @@ import time import glob from typing import NoReturn -import openpilot.selfdrive.sentry as sentry +import openpilot.system.sentry as sentry from openpilot.system.hardware.hw import Paths from openpilot.common.swaglog import cloudlog -from openpilot.system.version import get_commit +from openpilot.system.version import get_build_metadata MAX_SIZE = 1_000_000 * 100 # allow up to 100M MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("/crash/") @@ -124,7 +124,9 @@ def report_tombstone_apport(fn): clean_path = path.replace('/', '_') date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S") - new_fn = f"{date}_{(get_commit() or 'nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] + build_metadata = get_build_metadata() + + new_fn = f"{date}_{(build_metadata.openpilot.git_commit or 'nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] crashlog_dir = os.path.join(Paths.log_root(), "crash") os.makedirs(crashlog_dir, exist_ok=True) diff --git a/system/ubloxd/SConscript b/system/ubloxd/SConscript index 67d9856dad..ce09e235e6 100644 --- a/system/ubloxd/SConscript +++ b/system/ubloxd/SConscript @@ -1,6 +1,6 @@ -Import('env', 'common', 'cereal', 'messaging') +Import('env', 'common', 'messaging') -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread'] +loc_libs = [messaging, common, 'kaitai', 'pthread'] if GetOption('kaitai'): generated = Dir('generated').srcnode().abspath diff --git a/system/ubloxd/glonass.ksy b/system/ubloxd/glonass.ksy new file mode 100644 index 0000000000..be99f6e497 --- /dev/null +++ b/system/ubloxd/glonass.ksy @@ -0,0 +1,176 @@ +# http://gauss.gge.unb.ca/GLONASS.ICD.pdf +# some variables are misprinted but good in the old doc +# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf +meta: + id: glonass + endian: be + bit-endian: be +seq: + - id: idle_chip + type: b1 + - id: string_number + type: b4 + - id: data + type: + switch-on: string_number + cases: + 1: string_1 + 2: string_2 + 3: string_3 + 4: string_4 + 5: string_5 + _: string_non_immediate + - id: hamming_code + type: b8 + - id: pad_1 + type: b11 + - id: superframe_number + type: b16 + - id: pad_2 + type: b8 + - id: frame_number + type: b8 + +types: + string_1: + seq: + - id: not_used + type: b2 + - id: p1 + type: b2 + - id: t_k + type: b12 + - id: x_vel_sign + type: b1 + - id: x_vel_value + type: b23 + - id: x_accel_sign + type: b1 + - id: x_accel_value + type: b4 + - id: x_sign + type: b1 + - id: x_value + type: b26 + instances: + x_vel: + value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value' + x_accel: + value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value' + x: + value: 'x_sign ? (x_value * (-1)) : x_value' + string_2: + seq: + - id: b_n + type: b3 + - id: p2 + type: b1 + - id: t_b + type: b7 + - id: not_used + type: b5 + - id: y_vel_sign + type: b1 + - id: y_vel_value + type: b23 + - id: y_accel_sign + type: b1 + - id: y_accel_value + type: b4 + - id: y_sign + type: b1 + - id: y_value + type: b26 + instances: + y_vel: + value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value' + y_accel: + value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value' + y: + value: 'y_sign ? (y_value * (-1)) : y_value' + string_3: + seq: + - id: p3 + type: b1 + - id: gamma_n_sign + type: b1 + - id: gamma_n_value + type: b10 + - id: not_used + type: b1 + - id: p + type: b2 + - id: l_n + type: b1 + - id: z_vel_sign + type: b1 + - id: z_vel_value + type: b23 + - id: z_accel_sign + type: b1 + - id: z_accel_value + type: b4 + - id: z_sign + type: b1 + - id: z_value + type: b26 + instances: + gamma_n: + value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value' + z_vel: + value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value' + z_accel: + value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value' + z: + value: 'z_sign ? (z_value * (-1)) : z_value' + string_4: + seq: + - id: tau_n_sign + type: b1 + - id: tau_n_value + type: b21 + - id: delta_tau_n_sign + type: b1 + - id: delta_tau_n_value + type: b4 + - id: e_n + type: b5 + - id: not_used_1 + type: b14 + - id: p4 + type: b1 + - id: f_t + type: b4 + - id: not_used_2 + type: b3 + - id: n_t + type: b11 + - id: n + type: b5 + - id: m + type: b2 + instances: + tau_n: + value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value' + delta_tau_n: + value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value' + string_5: + seq: + - id: n_a + type: b11 + - id: tau_c + type: b32 + - id: not_used + type: b1 + - id: n_4 + type: b5 + - id: tau_gps + type: b22 + - id: l_n + type: b1 + string_non_immediate: + seq: + - id: data_1 + type: b64 + - id: data_2 + type: b8 diff --git a/system/ubloxd/glonass_fix.patch b/system/ubloxd/glonass_fix.patch new file mode 100644 index 0000000000..7eb973a348 --- /dev/null +++ b/system/ubloxd/glonass_fix.patch @@ -0,0 +1,13 @@ +diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp +index 5b17bc327..b5c6aa610 100644 +--- a/system/ubloxd/generated/glonass.cpp ++++ b/system/ubloxd/generated/glonass.cpp +@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass + void glonass_t::_read() { + m_idle_chip = m__io->read_bits_int_be(1); + m_string_number = m__io->read_bits_int_be(4); +- m__io->align_to_byte(); ++ //m__io->align_to_byte(); + switch (string_number()) { + case 4: { + m_data = new string_4_t(m__io, this, m__root); diff --git a/system/ubloxd/gps.ksy b/system/ubloxd/gps.ksy new file mode 100644 index 0000000000..893ad1b25b --- /dev/null +++ b/system/ubloxd/gps.ksy @@ -0,0 +1,189 @@ +# https://www.gps.gov/technical/icwg/IS-GPS-200E.pdf +meta: + id: gps + endian: be + bit-endian: be +seq: + - id: tlm + type: tlm + - id: how + type: how + - id: body + type: + switch-on: how.subframe_id + cases: + 1: subframe_1 + 2: subframe_2 + 3: subframe_3 + 4: subframe_4 +types: + tlm: + seq: + - id: preamble + contents: [0x8b] + - id: tlm + type: b14 + - id: integrity_status + type: b1 + - id: reserved + type: b1 + how: + seq: + - id: tow_count + type: b17 + - id: alert + type: b1 + - id: anti_spoof + type: b1 + - id: subframe_id + type: b3 + - id: reserved + type: b2 + subframe_1: + seq: + # Word 3 + - id: week_no + type: b10 + - id: code + type: b2 + - id: sv_accuracy + type: b4 + - id: sv_health + type: b6 + - id: iodc_msb + type: b2 + # Word 4 + - id: l2_p_data_flag + type: b1 + - id: reserved1 + type: b23 + # Word 5 + - id: reserved2 + type: b24 + # Word 6 + - id: reserved3 + type: b24 + # Word 7 + - id: reserved4 + type: b16 + - id: t_gd + type: s1 + # Word 8 + - id: iodc_lsb + type: u1 + - id: t_oc + type: u2 + # Word 9 + - id: af_2 + type: s1 + - id: af_1 + type: s2 + # Word 10 + - id: af_0_sign + type: b1 + - id: af_0_value + type: b21 + - id: reserved5 + type: b2 + instances: + af_0: + value: 'af_0_sign ? (af_0_value - (1 << 21)) : af_0_value' + subframe_2: + seq: + # Word 3 + - id: iode + type: u1 + - id: c_rs + type: s2 + # Word 4 & 5 + - id: delta_n + type: s2 + - id: m_0 + type: s4 + # Word 6 & 7 + - id: c_uc + type: s2 + - id: e + type: s4 + # Word 8 & 9 + - id: c_us + type: s2 + - id: sqrt_a + type: u4 + # Word 10 + - id: t_oe + type: u2 + - id: fit_interval_flag + type: b1 + - id: aoda + type: b5 + - id: reserved + type: b2 + subframe_3: + seq: + # Word 3 & 4 + - id: c_ic + type: s2 + - id: omega_0 + type: s4 + # Word 5 & 6 + - id: c_is + type: s2 + - id: i_0 + type: s4 + # Word 7 & 8 + - id: c_rc + type: s2 + - id: omega + type: s4 + # Word 9 + - id: omega_dot_sign + type: b1 + - id: omega_dot_value + type: b23 + # Word 10 + - id: iode + type: u1 + - id: idot_sign + type: b1 + - id: idot_value + type: b13 + - id: reserved + type: b2 + instances: + omega_dot: + value: 'omega_dot_sign ? (omega_dot_value - (1 << 23)) : omega_dot_value' + idot: + value: 'idot_sign ? (idot_value - (1 << 13)) : idot_value' + subframe_4: + seq: + # Word 3 + - id: data_id + type: b2 + - id: page_id + type: b6 + - id: body + type: + switch-on: page_id + cases: + 56: ionosphere_data + types: + ionosphere_data: + seq: + - id: a0 + type: s1 + - id: a1 + type: s1 + - id: a2 + type: s1 + - id: a3 + type: s1 + - id: b0 + type: s1 + - id: b1 + type: s1 + - id: b2 + type: s1 + - id: b3 + type: s1 + diff --git a/system/sensord/pigeond.py b/system/ubloxd/pigeond.py similarity index 97% rename from system/sensord/pigeond.py rename to system/ubloxd/pigeond.py index 78b3b07498..5711992cfb 100755 --- a/system/sensord/pigeond.py +++ b/system/ubloxd/pigeond.py @@ -7,7 +7,6 @@ import struct import requests import urllib.parse from datetime import datetime -from typing import List, Optional, Tuple from cereal import messaging from openpilot.common.params import Params @@ -41,7 +40,7 @@ def add_ubx_checksum(msg: bytes) -> bytes: B = (B + A) % 256 return msg + bytes([A, B]) -def get_assistnow_messages(token: bytes) -> List[bytes]: +def get_assistnow_messages(token: bytes) -> list[bytes]: # make request # TODO: implement adding the last known location r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({ @@ -62,7 +61,7 @@ def get_assistnow_messages(token: bytes) -> List[bytes]: return msgs -class TTYPigeon(): +class TTYPigeon: def __init__(self): self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0) @@ -238,7 +237,7 @@ def initialize_pigeon(pigeon: TTYPigeon) -> bool: return False return True -def deinitialize_and_exit(pigeon: Optional[TTYPigeon]): +def deinitialize_and_exit(pigeon: TTYPigeon | None): cloudlog.warning("Storing almanac in ublox flash") if pigeon is not None: @@ -259,7 +258,7 @@ def deinitialize_and_exit(pigeon: Optional[TTYPigeon]): set_power(False) sys.exit(0) -def create_pigeon() -> Tuple[TTYPigeon, messaging.PubMaster]: +def create_pigeon() -> tuple[TTYPigeon, messaging.PubMaster]: pigeon = None # register exit handler diff --git a/system/ubloxd/tests/print_gps_stats.py b/system/ubloxd/tests/print_gps_stats.py new file mode 100755 index 0000000000..edf94c060a --- /dev/null +++ b/system/ubloxd/tests/print_gps_stats.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python3 +import time +import cereal.messaging as messaging + +if __name__ == "__main__": + sm = messaging.SubMaster(['ubloxGnss', 'gpsLocationExternal']) + + while 1: + ug = sm['ubloxGnss'] + gle = sm['gpsLocationExternal'] + + try: + cnos = [] + for m in ug.measurementReport.measurements: + cnos.append(m.cno) + print("Sats: %d Accuracy: %.2f m cnos" % (ug.measurementReport.numMeas, gle.horizontalAccuracy), sorted(cnos)) + except Exception: + pass + sm.update() + time.sleep(0.1) diff --git a/system/ubloxd/tests/test_glonass_kaitai.cc b/system/ubloxd/tests/test_glonass_kaitai.cc new file mode 100644 index 0000000000..96f43742b4 --- /dev/null +++ b/system/ubloxd/tests/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "system/ubloxd/generated/glonass.h" + +typedef std::vector> string_data; + +#define IDLE_CHIP_IDX 0 +#define STRING_NUMBER_IDX 1 +// string data 1-5 +#define HC_IDX 0 +#define PAD1_IDX 1 +#define SUPERFRAME_IDX 2 +#define PAD2_IDX 3 +#define FRAME_IDX 4 + +// Indexes for string number 1 +#define ST1_NU_IDX 2 +#define ST1_P1_IDX 3 +#define ST1_T_K_IDX 4 +#define ST1_X_VEL_S_IDX 5 +#define ST1_X_VEL_V_IDX 6 +#define ST1_X_ACCEL_S_IDX 7 +#define ST1_X_ACCEL_V_IDX 8 +#define ST1_X_S_IDX 9 +#define ST1_X_V_IDX 10 +#define ST1_HC_OFF 11 + +// Indexes for string number 2 +#define ST2_BN_IDX 2 +#define ST2_P2_IDX 3 +#define ST2_TB_IDX 4 +#define ST2_NU_IDX 5 +#define ST2_Y_VEL_S_IDX 6 +#define ST2_Y_VEL_V_IDX 7 +#define ST2_Y_ACCEL_S_IDX 8 +#define ST2_Y_ACCEL_V_IDX 9 +#define ST2_Y_S_IDX 10 +#define ST2_Y_V_IDX 11 +#define ST2_HC_OFF 12 + +// Indexes for string number 3 +#define ST3_P3_IDX 2 +#define ST3_GAMMA_N_S_IDX 3 +#define ST3_GAMMA_N_V_IDX 4 +#define ST3_NU_1_IDX 5 +#define ST3_P_IDX 6 +#define ST3_L_N_IDX 7 +#define ST3_Z_VEL_S_IDX 8 +#define ST3_Z_VEL_V_IDX 9 +#define ST3_Z_ACCEL_S_IDX 10 +#define ST3_Z_ACCEL_V_IDX 11 +#define ST3_Z_S_IDX 12 +#define ST3_Z_V_IDX 13 +#define ST3_HC_OFF 14 + +// Indexes for string number 4 +#define ST4_TAU_N_S_IDX 2 +#define ST4_TAU_N_V_IDX 3 +#define ST4_DELTA_TAU_N_S_IDX 4 +#define ST4_DELTA_TAU_N_V_IDX 5 +#define ST4_E_N_IDX 6 +#define ST4_NU_1_IDX 7 +#define ST4_P4_IDX 8 +#define ST4_F_T_IDX 9 +#define ST4_NU_2_IDX 10 +#define ST4_N_T_IDX 11 +#define ST4_N_IDX 12 +#define ST4_M_IDX 13 +#define ST4_HC_OFF 14 + +// Indexes for string number 5 +#define ST5_N_A_IDX 2 +#define ST5_TAU_C_IDX 3 +#define ST5_NU_IDX 4 +#define ST5_N_4_IDX 5 +#define ST5_TAU_GPS_IDX 6 +#define ST5_L_N_IDX 7 +#define ST5_HC_OFF 8 + +// Indexes for non immediate +#define ST6_DATA_1_IDX 2 +#define ST6_DATA_2_IDX 3 +#define ST6_HC_OFF 4 + + +std::string generate_inp_data(string_data& data) { + std::string inp_data = ""; + for (auto& [b, v] : data) { + std::string tmp = std::bitset<64>(v).to_string(); + inp_data += tmp.substr(64-b, b); + } + assert(inp_data.size() == 128); + + std::string string_data; + string_data.reserve(16); + for (int i = 0; i < 128; i+=8) { + std::string substr = inp_data.substr(i, 8); + string_data.push_back((uint8_t)std::stoi(substr.c_str(), 0, 2)); + } + + return string_data; +} + +string_data generate_string_data(uint8_t string_number) { + + srand((unsigned)time(0)); + string_data data; // + data.push_back({1, 0}); // idle chip + data.push_back({4, string_number}); // string number + + if (string_number == 1) { + data.push_back({2, 3}); // not_used + data.push_back({2, 1}); // p1 + data.push_back({12, 113}); // t_k + data.push_back({1, rand() & 1}); // x_vel_sign + data.push_back({23, 7122}); // x_vel_value + data.push_back({1, rand() & 1}); // x_accel_sign + data.push_back({4, 3}); // x_accel_value + data.push_back({1, rand() & 1}); // x_sign + data.push_back({26, 33554431}); // x_value + } else if (string_number == 2) { + data.push_back({3, 3}); // b_n + data.push_back({1, 1}); // p2 + data.push_back({7, 123}); // t_b + data.push_back({5, 31}); // not_used + data.push_back({1, rand() & 1}); // y_vel_sign + data.push_back({23, 7422}); // y_vel_value + data.push_back({1, rand() & 1}); // y_accel_sign + data.push_back({4, 3}); // y_accel_value + data.push_back({1, rand() & 1}); // y_sign + data.push_back({26, 67108863}); // y_value + } else if (string_number == 3) { + data.push_back({1, 0}); // p3 + data.push_back({1, 1}); // gamma_n_sign + data.push_back({10, 123}); // gamma_n_value + data.push_back({1, 0}); // not_used + data.push_back({2, 2}); // p + data.push_back({1, 1}); // l_n + data.push_back({1, rand() & 1}); // z_vel_sign + data.push_back({23, 1337}); // z_vel_value + data.push_back({1, rand() & 1}); // z_accel_sign + data.push_back({4, 9}); // z_accel_value + data.push_back({1, rand() & 1}); // z_sign + data.push_back({26, 100023}); // z_value + } else if (string_number == 4) { + data.push_back({1, rand() & 1}); // tau_n_sign + data.push_back({21, 197152}); // tau_n_value + data.push_back({1, rand() & 1}); // delta_tau_n_sign + data.push_back({4, 4}); // delta_tau_n_value + data.push_back({5, 0}); // e_n + data.push_back({14, 2}); // not_used_1 + data.push_back({1, 1}); // p4 + data.push_back({4, 9}); // f_t + data.push_back({3, 3}); // not_used_2 + data.push_back({11, 2047}); // n_t + data.push_back({5, 2}); // n + data.push_back({2, 1}); // m + } else if (string_number == 5) { + data.push_back({11, 2047}); // n_a + data.push_back({32, 4294767295}); // tau_c + data.push_back({1, 0}); // not_used_1 + data.push_back({5, 2}); // n_4 + data.push_back({22, 4114304}); // tau_gps + data.push_back({1, 0}); // l_n + } else { // non-immediate data is not parsed + data.push_back({64, rand()}); // data_1 + data.push_back({8, 6}); // data_2 + } + + data.push_back({8, rand() & 0xFF}); // hamming code + data.push_back({11, rand() & 0x7FF}); // pad + data.push_back({16, rand() & 0xFFFF}); // superframe + data.push_back({8, rand() & 0xFF}); // pad + data.push_back({8, rand() & 0xFF}); // frame + return data; +} + +TEST_CASE("parse_string_number_1"){ + string_data data = generate_string_data(1); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str1(inp_data); + glonass_t str1_data(&str1); + glonass_t::string_1_t* s1 = static_cast(str1_data.data()); + + REQUIRE(s1->not_used() == data[ST1_NU_IDX].second); + REQUIRE(s1->p1() == data[ST1_P1_IDX].second); + REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second); + + int mul = s1->x_vel_sign() ? (-1) : 1; + REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul)); + mul = s1->x_accel_sign() ? (-1) : 1; + REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul)); + mul = s1->x_sign() ? (-1) : 1; + REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_2"){ + string_data data = generate_string_data(2); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str2(inp_data); + glonass_t str2_data(&str2); + glonass_t::string_2_t* s2 = static_cast(str2_data.data()); + + REQUIRE(s2->b_n() == data[ST2_BN_IDX].second); + REQUIRE(s2->not_used() == data[ST2_NU_IDX].second); + REQUIRE(s2->p2() == data[ST2_P2_IDX].second); + REQUIRE(s2->t_b() == data[ST2_TB_IDX].second); + int mul = s2->y_vel_sign() ? (-1) : 1; + REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul)); + mul = s2->y_accel_sign() ? (-1) : 1; + REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul)); + mul = s2->y_sign() ? (-1) : 1; + REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_3"){ + string_data data = generate_string_data(3); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str3(inp_data); + glonass_t str3_data(&str3); + glonass_t::string_3_t* s3 = static_cast(str3_data.data()); + + REQUIRE(s3->p3() == data[ST3_P3_IDX].second); + int mul = s3->gamma_n_sign() ? (-1) : 1; + REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul)); + REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second); + REQUIRE(s3->p() == data[ST3_P_IDX].second); + REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second); + mul = s3->z_vel_sign() ? (-1) : 1; + REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul)); + mul = s3->z_accel_sign() ? (-1) : 1; + REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul)); + mul = s3->z_sign() ? (-1) : 1; + REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul)); +} + +TEST_CASE("parse_string_number_4"){ + string_data data = generate_string_data(4); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str4(inp_data); + glonass_t str4_data(&str4); + glonass_t::string_4_t* s4 = static_cast(str4_data.data()); + + int mul = s4->tau_n_sign() ? (-1) : 1; + REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul)); + mul = s4->delta_tau_n_sign() ? (-1) : 1; + REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul)); + REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second); + REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second); + REQUIRE(s4->p4() == data[ST4_P4_IDX].second); + REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second); + REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second); + REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second); + REQUIRE(s4->n() == data[ST4_N_IDX].second); + REQUIRE(s4->m() == data[ST4_M_IDX].second); +} + +TEST_CASE("parse_string_number_5"){ + string_data data = generate_string_data(5); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second); + + kaitai::kstream str5(inp_data); + glonass_t str5_data(&str5); + glonass_t::string_5_t* s5 = static_cast(str5_data.data()); + + REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second); + REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second); + REQUIRE(s5->not_used() == data[ST5_NU_IDX].second); + REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second); + REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second); + REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second); +} + +TEST_CASE("parse_string_number_NI"){ + string_data data = generate_string_data((rand() % 10) + 6); + std::string inp_data = generate_inp_data(data); + + kaitai::kstream stream(inp_data); + glonass_t gl_string(&stream); + + REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second); + REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second); + REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second); + REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second); + REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second); + REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second); + REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second); + + kaitai::kstream strni(inp_data); + glonass_t strni_data(&strni); + glonass_t::string_non_immediate_t* sni = static_cast(strni_data.data()); + + REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); + REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); +} diff --git a/system/ubloxd/tests/test_glonass_runner.cc b/system/ubloxd/tests/test_glonass_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/system/ubloxd/tests/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/system/ubloxd/tests/test_pigeond.py b/system/ubloxd/tests/test_pigeond.py new file mode 100644 index 0000000000..202820e412 --- /dev/null +++ b/system/ubloxd/tests/test_pigeond.py @@ -0,0 +1,54 @@ +import pytest +import time + +import cereal.messaging as messaging +from cereal.services import SERVICE_LIST +from openpilot.common.gpio import gpio_read +from openpilot.selfdrive.test.helpers import with_processes +from openpilot.system.manager.process_config import managed_processes +from openpilot.system.hardware.tici.pins import GPIO + + +# TODO: test TTFF when we have good A-GNSS +@pytest.mark.tici +class TestPigeond: + + def teardown_method(self): + managed_processes['pigeond'].stop() + + @with_processes(['pigeond']) + def test_frequency(self): + sm = messaging.SubMaster(['ubloxRaw']) + + # setup time + for _ in range(int(5 * SERVICE_LIST['ubloxRaw'].frequency)): + sm.update() + + for _ in range(int(10 * SERVICE_LIST['ubloxRaw'].frequency)): + sm.update() + assert sm.all_checks() + + def test_startup_time(self): + for _ in range(5): + sm = messaging.SubMaster(['ubloxRaw']) + managed_processes['pigeond'].start() + + start_time = time.monotonic() + for __ in range(10): + sm.update(1 * 1000) + if sm.updated['ubloxRaw']: + break + assert sm.recv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time" + + et = time.monotonic() - start_time + assert et < 5, f"pigeond took {et:.1f}s to start" + managed_processes['pigeond'].stop() + + def test_turns_off_ublox(self): + for s in (0.1, 0.5, 1, 5): + managed_processes['pigeond'].start() + time.sleep(s) + managed_processes['pigeond'].stop() + + assert gpio_read(GPIO.UBLOX_RST_N) == 0 + assert gpio_read(GPIO.GNSS_PWR_EN) == 0 diff --git a/system/ubloxd/tests/ubloxd.py b/system/ubloxd/tests/ubloxd.py new file mode 100755 index 0000000000..c17387114f --- /dev/null +++ b/system/ubloxd/tests/ubloxd.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# type: ignore + +from openpilot.selfdrive.locationd.test import ublox +import struct + +baudrate = 460800 +rate = 100 # send new data every 100ms + + +def configure_ublox(dev): + # configure ports and solution parameters and rate + dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1) # enable only UBX on USB + dev.configure_port(port=0, inMask=0, outMask=0) # disable DDC + + payload = struct.pack(' UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) { auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal(); gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX); gpsLoc.setFlags(msg->flags()); + gpsLoc.setHasFix((msg->flags() % 2) == 1); gpsLoc.setLatitude(msg->lat() * 1e-07); gpsLoc.setLongitude(msg->lon() * 1e-07); gpsLoc.setAltitude(msg->height() * 1e-03); gpsLoc.setSpeed(msg->g_speed() * 1e-03); gpsLoc.setBearingDeg(msg->head_mot() * 1e-5); - gpsLoc.setAccuracy(msg->h_acc() * 1e-03); + gpsLoc.setHorizontalAccuracy(msg->h_acc() * 1e-03); std::tm timeinfo = std::tm(); timeinfo.tm_year = msg->year() - 1900; timeinfo.tm_mon = msg->month() - 1; diff --git a/system/ubloxd/ubx.ksy b/system/ubloxd/ubx.ksy new file mode 100644 index 0000000000..02c757fe71 --- /dev/null +++ b/system/ubloxd/ubx.ksy @@ -0,0 +1,293 @@ +meta: + id: ubx + endian: le +seq: + - id: magic + contents: [0xb5, 0x62] + - id: msg_type + type: u2be + - id: length + type: u2 + - id: body + type: + switch-on: msg_type + cases: + 0x0107: nav_pvt + 0x0213: rxm_sfrbx + 0x0215: rxm_rawx + 0x0a09: mon_hw + 0x0a0b: mon_hw2 + 0x0135: nav_sat +instances: + checksum: + pos: length + 6 + type: u2 + +types: + mon_hw: + seq: + - id: pin_sel + type: u4 + - id: pin_bank + type: u4 + - id: pin_dir + type: u4 + - id: pin_val + type: u4 + - id: noise_per_ms + type: u2 + - id: agc_cnt + type: u2 + - id: a_status + type: u1 + enum: antenna_status + - id: a_power + type: u1 + enum: antenna_power + - id: flags + type: u1 + - id: reserved1 + size: 1 + - id: used_mask + type: u4 + - id: vp + size: 17 + - id: jam_ind + type: u1 + - id: reserved2 + size: 2 + - id: pin_irq + type: u4 + - id: pull_h + type: u4 + - id: pull_l + type: u4 + enums: + antenna_status: + 0: init + 1: dontknow + 2: ok + 3: short + 4: open + antenna_power: + 0: off + 1: on + 2: dontknow + + mon_hw2: + seq: + - id: ofs_i + type: s1 + - id: mag_i + type: u1 + - id: ofs_q + type: s1 + - id: mag_q + type: u1 + - id: cfg_source + type: u1 + enum: config_source + - id: reserved1 + size: 3 + - id: low_lev_cfg + type: u4 + - id: reserved2 + size: 8 + - id: post_status + type: u4 + - id: reserved3 + size: 4 + + enums: + config_source: + 113: rom + 111: otp + 112: config_pins + 102: flash + + rxm_sfrbx: + seq: + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved1 + size: 1 + - id: freq_id + type: u1 + - id: num_words + type: u1 + - id: reserved2 + size: 1 + - id: version + type: u1 + - id: reserved3 + size: 1 + - id: body + type: u4 + repeat: expr + repeat-expr: num_words + + rxm_rawx: + seq: + - id: rcv_tow + type: f8 + - id: week + type: u2 + - id: leap_s + type: s1 + - id: num_meas + type: u1 + - id: rec_stat + type: u1 + - id: reserved1 + size: 3 + - id: meas + type: measurement + size: 32 + repeat: expr + repeat-expr: num_meas + types: + measurement: + seq: + - id: pr_mes + type: f8 + - id: cp_mes + type: f8 + - id: do_mes + type: f4 + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: reserved2 + size: 1 + - id: freq_id + type: u1 + - id: lock_time + type: u2 + - id: cno + type: u1 + - id: pr_stdev + type: u1 + - id: cp_stdev + type: u1 + - id: do_stdev + type: u1 + - id: trk_stat + type: u1 + - id: reserved3 + size: 1 + nav_sat: + seq: + - id: itow + type: u4 + - id: version + type: u1 + - id: num_svs + type: u1 + - id: reserved + size: 2 + - id: svs + type: nav + size: 12 + repeat: expr + repeat-expr: num_svs + types: + nav: + seq: + - id: gnss_id + type: u1 + enum: gnss_type + - id: sv_id + type: u1 + - id: cno + type: u1 + - id: elev + type: s1 + - id: azim + type: s2 + - id: pr_res + type: s2 + - id: flags + type: u4 + + nav_pvt: + seq: + - id: i_tow + type: u4 + - id: year + type: u2 + - id: month + type: u1 + - id: day + type: u1 + - id: hour + type: u1 + - id: min + type: u1 + - id: sec + type: u1 + - id: valid + type: u1 + - id: t_acc + type: u4 + - id: nano + type: s4 + - id: fix_type + type: u1 + - id: flags + type: u1 + - id: flags2 + type: u1 + - id: num_sv + type: u1 + - id: lon + type: s4 + - id: lat + type: s4 + - id: height + type: s4 + - id: h_msl + type: s4 + - id: h_acc + type: u4 + - id: v_acc + type: u4 + - id: vel_n + type: s4 + - id: vel_e + type: s4 + - id: vel_d + type: s4 + - id: g_speed + type: s4 + - id: head_mot + type: s4 + - id: s_acc + type: s4 + - id: head_acc + type: u4 + - id: p_dop + type: u2 + - id: flags3 + type: u1 + - id: reserved1 + size: 5 + - id: head_veh + type: s4 + - id: mag_dec + type: s2 + - id: mag_acc + type: u2 +enums: + gnss_type: + 0: gps + 1: sbas + 2: galileo + 3: beidou + 4: imes + 5: qzss + 6: glonass diff --git a/system/ugpsd.py b/system/ugpsd.py new file mode 100755 index 0000000000..34b20b01c8 --- /dev/null +++ b/system/ugpsd.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +import os +import time +import traceback +import serial +import datetime +import numpy as np +from collections import defaultdict + +from cereal import log +import cereal.messaging as messaging +from openpilot.common.retry import retry +from openpilot.common.swaglog import cloudlog +from openpilot.system.qcomgpsd.qcomgpsd import at_cmd, wait_for_modem + + +def sfloat(n: str): + return float(n) if len(n) > 0 else 0 + +def checksum(s: str): + ret = 0 + for c in s[1:-3]: + ret ^= ord(c) + return format(ret, '02X') + +class Unicore: + def __init__(self): + self.s = serial.Serial('/dev/ttyHS0', 115200) + self.s.timeout = 1 + self.s.writeTimeout = 1 + self.s.newline = b'\r\n' + + self.s.flush() + self.s.reset_input_buffer() + self.s.reset_output_buffer() + self.s.read(2048) + + def send(self, cmd): + self.s.write(cmd.encode('utf8') + b'\r') + resp = self.s.read(2048) + print(len(resp), cmd, "\n", resp) + assert b"OK" in resp + + def recv(self): + return self.s.readline() + +def build_msg(state): + """ + NMEA sentences: + https://campar.in.tum.de/twiki/pub/Chair/NaviGpsDemon/nmea.html#RMC + NAV messages: + https://www.unicorecomm.com/assets/upload/file/UFirebird_Standard_Positioning_Products_Protocol_Specification_CH.pdf + """ + + msg = messaging.new_message('gpsLocation', valid=True) + gps = msg.gpsLocation + + gnrmc = state['$GNRMC'] + gps.hasFix = gnrmc[1] == 'A' + gps.source = log.GpsLocationData.SensorSource.unicore + gps.latitude = (sfloat(gnrmc[3][:2]) + (sfloat(gnrmc[3][2:]) / 60)) * (1 if gnrmc[4] == "N" else -1) + gps.longitude = (sfloat(gnrmc[5][:3]) + (sfloat(gnrmc[5][3:]) / 60)) * (1 if gnrmc[6] == "E" else -1) + + try: + date = gnrmc[9][:6] + dt = datetime.datetime.strptime(f"{date} {gnrmc[1]}", '%d%m%y %H%M%S.%f') + gps.unixTimestampMillis = dt.timestamp()*1e3 + except Exception: + pass + + gps.bearingDeg = sfloat(gnrmc[8]) + + if len(state['$GNGGA']): + gngga = state['$GNGGA'] + if gngga[10] == 'M': + gps.altitude = sfloat(gngga[9]) + + if len(state['$GNGSA']): + gngsa = state['$GNGSA'] + gps.horizontalAccuracy = sfloat(gngsa[4]) + gps.verticalAccuracy = sfloat(gngsa[5]) + + #if len(state['$NAVACC']): + # # $NAVVEL,264415000,5,3,0.375,0.141,-0.735,-65.450*2A + # navacc = state['$NAVACC'] + # gps.horizontalAccuracy = sfloat(navacc[3]) + # gps.speedAccuracy = sfloat(navacc[4]) + # gps.bearingAccuracyDeg = sfloat(navacc[5]) + + if len(state['$NAVVEL']): + # $NAVVEL,264415000,5,3,0.375,0.141,-0.735,-65.450*2A + navvel = state['$NAVVEL'] + vECEF = [ + sfloat(navvel[4]), + sfloat(navvel[5]), + sfloat(navvel[6]), + ] + + lat = np.radians(gps.latitude) + lon = np.radians(gps.longitude) + R = np.array([ + [-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)], + [-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)], + [np.cos(lat), 0, -np.sin(lat)] + ]) + + vNED = [float(x) for x in R.dot(vECEF)] + gps.vNED = vNED + gps.speed = np.linalg.norm(vNED) + + # TODO: set these from the module + gps.bearingAccuracyDeg = 5. + gps.speedAccuracy = 3. + + return msg + + +@retry(attempts=10, delay=0.1) +def setup(u): + at_cmd('AT+CGPS=0') + at_cmd('AT+CGPS=1') + time.sleep(1.0) + + # setup NAVXXX outputs + for i in range(4): + u.send(f"$CFGMSG,1,{i},1") + for i in (1, 3): + u.send(f"$CFGMSG,3,{i},1") + + # 10Hz NAV outputs + u.send("$CFGNAV,100,100,1000") + + +def main(): + wait_for_modem("AT+CGPS?") + + u = Unicore() + setup(u) + + state = defaultdict(list) + pm = messaging.PubMaster(['gpsLocation']) + while True: + try: + msg = u.recv().decode('utf8').strip() + if "DEBUG" in os.environ: + print(repr(msg)) + + if len(msg) > 0: + if checksum(msg) != msg.split('*')[1]: + cloudlog.error(f"invalid checksum: {repr(msg)}") + continue + + k = msg.split(',')[0] + state[k] = msg.split(',') + if '$GNRMC' not in msg: + continue + + pm.send('gpsLocation', build_msg(state)) + except Exception: + traceback.print_exc() + cloudlog.exception("gps.issue") + + +if __name__ == "__main__": + main() diff --git a/system/hardware/tici/casync.py b/system/updated/casync/casync.py similarity index 75% rename from system/hardware/tici/casync.py rename to system/updated/casync/casync.py index 993336616d..7a3303a9e9 100755 --- a/system/hardware/tici/casync.py +++ b/system/updated/casync/casync.py @@ -2,15 +2,19 @@ import io import lzma import os +import pathlib import struct import sys import time from abc import ABC, abstractmethod from collections import defaultdict, namedtuple -from typing import Callable, Dict, List, Optional, Tuple +from collections.abc import Callable +from typing import IO import requests from Crypto.Hash import SHA512 +from openpilot.system.updated.casync import tar +from openpilot.system.updated.casync.common import create_casync_tar_package CA_FORMAT_INDEX = 0x96824d9c7b129ff9 CA_FORMAT_TABLE = 0xe75b9e112f17417d @@ -28,7 +32,7 @@ CHUNK_DOWNLOAD_RETRIES = 3 CAIBX_DOWNLOAD_TIMEOUT = 120 Chunk = namedtuple('Chunk', ['sha', 'offset', 'length']) -ChunkDict = Dict[bytes, Chunk] +ChunkDict = dict[bytes, Chunk] class ChunkReader(ABC): @@ -37,20 +41,25 @@ class ChunkReader(ABC): ... -class FileChunkReader(ChunkReader): +class BinaryChunkReader(ChunkReader): """Reads chunks from a local file""" - def __init__(self, fn: str) -> None: + def __init__(self, file_like: IO[bytes]) -> None: super().__init__() - self.f = open(fn, 'rb') - - def __del__(self): - self.f.close() + self.f = file_like def read(self, chunk: Chunk) -> bytes: self.f.seek(chunk.offset) return self.f.read(chunk.length) +class FileChunkReader(BinaryChunkReader): + def __init__(self, path: str) -> None: + super().__init__(open(path, 'rb')) + + def __del__(self): + self.f.close() + + class RemoteChunkReader(ChunkReader): """Reads lzma compressed chunks from a remote store""" @@ -83,7 +92,21 @@ class RemoteChunkReader(ChunkReader): return decompressor.decompress(contents) -def parse_caibx(caibx_path: str) -> List[Chunk]: +class DirectoryTarChunkReader(BinaryChunkReader): + """creates a tar archive of a directory and reads chunks from it""" + + def __init__(self, path: str, cache_file: str) -> None: + create_casync_tar_package(pathlib.Path(path), pathlib.Path(cache_file)) + + self.f = open(cache_file, "rb") + return super().__init__(self.f) + + def __del__(self): + self.f.close() + os.unlink(self.f.name) + + +def parse_caibx(caibx_path: str) -> list[Chunk]: """Parses the chunks from a caibx file. Can handle both local and remote files. Returns a list of chunks with hash, offset and length""" caibx: io.BufferedIOBase @@ -132,7 +155,7 @@ def parse_caibx(caibx_path: str) -> List[Chunk]: return chunks -def build_chunk_dict(chunks: List[Chunk]) -> ChunkDict: +def build_chunk_dict(chunks: list[Chunk]) -> ChunkDict: """Turn a list of chunks into a dict for faster lookups based on hash. Keep first chunk since it's more likely to be already downloaded.""" r = {} @@ -142,11 +165,11 @@ def build_chunk_dict(chunks: List[Chunk]) -> ChunkDict: return r -def extract(target: List[Chunk], - sources: List[Tuple[str, ChunkReader, ChunkDict]], +def extract(target: list[Chunk], + sources: list[tuple[str, ChunkReader, ChunkDict]], out_path: str, - progress: Optional[Callable[[int], None]] = None): - stats: Dict[str, int] = defaultdict(int) + progress: Callable[[int], None] = None): + stats: dict[str, int] = defaultdict(int) mode = 'rb+' if os.path.exists(out_path) else 'wb' with open(out_path, mode) as out: @@ -181,7 +204,22 @@ def extract(target: List[Chunk], return stats -def print_stats(stats: Dict[str, int]): +def extract_directory(target: list[Chunk], + sources: list[tuple[str, ChunkReader, ChunkDict]], + out_path: str, + tmp_file: str, + progress: Callable[[int], None] = None): + """extract a directory stored as a casync tar archive""" + + stats = extract(target, sources, tmp_file, progress) + + with open(tmp_file, "rb") as f: + tar.extract_tar_archive(f, pathlib.Path(out_path)) + + return stats + + +def print_stats(stats: dict[str, int]): total_bytes = sum(stats.values()) print(f"Total size: {total_bytes / 1024 / 1024:.2f} MB") for name, total in stats.items(): diff --git a/system/updated/casync/common.py b/system/updated/casync/common.py new file mode 100644 index 0000000000..6979f5cb06 --- /dev/null +++ b/system/updated/casync/common.py @@ -0,0 +1,61 @@ +import dataclasses +import json +import pathlib +import subprocess + +from openpilot.system.version import BUILD_METADATA_FILENAME, BuildMetadata +from openpilot.system.updated.casync import tar + + +CASYNC_ARGS = ["--with=symlinks", "--with=permissions", "--compression=xz", "--chunk-size=16M"] +CASYNC_FILES = [BUILD_METADATA_FILENAME] + + +def run(cmd): + return subprocess.check_output(cmd) + + +def get_exclude_set(path) -> set[str]: + exclude_set = set(CASYNC_FILES) + + for file in path.rglob("*"): + if file.is_file() or file.is_symlink(): + + while file.resolve() != path.resolve(): + exclude_set.add(str(file.relative_to(path))) + + file = file.parent + + return exclude_set + + +def create_build_metadata_file(path: pathlib.Path, build_metadata: BuildMetadata): + with open(path / BUILD_METADATA_FILENAME, "w") as f: + build_metadata_dict = dataclasses.asdict(build_metadata) + build_metadata_dict["openpilot"].pop("is_dirty") # this is determined at runtime + build_metadata_dict.pop("channel") # channel is unrelated to the build itself + f.write(json.dumps(build_metadata_dict)) + + +def is_not_git(path: pathlib.Path) -> bool: + return ".git" not in path.parts + + +def create_casync_tar_package(target_dir: pathlib.Path, output_path: pathlib.Path): + tar.create_tar_archive(output_path, target_dir, is_not_git) + + +def create_casync_from_file(file: pathlib.Path, output_dir: pathlib.Path, caibx_name: str): + caibx_file = output_dir / f"{caibx_name}.caibx" + run(["casync", "make", *CASYNC_ARGS, caibx_file, str(file)]) + + return caibx_file + + +def create_casync_release(target_dir: pathlib.Path, output_dir: pathlib.Path, caibx_name: str): + tar_file = output_dir / f"{caibx_name}.tar" + create_casync_tar_package(target_dir, tar_file) + caibx_file = create_casync_from_file(tar_file, output_dir, caibx_name) + tar_file.unlink() + digest = run(["casync", "digest", *CASYNC_ARGS, target_dir]).decode("utf-8").strip() + return digest, caibx_file diff --git a/system/updated/casync/tar.py b/system/updated/casync/tar.py new file mode 100644 index 0000000000..a5a8238bba --- /dev/null +++ b/system/updated/casync/tar.py @@ -0,0 +1,39 @@ +import pathlib +import tarfile +from typing import IO +from collections.abc import Callable + + +def include_default(_) -> bool: + return True + + +def create_tar_archive(filename: pathlib.Path, directory: pathlib.Path, include: Callable[[pathlib.Path], bool] = include_default): + """Creates a tar archive of a directory""" + + with tarfile.open(filename, 'w') as tar: + for file in sorted(directory.rglob("*"), key=lambda f: f.stat().st_size if f.is_file() else 0, reverse=True): + if not include(file): + continue + relative_path = str(file.relative_to(directory)) + if file.is_symlink(): + info = tarfile.TarInfo(relative_path) + info.type = tarfile.SYMTYPE + info.linkpath = str(file.readlink()) + tar.addfile(info) + + elif file.is_file(): + info = tarfile.TarInfo(relative_path) + info.size = file.stat().st_size + info.type = tarfile.REGTYPE + info.mode = file.stat().st_mode + with file.open('rb') as f: + tar.addfile(info, f) + + +def extract_tar_archive(fh: IO[bytes], directory: pathlib.Path): + """Extracts a tar archive to a directory""" + + tar = tarfile.open(fileobj=fh, mode='r') + tar.extractall(str(directory), filter=lambda info, path: info) + tar.close() diff --git a/system/updated/casync/tests/test_casync.py b/system/updated/casync/tests/test_casync.py new file mode 100644 index 0000000000..bc171e7432 --- /dev/null +++ b/system/updated/casync/tests/test_casync.py @@ -0,0 +1,264 @@ +import pytest +import os +import pathlib +import tempfile +import subprocess + +from openpilot.system.updated.casync import casync +from openpilot.system.updated.casync import tar + +# dd if=/dev/zero of=/tmp/img.raw bs=1M count=2 +# sudo losetup -f /tmp/img.raw +# losetup -a | grep img.raw +LOOPBACK = os.environ.get('LOOPBACK', None) + + +@pytest.mark.skip("not used yet") +class TestCasync: + @classmethod + def setup_class(cls): + cls.tmpdir = tempfile.TemporaryDirectory() + + # Build example contents + chunk_a = [i % 256 for i in range(1024)] * 512 + chunk_b = [(256 - i) % 256 for i in range(1024)] * 512 + zeroes = [0] * (1024 * 128) + contents = chunk_a + chunk_b + zeroes + chunk_a + + cls.contents = bytes(contents) + + # Write to file + cls.orig_fn = os.path.join(cls.tmpdir.name, 'orig.bin') + with open(cls.orig_fn, 'wb') as f: + f.write(cls.contents) + + # Create casync files + cls.manifest_fn = os.path.join(cls.tmpdir.name, 'orig.caibx') + cls.store_fn = os.path.join(cls.tmpdir.name, 'store') + subprocess.check_output(["casync", "make", "--compression=xz", "--store", cls.store_fn, cls.manifest_fn, cls.orig_fn]) + + target = casync.parse_caibx(cls.manifest_fn) + hashes = [c.sha.hex() for c in target] + + # Ensure we have chunk reuse + assert len(hashes) > len(set(hashes)) + + def setup_method(self): + # Clear target_lo + if LOOPBACK is not None: + self.target_lo = LOOPBACK + with open(self.target_lo, 'wb') as f: + f.write(b"0" * len(self.contents)) + + self.target_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) + self.seed_fn = os.path.join(self.tmpdir.name, next(tempfile._get_candidate_names())) + + def teardown_method(self): + for fn in [self.target_fn, self.seed_fn]: + try: + os.unlink(fn) + except FileNotFoundError: + pass + + def test_simple_extract(self): + target = casync.parse_caibx(self.manifest_fn) + + sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as target_f: + assert target_f.read() == self.contents + + assert stats['remote'] == len(self.contents) + + def test_seed(self): + target = casync.parse_caibx(self.manifest_fn) + + # Populate seed with half of the target contents + with open(self.seed_fn, 'wb') as seed_f: + seed_f.write(self.contents[:len(self.contents) // 2]) + + sources = [('seed', casync.FileChunkReader(self.seed_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as target_f: + assert target_f.read() == self.contents + + assert stats['seed'] > 0 + assert stats['remote'] < len(self.contents) + + def test_already_done(self): + """Test that an already flashed target doesn't download any chunks""" + target = casync.parse_caibx(self.manifest_fn) + + with open(self.target_fn, 'wb') as f: + f.write(self.contents) + + sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as f: + assert f.read() == self.contents + + assert stats['target'] == len(self.contents) + + def test_chunk_reuse(self): + """Test that chunks that are reused are only downloaded once""" + target = casync.parse_caibx(self.manifest_fn) + + # Ensure target exists + with open(self.target_fn, 'wb'): + pass + + sources = [('target', casync.FileChunkReader(self.target_fn), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_fn) + + with open(self.target_fn, 'rb') as f: + assert f.read() == self.contents + + assert stats['remote'] < len(self.contents) + + @pytest.mark.skipif(not LOOPBACK, reason="requires loopback device") + def test_lo_simple_extract(self): + target = casync.parse_caibx(self.manifest_fn) + sources = [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_lo) + + with open(self.target_lo, 'rb') as target_f: + assert target_f.read(len(self.contents)) == self.contents + + assert stats['remote'] == len(self.contents) + + @pytest.mark.skipif(not LOOPBACK, reason="requires loopback device") + def test_lo_chunk_reuse(self): + """Test that chunks that are reused are only downloaded once""" + target = casync.parse_caibx(self.manifest_fn) + + sources = [('target', casync.FileChunkReader(self.target_lo), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract(target, sources, self.target_lo) + + with open(self.target_lo, 'rb') as f: + assert f.read(len(self.contents)) == self.contents + + assert stats['remote'] < len(self.contents) + + +@pytest.mark.skip("not used yet") +class TestCasyncDirectory: + """Tests extracting a directory stored as a casync tar archive""" + + NUM_FILES = 16 + + @classmethod + def setup_cache(cls, directory, files=None): + if files is None: + files = range(cls.NUM_FILES) + + chunk_a = [i % 256 for i in range(1024)] * 512 + chunk_b = [(256 - i) % 256 for i in range(1024)] * 512 + zeroes = [0] * (1024 * 128) + cls.contents = chunk_a + chunk_b + zeroes + chunk_a + cls.contents = bytes(cls.contents) + + for i in files: + with open(os.path.join(directory, f"file_{i}.txt"), "wb") as f: + f.write(cls.contents) + + os.symlink(f"file_{i}.txt", os.path.join(directory, f"link_{i}.txt")) + + @classmethod + def setup_class(cls): + cls.tmpdir = tempfile.TemporaryDirectory() + + # Create casync files + cls.manifest_fn = os.path.join(cls.tmpdir.name, 'orig.caibx') + cls.store_fn = os.path.join(cls.tmpdir.name, 'store') + + cls.directory_to_extract = tempfile.TemporaryDirectory() + cls.setup_cache(cls.directory_to_extract.name) + + cls.orig_fn = os.path.join(cls.tmpdir.name, 'orig.tar') + tar.create_tar_archive(cls.orig_fn, pathlib.Path(cls.directory_to_extract.name)) + + subprocess.check_output(["casync", "make", "--compression=xz", "--store", cls.store_fn, cls.manifest_fn, cls.orig_fn]) + + @classmethod + def teardown_class(cls): + cls.tmpdir.cleanup() + cls.directory_to_extract.cleanup() + + def setup_method(self): + self.cache_dir = tempfile.TemporaryDirectory() + self.working_dir = tempfile.TemporaryDirectory() + self.out_dir = tempfile.TemporaryDirectory() + + def teardown_method(self): + self.cache_dir.cleanup() + self.working_dir.cleanup() + self.out_dir.cleanup() + + def run_test(self): + target = casync.parse_caibx(self.manifest_fn) + + cache_filename = os.path.join(self.working_dir.name, "cache.tar") + tmp_filename = os.path.join(self.working_dir.name, "tmp.tar") + + sources = [('cache', casync.DirectoryTarChunkReader(self.cache_dir.name, cache_filename), casync.build_chunk_dict(target))] + sources += [('remote', casync.RemoteChunkReader(self.store_fn), casync.build_chunk_dict(target))] + + stats = casync.extract_directory(target, sources, pathlib.Path(self.out_dir.name), tmp_filename) + + with open(os.path.join(self.out_dir.name, "file_0.txt"), "rb") as f: + assert f.read() == self.contents + + with open(os.path.join(self.out_dir.name, "link_0.txt"), "rb") as f: + assert f.read() == self.contents + assert os.readlink(os.path.join(self.out_dir.name, "link_0.txt")) == "file_0.txt" + + return stats + + def test_no_cache(self): + self.setup_cache(self.cache_dir.name, []) + stats = self.run_test() + assert stats['remote'] > 0 + assert stats['cache'] == 0 + + def test_full_cache(self): + self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) + stats = self.run_test() + assert stats['remote'] == 0 + assert stats['cache'] > 0 + + def test_one_file_cache(self): + self.setup_cache(self.cache_dir.name, range(1)) + stats = self.run_test() + assert stats['remote'] > 0 + assert stats['cache'] > 0 + assert stats['cache'] < stats['remote'] + + def test_one_file_incorrect_cache(self): + self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) + with open(os.path.join(self.cache_dir.name, "file_0.txt"), "wb") as f: + f.write(b"1234") + + stats = self.run_test() + assert stats['remote'] > 0 + assert stats['cache'] > 0 + assert stats['cache'] > stats['remote'] + + def test_one_file_missing_cache(self): + self.setup_cache(self.cache_dir.name, range(self.NUM_FILES)) + os.unlink(os.path.join(self.cache_dir.name, "file_12.txt")) + + stats = self.run_test() + assert stats['remote'] > 0 + assert stats['cache'] > 0 + assert stats['cache'] > stats['remote'] diff --git a/system/updated/common.py b/system/updated/common.py new file mode 100644 index 0000000000..6bb745f6b0 --- /dev/null +++ b/system/updated/common.py @@ -0,0 +1,16 @@ +import os +import pathlib + + +def get_consistent_flag(path: str) -> bool: + consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) + return consistent_file.is_file() + +def set_consistent_flag(path: str, consistent: bool) -> None: + os.sync() + consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) + if consistent: + consistent_file.touch() + elif not consistent: + consistent_file.unlink(missing_ok=True) + os.sync() diff --git a/system/updated/tests/test_base.py b/system/updated/tests/test_base.py new file mode 100644 index 0000000000..928d07cbe3 --- /dev/null +++ b/system/updated/tests/test_base.py @@ -0,0 +1,259 @@ +import os +import pathlib +import shutil +import signal +import stat +import subprocess +import tempfile +import time +import pytest + +from openpilot.common.params import Params +from openpilot.system.manager.process import ManagerProcess +from openpilot.selfdrive.test.helpers import processes_context + + +def get_consistent_flag(path: str) -> bool: + consistent_file = pathlib.Path(os.path.join(path, ".overlay_consistent")) + return consistent_file.is_file() + + +def run(args, **kwargs): + return subprocess.check_output(args, **kwargs) + + +def update_release(directory, name, version, agnos_version, release_notes): + with open(directory / "RELEASES.md", "w") as f: + f.write(release_notes) + + (directory / "common").mkdir(exist_ok=True) + + with open(directory / "common" / "version.h", "w") as f: + f.write(f'#define COMMA_VERSION "{version}"') + + launch_env = directory / "launch_env.sh" + with open(launch_env, "w") as f: + f.write(f'export AGNOS_VERSION="{agnos_version}"') + + st = os.stat(launch_env) + os.chmod(launch_env, st.st_mode | stat.S_IEXEC) + + test_symlink = directory / "test_symlink" + if not os.path.exists(str(test_symlink)): + os.symlink("common/version.h", test_symlink) + + +def get_version(path: str) -> str: + with open(os.path.join(path, "common", "version.h")) as f: + return f.read().split('"')[1] + + +@pytest.mark.slow # TODO: can we test overlayfs in GHA? +class TestBaseUpdate: + @classmethod + def setup_class(cls): + if "Base" in cls.__name__: + pytest.skip() + + def setup_method(self): + self.tmpdir = tempfile.mkdtemp() + + run(["sudo", "mount", "-t", "tmpfs", "tmpfs", self.tmpdir]) # overlayfs doesn't work inside of docker unless this is a tmpfs + + self.mock_update_path = pathlib.Path(self.tmpdir) + + self.params = Params() + + self.basedir = self.mock_update_path / "openpilot" + self.basedir.mkdir() + + self.staging_root = self.mock_update_path / "safe_staging" + self.staging_root.mkdir() + + self.remote_dir = self.mock_update_path / "remote" + self.remote_dir.mkdir() + + os.environ["UPDATER_STAGING_ROOT"] = str(self.staging_root) + os.environ["UPDATER_LOCK_FILE"] = str(self.mock_update_path / "safe_staging_overlay.lock") + + self.MOCK_RELEASES = { + "release3": ("0.1.2", "1.2", "0.1.2 release notes"), + "master": ("0.1.3", "1.2", "0.1.3 release notes"), + } + + @pytest.fixture(autouse=True) + def mock_basedir(self, mocker): + mocker.patch("openpilot.common.basedir.BASEDIR", self.basedir) + + def set_target_branch(self, branch): + self.params.put("UpdaterTargetBranch", branch) + + def setup_basedir_release(self, release): + self.params = Params() + self.set_target_branch(release) + + def update_remote_release(self, release): + raise NotImplementedError("") + + def setup_remote_release(self, release): + raise NotImplementedError("") + + def additional_context(self): + raise NotImplementedError("") + + def teardown_method(self): + try: + run(["sudo", "umount", "-l", str(self.staging_root / "merged")]) + run(["sudo", "umount", "-l", self.tmpdir]) + shutil.rmtree(self.tmpdir) + except Exception: + print("cleanup failed...") + + def wait_for_condition(self, condition, timeout=12): + start = time.monotonic() + while True: + waited = time.monotonic() - start + if condition(): + print(f"waited {waited}s for condition ") + return waited + + if waited > timeout: + raise TimeoutError("timed out waiting for condition") + + time.sleep(1) + + def _test_finalized_update(self, branch, version, agnos_version, release_notes): + assert get_version(str(self.staging_root / "finalized")) == version + assert get_consistent_flag(str(self.staging_root / "finalized")) + assert os.access(str(self.staging_root / "finalized" / "launch_env.sh"), os.X_OK) + + with open(self.staging_root / "finalized" / "test_symlink") as f: + assert version in f.read() + +class ParamsBaseUpdateTest(TestBaseUpdate): + def _test_finalized_update(self, branch, version, agnos_version, release_notes): + assert self.params.get("UpdaterNewDescription", encoding="utf-8").startswith(f"{version} / {branch}") + assert self.params.get("UpdaterNewReleaseNotes", encoding="utf-8") == f"

    {release_notes}

    \n" + super()._test_finalized_update(branch, version, agnos_version, release_notes) + + def send_check_for_updates_signal(self, updated: ManagerProcess): + updated.signal(signal.SIGUSR1.value) + + def send_download_signal(self, updated: ManagerProcess): + updated.signal(signal.SIGHUP.value) + + def _test_params(self, branch, fetch_available, update_available): + assert self.params.get("UpdaterTargetBranch", encoding="utf-8") == branch + assert self.params.get_bool("UpdaterFetchAvailable") == fetch_available + assert self.params.get_bool("UpdateAvailable") == update_available + + def wait_for_idle(self): + self.wait_for_condition(lambda: self.params.get("UpdaterState", encoding="utf-8") == "idle") + + def wait_for_failed(self): + self.wait_for_condition(lambda: self.params.get("UpdateFailedCount", encoding="utf-8") is not None and \ + int(self.params.get("UpdateFailedCount", encoding="utf-8")) > 0) + + def wait_for_fetch_available(self): + self.wait_for_condition(lambda: self.params.get_bool("UpdaterFetchAvailable")) + + def wait_for_update_available(self): + self.wait_for_condition(lambda: self.params.get_bool("UpdateAvailable")) + + def test_no_update(self): + # Start on release3, ensure we don't fetch any updates + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.send_check_for_updates_signal(updated) + + self.wait_for_idle() + + self._test_params("release3", False, False) + + def test_new_release(self): + # Start on release3, simulate a release3 commit, ensure we fetch that update properly + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.2", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("release3", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("release3", False, True) + self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) + + def test_switch_branches(self): + # Start on release3, request to switch to master manually, ensure we switched + self.setup_remote_release("release3") + self.setup_remote_release("master") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.set_target_branch("master") + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("master", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("master", False, True) + self._test_finalized_update("master", *self.MOCK_RELEASES["master"]) + + def test_agnos_update(self, mocker): + # Start on release3, push an update with an agnos change + self.setup_remote_release("release3") + self.setup_basedir_release("release3") + + with self.additional_context(), processes_context(["updated"]) as [updated]: + mocker.patch("openpilot.system.hardware.AGNOS", "True") + mocker.patch("openpilot.system.hardware.tici.hardware.Tici.get_os_version", "1.2") + mocker.patch("openpilot.system.hardware.tici.agnos.get_target_slot_number") + mocker.patch("openpilot.system.hardware.tici.agnos.flash_agnos_update") + + self._test_params("release3", False, False) + self.wait_for_idle() + self._test_params("release3", False, False) + + self.MOCK_RELEASES["release3"] = ("0.1.3", "1.3", "0.1.3 release notes") + self.update_remote_release("release3") + + self.send_check_for_updates_signal(updated) + + self.wait_for_fetch_available() + + self._test_params("release3", True, False) + + self.send_download_signal(updated) + + self.wait_for_update_available() + + self._test_params("release3", False, True) + self._test_finalized_update("release3", *self.MOCK_RELEASES["release3"]) diff --git a/system/updated/tests/test_git.py b/system/updated/tests/test_git.py new file mode 100644 index 0000000000..5a5a27000b --- /dev/null +++ b/system/updated/tests/test_git.py @@ -0,0 +1,22 @@ +import contextlib +from openpilot.system.updated.tests.test_base import ParamsBaseUpdateTest, run, update_release + + +class TestUpdateDGitStrategy(ParamsBaseUpdateTest): + def update_remote_release(self, release): + update_release(self.remote_dir, release, *self.MOCK_RELEASES[release]) + run(["git", "add", "."], cwd=self.remote_dir) + run(["git", "commit", "-m", f"openpilot release {release}"], cwd=self.remote_dir) + + def setup_remote_release(self, release): + run(["git", "init"], cwd=self.remote_dir) + run(["git", "checkout", "-b", release], cwd=self.remote_dir) + self.update_remote_release(release) + + def setup_basedir_release(self, release): + super().setup_basedir_release(release) + run(["git", "clone", "-b", release, self.remote_dir, self.basedir]) + + @contextlib.contextmanager + def additional_context(self): + yield diff --git a/selfdrive/updated.py b/system/updated/updated.py similarity index 97% rename from selfdrive/updated.py rename to system/updated/updated.py index 3e4a849d21..d43f439af5 100755 --- a/selfdrive/updated.py +++ b/system/updated/updated.py @@ -11,16 +11,15 @@ import time import threading from collections import defaultdict from pathlib import Path -from typing import List, Union, Optional -from markdown_it import MarkdownIt from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.time import system_time_valid -from openpilot.system.hardware import AGNOS, HARDWARE +from openpilot.common.markdown import parse_markdown from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert -from openpilot.system.version import is_tested_branch +from openpilot.system.hardware import AGNOS, HARDWARE +from openpilot.system.version import get_build_metadata LOCK_FILE = os.getenv("UPDATER_LOCK_FILE", "/tmp/safe_staging_overlay.lock") STAGING_ROOT = os.getenv("UPDATER_STAGING_ROOT", "/data/safe_staging") @@ -64,7 +63,7 @@ def write_time_to_param(params, param) -> None: t = datetime.datetime.utcnow() params.put(param, t.isoformat().encode('utf8')) -def read_time_from_param(params, param) -> Optional[datetime.datetime]: +def read_time_from_param(params, param) -> datetime.datetime | None: t = params.get(param, encoding='utf8') try: return datetime.datetime.fromisoformat(t) @@ -72,7 +71,7 @@ def read_time_from_param(params, param) -> Optional[datetime.datetime]: pass return None -def run(cmd: List[str], cwd: Optional[str] = None) -> str: +def run(cmd: list[str], cwd: str = None) -> str: return subprocess.check_output(cmd, cwd=cwd, stderr=subprocess.STDOUT, encoding='utf8') @@ -90,7 +89,7 @@ def parse_release_notes(basedir: str) -> bytes: with open(os.path.join(basedir, "RELEASES.md"), "rb") as f: r = f.read().split(b'\n\n', 1)[0] # Slice latest release notes try: - return bytes(MarkdownIt().render(r.decode("utf-8")), encoding="utf-8") + return bytes(parse_markdown(r.decode("utf-8")), encoding="utf-8") except Exception: return r + b"\n" except FileNotFoundError: @@ -234,7 +233,7 @@ def handle_agnos_update() -> None: class Updater: def __init__(self): self.params = Params() - self.branches = defaultdict(lambda: '') + self.branches = defaultdict(str) self._has_internet: bool = False @property @@ -243,7 +242,7 @@ class Updater: @property def target_branch(self) -> str: - b: Union[str, None] = self.params.get("UpdaterTargetBranch", encoding='utf-8') + b: str | None = self.params.get("UpdaterTargetBranch", encoding='utf-8') if b is None: b = self.get_branch(BASEDIR) return b @@ -272,7 +271,7 @@ class Updater: def get_commit_hash(self, path: str = OVERLAY_MERGED) -> str: return run(["git", "rev-parse", "HEAD"], path).rstrip() - def set_params(self, update_success: bool, failed_count: int, exception: Optional[str]) -> None: + def set_params(self, update_success: bool, failed_count: int, exception: str | None) -> None: self.params.put("UpdateFailedCount", str(failed_count)) self.params.put("UpdaterTargetBranch", self.target_branch) @@ -326,8 +325,9 @@ class Updater: now = datetime.datetime.utcnow() dt = now - last_update + build_metadata = get_build_metadata() if failed_count > 15 and exception is not None and self.has_internet: - if is_tested_branch(): + if build_metadata.tested_channel: extra_text = "Ensure the software is correctly installed. Uninstall and re-install if this error persists." else: extra_text = exception diff --git a/system/version.py b/system/version.py index e34458f16f..94964526b3 100755 --- a/system/version.py +++ b/system/version.py @@ -1,104 +1,42 @@ #!/usr/bin/env python3 +from dataclasses import dataclass +from functools import cache +import json import os +import pathlib import subprocess -from typing import List, Callable, TypeVar -from functools import lru_cache from openpilot.common.basedir import BASEDIR from openpilot.common.swaglog import cloudlog +from openpilot.common.git import get_commit, get_origin, get_branch, get_short_branch, get_commit_date RELEASE_BRANCHES = ['release3-staging', 'release3', 'nightly'] TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging'] +BUILD_METADATA_FILENAME = "build.json" + training_version: bytes = b"0.2.0" terms_version: bytes = b"2" -_RT = TypeVar("_RT") -def cache(user_function: Callable[..., _RT], /) -> Callable[..., _RT]: - return lru_cache(maxsize=None)(user_function) - - -def run_cmd(cmd: List[str]) -> str: - return subprocess.check_output(cmd, encoding='utf8').strip() - - -def run_cmd_default(cmd: List[str], default: str = "") -> str: - try: - return run_cmd(cmd) - except subprocess.CalledProcessError: - return default - - -@cache -def get_commit(branch: str = "HEAD") -> str: - return run_cmd_default(["git", "rev-parse", branch]) - - -@cache -def get_commit_date(commit: str = "HEAD") -> str: - return run_cmd_default(["git", "show", "--no-patch", "--format='%ct %ci'", commit]) - -@cache -def get_short_branch() -> str: - return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "HEAD"]) - - -@cache -def get_branch() -> str: - return run_cmd_default(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"]) - - -@cache -def get_origin() -> str: - try: - local_branch = run_cmd(["git", "name-rev", "--name-only", "HEAD"]) - tracking_remote = run_cmd(["git", "config", "branch." + local_branch + ".remote"]) - return run_cmd(["git", "config", "remote." + tracking_remote + ".url"]) - except subprocess.CalledProcessError: # Not on a branch, fallback - return run_cmd_default(["git", "config", "--get", "remote.origin.url"]) - - -@cache -def get_normalized_origin() -> str: - return get_origin() \ - .replace("git@", "", 1) \ - .replace(".git", "", 1) \ - .replace("https://", "", 1) \ - .replace(":", "/", 1) - - -@cache -def get_version() -> str: - with open(os.path.join(BASEDIR, "common", "version.h")) as _versionf: +def get_version(path: str = BASEDIR) -> str: + with open(os.path.join(path, "common", "version.h")) as _versionf: version = _versionf.read().split('"')[1] return version -@cache -def get_short_version() -> str: - return get_version().split('-')[0] - -@cache -def is_prebuilt() -> bool: - return os.path.exists(os.path.join(BASEDIR, 'prebuilt')) +def get_release_notes(path: str = BASEDIR) -> str: + with open(os.path.join(path, "RELEASES.md")) as f: + return f.read().split('\n\n', 1)[0] -@cache -def is_comma_remote() -> bool: - # note to fork maintainers, this is used for release metrics. please do not - # touch this to get rid of the orange startup alert. there's better ways to do that - return get_normalized_origin() == "github.com/commaai/openpilot" @cache -def is_tested_branch() -> bool: - return get_short_branch() in TESTED_BRANCHES +def is_prebuilt(path: str = BASEDIR) -> bool: + return os.path.exists(os.path.join(path, 'prebuilt')) -@cache -def is_release_branch() -> bool: - return get_short_branch() in RELEASE_BRANCHES @cache -def is_dirty() -> bool: +def is_dirty(cwd: str = BASEDIR) -> bool: origin = get_origin() branch = get_branch() if not origin or not branch: @@ -107,14 +45,14 @@ def is_dirty() -> bool: dirty = False try: # Actually check dirty files - if not is_prebuilt(): + if not is_prebuilt(cwd): # This is needed otherwise touched files might show up as modified try: - subprocess.check_call(["git", "update-index", "--refresh"]) + subprocess.check_call(["git", "update-index", "--refresh"], cwd=cwd) except subprocess.CalledProcessError: pass - dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0) + dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"], cwd=cwd)) != 0 except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") dirty = True @@ -122,6 +60,101 @@ def is_dirty() -> bool: return dirty +@dataclass +class OpenpilotMetadata: + version: str + release_notes: str + git_commit: str + git_origin: str + git_commit_date: str + build_style: str + is_dirty: bool # whether there are local changes + + @property + def short_version(self) -> str: + return self.version.split('-')[0] + + @property + def comma_remote(self) -> bool: + # note to fork maintainers, this is used for release metrics. please do not + # touch this to get rid of the orange startup alert. there's better ways to do that + return self.git_normalized_origin == "github.com/commaai/openpilot" + + @property + def git_normalized_origin(self) -> str: + return self.git_origin \ + .replace("git@", "", 1) \ + .replace(".git", "", 1) \ + .replace("https://", "", 1) \ + .replace(":", "/", 1) + + +@dataclass +class BuildMetadata: + channel: str + openpilot: OpenpilotMetadata + + @property + def tested_channel(self) -> bool: + return self.channel in TESTED_BRANCHES + + @property + def release_channel(self) -> bool: + return self.channel in RELEASE_BRANCHES + + @property + def canonical(self) -> str: + return f"{self.openpilot.version}-{self.openpilot.git_commit}-{self.openpilot.build_style}" + + @property + def ui_description(self) -> str: + return f"{self.openpilot.version} / {self.openpilot.git_commit[:6]} / {self.channel}" + + +def build_metadata_from_dict(build_metadata: dict) -> BuildMetadata: + channel = build_metadata.get("channel", "unknown") + openpilot_metadata = build_metadata.get("openpilot", {}) + version = openpilot_metadata.get("version", "unknown") + release_notes = openpilot_metadata.get("release_notes", "unknown") + git_commit = openpilot_metadata.get("git_commit", "unknown") + git_origin = openpilot_metadata.get("git_origin", "unknown") + git_commit_date = openpilot_metadata.get("git_commit_date", "unknown") + build_style = openpilot_metadata.get("build_style", "unknown") + return BuildMetadata(channel, + OpenpilotMetadata( + version=version, + release_notes=release_notes, + git_commit=git_commit, + git_origin=git_origin, + git_commit_date=git_commit_date, + build_style=build_style, + is_dirty=False)) + + +def get_build_metadata(path: str = BASEDIR) -> BuildMetadata: + build_metadata_path = pathlib.Path(path) / BUILD_METADATA_FILENAME + + if build_metadata_path.exists(): + build_metadata = json.loads(build_metadata_path.read_text()) + return build_metadata_from_dict(build_metadata) + + git_folder = pathlib.Path(path) / ".git" + + if git_folder.exists(): + return BuildMetadata(get_short_branch(path), + OpenpilotMetadata( + version=get_version(path), + release_notes=get_release_notes(path), + git_commit=get_commit(path), + git_origin=get_origin(path), + git_commit_date=get_commit_date(path), + build_style="unknown", + is_dirty=is_dirty(path))) + + cloudlog.exception("unable to get build metadata") + raise Exception("invalid build metadata") + + if __name__ == "__main__": from openpilot.common.params import Params @@ -129,12 +162,4 @@ if __name__ == "__main__": params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) - print(f"Dirty: {is_dirty()}") - print(f"Version: {get_version()}") - print(f"Short version: {get_short_version()}") - print(f"Origin: {get_origin()}") - print(f"Normalized origin: {get_normalized_origin()}") - print(f"Branch: {get_branch()}") - print(f"Short branch: {get_short_branch()}") - print(f"Prebuilt: {is_prebuilt()}") - print(f"Commit date: {get_commit_date()}") + print(get_build_metadata()) diff --git a/system/webrtc/device/audio.py b/system/webrtc/device/audio.py index 3c78be6752..4b22033e03 100644 --- a/system/webrtc/device/audio.py +++ b/system/webrtc/device/audio.py @@ -1,6 +1,5 @@ import asyncio import io -from typing import Optional, List, Tuple import aiortc import av @@ -17,7 +16,7 @@ class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack): pyaudio.paFloat32: 'flt', } - def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: Optional[int] = None): + def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: int = None): super().__init__() self.p = pyaudio.PyAudio() @@ -49,7 +48,7 @@ class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack): class AudioOutputSpeaker: - def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: Optional[int] = None): + def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: int = None): chunk_size = int(packet_time * rate) self.p = pyaudio.PyAudio() @@ -62,7 +61,7 @@ class AudioOutputSpeaker: output=True, output_device_index=device_index, stream_callback=self.__pyaudio_callback) - self.tracks_and_tasks: List[Tuple[aiortc.MediaStreamTrack, Optional[asyncio.Task]]] = [] + self.tracks_and_tasks: list[tuple[aiortc.MediaStreamTrack, asyncio.Task | None]] = [] def __pyaudio_callback(self, in_data, frame_count, time_info, status): if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2: diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py index 314f812834..1bca909294 100644 --- a/system/webrtc/device/video.py +++ b/system/webrtc/device/video.py @@ -1,5 +1,4 @@ import asyncio -from typing import Optional import av from teleoprtc.tracks import TiciVideoStreamTrack @@ -40,5 +39,5 @@ class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): return packet - def codec_preference(self) -> Optional[str]: + def codec_preference(self) -> str | None: return "H264" diff --git a/system/webrtc/schema.py b/system/webrtc/schema.py index f659b34293..d80986ebf2 100644 --- a/system/webrtc/schema.py +++ b/system/webrtc/schema.py @@ -1,8 +1,8 @@ import capnp -from typing import Union, List, Dict, Any +from typing import Any -def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]: +def generate_type(type_walker, schema_walker) -> str | list[Any] | dict[str, Any]: data_type = next(type_walker) if data_type.which() == 'struct': return generate_struct(next(schema_walker)) @@ -15,11 +15,11 @@ def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, return str(data_type.which()) -def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]: +def generate_struct(schema: capnp.lib.capnp._StructSchema) -> dict[str, Any]: return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")} -def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]: +def generate_field(field: capnp.lib.capnp._StructSchemaField) -> str | list[Any] | dict[str, Any]: def schema_walker(field): yield field.schema diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py new file mode 100644 index 0000000000..e89e42e440 --- /dev/null +++ b/system/webrtc/tests/test_stream_session.py @@ -0,0 +1,100 @@ +import asyncio +import json +# for aiortc and its dependencies +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +from aiortc import RTCDataChannel +from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE +import capnp +import pyaudio +from cereal import messaging, log + +from openpilot.system.webrtc.webrtcd import CerealOutgoingMessageProxy, CerealIncomingMessageProxy +from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack +from openpilot.system.webrtc.device.audio import AudioInputStreamTrack +from openpilot.common.realtime import DT_DMON + + +class TestStreamSession: + def setup_method(self): + self.loop = asyncio.new_event_loop() + + def teardown_method(self): + self.loop.stop() + self.loop.close() + + def test_outgoing_proxy(self, mocker): + test_msg = log.Event.new_message() + test_msg.logMonoTime = 123 + test_msg.valid = True + test_msg.customReservedRawData0 = b"test" + expected_dict = {"type": "customReservedRawData0", "logMonoTime": 123, "valid": True, "data": "test"} + expected_json = json.dumps(expected_dict).encode() + + channel = mocker.Mock(spec=RTCDataChannel) + mocked_submaster = messaging.SubMaster(["customReservedRawData0"]) + def mocked_update(t): + mocked_submaster.update_msgs(0, [test_msg]) + + mocker.patch.object(messaging.SubMaster, "update", side_effect=mocked_update) + proxy = CerealOutgoingMessageProxy(mocked_submaster) + proxy.add_channel(channel) + + proxy.update() + + channel.send.assert_called_once_with(expected_json) + + def test_incoming_proxy(self, mocker): + tested_msgs = [ + {"type": "customReservedRawData0", "data": "test"}, # primitive + {"type": "can", "data": [{"address": 0, "busTime": 0, "dat": "", "src": 0}]}, # list + {"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict + ] + + mocked_pubmaster = mocker.MagicMock(spec=messaging.PubMaster) + + proxy = CerealIncomingMessageProxy(mocked_pubmaster) + + for msg in tested_msgs: + proxy.send(json.dumps(msg).encode()) + + mocked_pubmaster.send.assert_called_once() + mt, md = mocked_pubmaster.send.call_args.args + assert mt == msg["type"] + assert isinstance(md, capnp._DynamicStructBuilder) + assert hasattr(md, msg["type"]) + + mocked_pubmaster.reset_mock() + + def test_livestream_track(self, mocker): + fake_msg = messaging.new_message("livestreamDriverEncodeData") + + config = {"receive.return_value": fake_msg.to_bytes()} + mocker.patch("msgq.SubSocket", spec=True, **config) + track = LiveStreamVideoStreamTrack("driver") + + assert track.id.startswith("driver") + assert track.codec_preference() == "H264" + + for i in range(5): + packet = self.loop.run_until_complete(track.recv()) + assert packet.time_base == VIDEO_TIME_BASE + assert packet.pts == int(i * DT_DMON * VIDEO_CLOCK_RATE) + assert packet.size == 0 + + def test_input_audio_track(self, mocker): + packet_time, rate = 0.02, 16000 + sample_count = int(packet_time * rate) + mocked_stream = mocker.MagicMock(spec=pyaudio.Stream) + mocked_stream.read.return_value = b"\x00" * 2 * sample_count + + config = {"open.side_effect": lambda *args, **kwargs: mocked_stream} + mocker.patch("pyaudio.PyAudio", spec=True, **config) + track = AudioInputStreamTrack(audio_format=pyaudio.paInt16, packet_time=packet_time, rate=rate) + + for i in range(5): + frame = self.loop.run_until_complete(track.recv()) + assert frame.rate == rate + assert frame.samples == sample_count + assert frame.pts == i * sample_count diff --git a/system/webrtc/tests/test_webrtcd.py b/system/webrtc/tests/test_webrtcd.py new file mode 100644 index 0000000000..d4b659a3aa --- /dev/null +++ b/system/webrtc/tests/test_webrtcd.py @@ -0,0 +1,64 @@ +import pytest +import asyncio +import json +# for aiortc and its dependencies +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +from openpilot.system.webrtc.webrtcd import get_stream + +import aiortc +from teleoprtc import WebRTCOfferBuilder +from parameterized import parameterized_class + + +@parameterized_class(("in_services", "out_services"), [ + (["testJoystick"], ["carState"]), + ([], ["carState"]), + (["testJoystick"], []), + ([], []), +]) +@pytest.mark.asyncio +class TestWebrtcdProc: + async def assertCompletesWithTimeout(self, awaitable, timeout=1): + try: + async with asyncio.timeout(timeout): + await awaitable + except TimeoutError: + pytest.fail("Timeout while waiting for awaitable to complete") + + async def test_webrtcd(self, mocker): + mock_request = mocker.MagicMock() + async def connect(offer): + body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services} + mock_request.json.side_effect = mocker.AsyncMock(return_value=body) + response = await get_stream(mock_request) + response_json = json.loads(response.text) + return aiortc.RTCSessionDescription(**response_json) + + builder = WebRTCOfferBuilder(connect) + builder.offer_to_receive_video_stream("road") + builder.offer_to_receive_audio_stream() + if len(self.in_services) > 0 or len(self.out_services) > 0: + builder.add_messaging() + + stream = builder.stream() + + await self.assertCompletesWithTimeout(stream.start()) + await self.assertCompletesWithTimeout(stream.wait_for_connection()) + + assert stream.has_incoming_video_track("road") + assert stream.has_incoming_audio_track() + assert stream.has_messaging_channel() == (len(self.in_services) > 0 or len(self.out_services) > 0) + + video_track, audio_track = stream.get_incoming_video_track("road"), stream.get_incoming_audio_track() + await self.assertCompletesWithTimeout(video_track.recv()) + await self.assertCompletesWithTimeout(audio_track.recv()) + + await self.assertCompletesWithTimeout(stream.stop()) + + # cleanup, very implementation specific, test may break if it changes + assert mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test" + _, session = mock_request.app["streams"].__setitem__.call_args.args + await self.assertCompletesWithTimeout(session.post_run_cleanup()) + diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index cc26d50daf..afd346857f 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -6,7 +6,7 @@ import json import uuid import logging from dataclasses import dataclass, field -from typing import Any, List, Optional, Union, TYPE_CHECKING +from typing import Any, TYPE_CHECKING # aiortc and its dependencies have lots of internal warnings :( import warnings @@ -24,7 +24,7 @@ from cereal import messaging, log class CerealOutgoingMessageProxy: def __init__(self, sm: messaging.SubMaster): self.sm = sm - self.channels: List['RTCDataChannel'] = [] + self.channels: list[RTCDataChannel] = [] def add_channel(self, channel: 'RTCDataChannel'): self.channels.append(channel) @@ -97,13 +97,27 @@ class CerealProxyRunner: except InvalidStateError: self.logger.warning("Cereal outgoing proxy invalid state (connection closed)") break - except Exception as ex: - self.logger.error("Cereal outgoing proxy failure: %s", ex) + except Exception: + self.logger.exception("Cereal outgoing proxy failure") await asyncio.sleep(0.01) +class DynamicPubMaster(messaging.PubMaster): + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + self.lock = asyncio.Lock() + + async def add_services_if_needed(self, services): + async with self.lock: + for service in services: + if service not in self.sock: + self.sock[service] = messaging.pub_sock(service) + + class StreamSession: - def __init__(self, sdp: str, cameras: List[str], incoming_services: List[str], outgoing_services: List[str], debug_mode: bool = False): + shared_pub_master = DynamicPubMaster([]) + + def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], debug_mode: bool = False): from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack from aiortc.contrib.media import MediaBlackhole from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack @@ -128,12 +142,18 @@ class StreamSession: self.stream = builder.stream() self.identifier = str(uuid.uuid4()) - self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) - self.incoming_bridge = CerealIncomingMessageProxy(messaging.PubMaster(incoming_services)) - self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) - - self.audio_output: Optional[Union[AudioOutputSpeaker, MediaBlackhole]] = None - self.run_task: Optional[asyncio.Task] = None + self.incoming_bridge: CerealIncomingMessageProxy | None = None + self.incoming_bridge_services = incoming_services + self.outgoing_bridge: CerealOutgoingMessageProxy | None = None + self.outgoing_bridge_runner: CerealProxyRunner | None = None + if len(incoming_services) > 0: + self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) + if len(outgoing_services) > 0: + self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) + self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) + + self.audio_output: AudioOutputSpeaker | MediaBlackhole | None = None + self.run_task: asyncio.Task | None = None self.logger = logging.getLogger("webrtcd") self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s", self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services) @@ -152,19 +172,23 @@ class StreamSession: return await self.stream.start() async def message_handler(self, message: bytes): + assert self.incoming_bridge is not None try: self.incoming_bridge.send(message) - except Exception as ex: - self.logger.error("Cereal incoming proxy failure: %s", ex) + except Exception: + self.logger.exception("Cereal incoming proxy failure") async def run(self): try: await self.stream.wait_for_connection() if self.stream.has_messaging_channel(): - self.stream.set_message_handler(self.message_handler) - channel = self.stream.get_messaging_channel() - self.outgoing_bridge_runner.proxy.add_channel(channel) - self.outgoing_bridge_runner.start() + if self.incoming_bridge is not None: + await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services) + self.stream.set_message_handler(self.message_handler) + if self.outgoing_bridge_runner is not None: + channel = self.stream.get_messaging_channel() + self.outgoing_bridge_runner.proxy.add_channel(channel) + self.outgoing_bridge_runner.start() if self.stream.has_incoming_audio_track(): track = self.stream.get_incoming_audio_track(buffered=False) self.audio_output = self.audio_output_cls() @@ -176,12 +200,13 @@ class StreamSession: await self.post_run_cleanup() self.logger.info("Stream session (%s) ended", self.identifier) - except Exception as ex: - self.logger.error("Stream session failure: %s", ex) + except Exception: + self.logger.exception("Stream session failure") async def post_run_cleanup(self): await self.stream.stop() - self.outgoing_bridge_runner.stop() + if self.outgoing_bridge is not None: + self.outgoing_bridge_runner.stop() if self.audio_output: self.audio_output.stop() @@ -189,9 +214,9 @@ class StreamSession: @dataclass class StreamRequestBody: sdp: str - cameras: List[str] - bridge_services_in: List[str] = field(default_factory=list) - bridge_services_out: List[str] = field(default_factory=list) + cameras: list[str] + bridge_services_in: list[str] = field(default_factory=list) + bridge_services_out: list[str] = field(default_factory=list) async def get_stream(request: 'web.Request'): diff --git a/teleoprtc b/teleoprtc new file mode 120000 index 0000000000..3d3dbc8dea --- /dev/null +++ b/teleoprtc @@ -0,0 +1 @@ +teleoprtc_repo/teleoprtc \ No newline at end of file diff --git a/teleoprtc_repo/.gitignore b/teleoprtc_repo/.gitignore new file mode 100644 index 0000000000..e9863cb7ee --- /dev/null +++ b/teleoprtc_repo/.gitignore @@ -0,0 +1,163 @@ +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +.pybuilder/ +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# poetry +# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. +# This is especially recommended for binary packages to ensure reproducibility, and is more +# commonly ignored for libraries. +# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control +#poetry.lock + +# pdm +# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. +#pdm.lock +# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it +# in version control. +# https://pdm.fming.dev/#use-with-ide +.pdm.toml + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ + +# PyCharm +# JetBrains specific template is maintained in a separate JetBrains.gitignore that can +# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore +# and can be added to the global gitignore or merged into this file. For a more nuclear +# option (not recommended) you can uncomment the following to ignore the entire idea folder. +#.idea/ + +*.swp +*.swo \ No newline at end of file diff --git a/teleoprtc_repo/README.md b/teleoprtc_repo/README.md new file mode 100644 index 0000000000..f20d25db50 --- /dev/null +++ b/teleoprtc_repo/README.md @@ -0,0 +1,4 @@ +# teleoprtc + +Set of abstractions for webRTC communication with [openpilot](https://github.com/commaai/openpilot). + diff --git a/teleoprtc_repo/examples/face_detection/README.md b/teleoprtc_repo/examples/face_detection/README.md new file mode 100644 index 0000000000..74d6ea2cf0 --- /dev/null +++ b/teleoprtc_repo/examples/face_detection/README.md @@ -0,0 +1,10 @@ +## Face detection demo + +Run simple face detection model on video stream from driver camera of comma three. + +This example streams video frames, runs face-detection model and displays window with live detection results (bounding boxes). + +```sh +# pass the ip address of comma three, if running remotely (by default localhost) +python3 face_detection.py [--host comma-ip-address] +``` \ No newline at end of file diff --git a/teleoprtc_repo/examples/face_detection/face_detection.py b/teleoprtc_repo/examples/face_detection/face_detection.py new file mode 100755 index 0000000000..f6fba90640 --- /dev/null +++ b/teleoprtc_repo/examples/face_detection/face_detection.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 + +import argparse +import asyncio + +import aiortc +import aiohttp +import cv2 +import pygame + +from teleoprtc import WebRTCOfferBuilder, StreamingOffer + + +def pygame_should_quit(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + return True + return False + + +class WebrtcdConnectionProvider: + """ + Connection provider reaching webrtcd server on comma three + """ + def __init__(self, host, port=5001): + self.url = f"http://{host}:{port}/stream" + + async def __call__(self, offer: StreamingOffer) -> aiortc.RTCSessionDescription: + async with aiohttp.ClientSession() as session: + body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': [], 'bridge_services_out': []} + async with session.post(self.url, json=body) as resp: + payload = await resp.json() + answer = aiortc.RTCSessionDescription(**payload) + return answer + + +class FaceDetector: + """ + Simple face detector using opencv + """ + def __init__(self): + self.classifier = cv2.CascadeClassifier(cv2.data.haarcascades + 'haarcascade_frontalface_default.xml') + + def detect(self, array): + gray_array = cv2.cvtColor(array, cv2.COLOR_RGB2GRAY) + faces = self.classifier.detectMultiScale(gray_array, scaleFactor=1.1, minNeighbors=5, minSize=(30, 30)) + return faces + + def draw(self, array, faces): + for (x, y, w, h) in faces: + cv2.rectangle(array, (x, y), (x + w, y + h), (0, 255, 0), 2) + return array + + +async def run_face_detection(stream): + # setup pygame window + pygame.init() + screen_width, screen_height = 1280, 720 + screen = pygame.display.set_mode((screen_width, screen_height)) + pygame.display.set_caption("Face detection demo") + surface = pygame.Surface((screen_width, screen_height)) + + # get the driver camera video track from the stream + # generally its better to reuse the track object instead of getting it every time + track = stream.get_incoming_video_track("driver", buffered=False) + # cv2 face detector + detector = FaceDetector() + while stream.is_connected_and_ready and not pygame_should_quit(): + try: + # receive frame as pyAV VideoFrame, convert to rgb24 numpy array + frame = await track.recv() + array = frame.to_ndarray(format="rgb24") + + # detect faces and draw rects around them + resized_array = cv2.resize(array, (screen_width, screen_height)) + faces = detector.detect(resized_array) + detector.draw(resized_array, faces) + + # display the image + pygame.surfarray.blit_array(surface, resized_array.swapaxes(0, 1)) + screen.blit(surface, (0, 0)) + pygame.display.flip() + + print("Received frame from", "driver", frame.time) + except aiortc.mediastreams.MediaStreamError: + break + + pygame.quit() + await stream.stop() + + +async def run(args): + # build your own the offer stream + builder = WebRTCOfferBuilder(WebrtcdConnectionProvider(args.host)) + # request video stream from drivers camera + builder.offer_to_receive_video_stream("driver") + # add cereal messaging streaming support + builder.add_messaging() + + stream = builder.stream() + + # start the stream then wait for connection + # server will receive the offer and attempt to fulfill it + await stream.start() + await stream.wait_for_connection() + # all the tracks and channel are ready to be used at this point + + assert stream.has_incoming_video_track("driver") and stream.has_messaging_channel() + + # run face detection loop on the drivers camera + await run_face_detection(stream) + + +if __name__=='__main__': + parser = argparse.ArgumentParser() + parser.add_argument("--host", default="localhost", help="Host for webrtcd server") + + args = parser.parse_args() + asyncio.run(run(args)) diff --git a/teleoprtc_repo/examples/videostream_cli/README.md b/teleoprtc_repo/examples/videostream_cli/README.md new file mode 100644 index 0000000000..02b49b1085 --- /dev/null +++ b/teleoprtc_repo/examples/videostream_cli/README.md @@ -0,0 +1 @@ +# VideoStream CLI diff --git a/teleoprtc_repo/examples/videostream_cli/cli.py b/teleoprtc_repo/examples/videostream_cli/cli.py new file mode 100755 index 0000000000..9640edc197 --- /dev/null +++ b/teleoprtc_repo/examples/videostream_cli/cli.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python + +import argparse +import asyncio +import dataclasses +import json +import logging + +import aiortc +from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack + +from teleoprtc import WebRTCOfferBuilder, WebRTCAnswerBuilder +from teleoprtc.stream import StreamingOffer +from teleoprtc.info import parse_info_from_offer + + +async def async_input(): + return await asyncio.to_thread(input) + + +async def StdioConnectionProvider(offer: StreamingOffer) -> aiortc.RTCSessionDescription: + print("-- Please send this JSON to server --") + print(json.dumps(dataclasses.asdict(offer))) + print("-- Press enter when the answer is ready --") + raw_payload = await async_input() + payload = json.loads(raw_payload) + answer = aiortc.RTCSessionDescription(**payload) + + return answer + + +async def run_answer(args): + streams = [] + while True: + print("-- Please enter a JSON from client --") + raw_payload = await async_input() + + payload = json.loads(raw_payload) + offer = StreamingOffer(**payload) + info = parse_info_from_offer(offer.sdp) + assert len(offer.video) == info.n_expected_camera_tracks + video_tracks = [VideoStreamTrack() for _ in offer.video] + audio_tracks = [AudioStreamTrack()] if info.expected_audio_track else [] + + stream_builder = WebRTCAnswerBuilder(offer.sdp) + for cam, track in zip(offer.video, video_tracks, strict=True): + stream_builder.add_video_stream(cam, track) + for track in audio_tracks: + stream_builder.add_audio_stream(track) + stream = stream_builder.stream() + answer = await stream.start() + streams.append(stream) + + print("-- Please send this JSON to client --") + print(json.dumps({"sdp": answer.sdp, "type": answer.type})) + + await stream.wait_for_connection() + + +async def run_offer(args): + stream_builder = WebRTCOfferBuilder(StdioConnectionProvider) + for cam in args.cameras: + stream_builder.offer_to_receive_video_stream(cam) + if args.audio: + stream_builder.offer_to_receive_audio_stream() + if args.messaging: + stream_builder.add_messaging() + stream = stream_builder.stream() + _ = await stream.start() + await stream.wait_for_connection() + print("Connection established and all tracks are ready") + + video_tracks = [stream.get_incoming_video_track(cam, False) for cam in args.cameras] + audio_track = None + if stream.has_incoming_audio_track(): + audio_track = stream.get_incoming_audio_track(False) + while True: + try: + frames = await asyncio.gather(*[track.recv() for track in video_tracks]) + for key, frame in zip(args.cameras, frames, strict=True): + print("Received frame from", key, frame.time) + if audio_track: + frame = await audio_track.recv() + print("Received frame from audio", frame.time) + except aiortc.mediastreams.MediaStreamError: + return + print("=====================================") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + subparsers = parser.add_subparsers(dest="command", required=True) + + offer_parser = subparsers.add_parser("offer", description="Create offer stream") + offer_parser.add_argument("--audio", action="store_true", help="Offer to receive audio") + offer_parser.add_argument("--messaging", action="store_true", help="Add messaging support") + offer_parser.add_argument("cameras", metavar="CAMERA", type=str, nargs="+", default=[], help="Camera types to stream") + + answer_parser = subparsers.add_parser("answer", description="Create answer stream") + + args = parser.parse_args() + + logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()]) + logger = logging.getLogger("WebRTCStream") + logger.setLevel(logging.DEBUG) + + loop = asyncio.get_event_loop() + if args.command == "offer": + loop.run_until_complete(run_offer(args)) + elif args.command == "answer": + loop.run_until_complete(run_answer(args)) diff --git a/teleoprtc_repo/pyproject.toml b/teleoprtc_repo/pyproject.toml new file mode 100644 index 0000000000..ffdef0380d --- /dev/null +++ b/teleoprtc_repo/pyproject.toml @@ -0,0 +1,43 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" + +[project] +name = "teleoprtc" +version = "1.0.1" +authors = [{ name="Vehicle Researcher", email="user@comma.ai" }] +description = "Comma webRTC abstractions" +readme = "README.md" +license = { file="LICENSE" } +requires-python = ">=3.8" +classifiers = [ + "Programming Language :: Python :: 3", + "License :: OSI Approved :: MIT License", + "Operating System :: OS Independent", +] +dependencies = [ + "aiortc>=1.6.0", + "aiohttp>=3.7.0", + "av>=11.0.0,<13.0.0", + "numpy>=1.19.0", +] + +[project.optional-dependencies] +dev = [ + "parameterized>=0.8", + "pre-commit" +] + +[project.urls] +"Homepage" = "https://github.com/commaai/teleoprtc" +"Bug Tracker" = "https://github.com/commaai/teleoprtc/issues" + +# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml +[tool.ruff] +line-length = 160 +target-version="py38" + +[tool.ruff.lint] +select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF008", "RUF100", "A", "B", "TID251"] +ignore = ["W292", "E741", "E402", "C408", "ISC003", "B027", "B024"] +flake8-implicit-str-concat.allow-multiline=false diff --git a/teleoprtc_repo/scripts/bump_tag.sh b/teleoprtc_repo/scripts/bump_tag.sh new file mode 100755 index 0000000000..0f15c558e0 --- /dev/null +++ b/teleoprtc_repo/scripts/bump_tag.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash + +BRANCH="$(git branch --show-current)" +TOML_VERSION="$(python3 -c 'import tomllib; print(tomllib.load(open("pyproject.toml", "rb"))["project"]["version"])')" +LATEST_TAG_VERSION="$(git tag --list | sort -V -r | head -n 1)" +TAGGED_VERSION="" + +if [[ "$BRANCH" != "master" ]]; then + echo "Not on master branch." + exit 1 +fi + +if [[ "$TOML_VERSION" == "$LATEST_TAG_VERSION" ]]; then + TAGGED_VERSION=$(echo "$TOML_VERSION" | python3 -c "v = input().split('.'); v[-1]=str(int(v[-1])+1); print('.'.join(v))") + sed -i "s/version = \"$TOML_VERSION\"/version = \"$TAGGED_VERSION\"/" pyproject.toml +elif [[ -z "$LATEST_TAG_VERSION" ]] || printf "$LATEST_TAG_VERSION\n$TOML_VERSION" | sort -V -C; then + TAGGED_VERSION="$TOML_VERSION" +else + echo "Version in pyproject.toml is lower than the latest tag version." + exit 1 +fi + +echo "Tagging $TAGGED_VERSION..." +if [[ -n "$(git ls-files -m | grep pyproject.toml)" ]]; then + echo "Commiting pyproject.toml..." + git add pyproject.toml + git commit --no-verify -m "Bump version to $TAGGED_VERSION" +fi +git tag "$TAGGED_VERSION" diff --git a/teleoprtc_repo/scripts/publish_pypi.sh b/teleoprtc_repo/scripts/publish_pypi.sh new file mode 100755 index 0000000000..1cb0563ee0 --- /dev/null +++ b/teleoprtc_repo/scripts/publish_pypi.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash + +set -e + +if [[ -z "$1" ]]; then + echo "Usage: $0 " + exit 1 +fi +PYPI_TOKEN="$1" + +# install required packages +pip install --upgrade twine build + +# build the package +python3 -m build + +# upload to PyPI +REPOSITORY="" +if [[ -n "$TEST_UPLOAD" ]]; then + REPOSITORY="--repository testpypi" +fi + +python3 -m twine upload $REPOSITORY --username __token__ --password "$PYPI_TOKEN" dist/* diff --git a/teleoprtc/__init__.py b/teleoprtc_repo/teleoprtc/__init__.py similarity index 100% rename from teleoprtc/__init__.py rename to teleoprtc_repo/teleoprtc/__init__.py diff --git a/teleoprtc/builder.py b/teleoprtc_repo/teleoprtc/builder.py similarity index 98% rename from teleoprtc/builder.py rename to teleoprtc_repo/teleoprtc/builder.py index 9ec183e267..d054f9feda 100644 --- a/teleoprtc/builder.py +++ b/teleoprtc_repo/teleoprtc/builder.py @@ -1,5 +1,5 @@ import abc -from typing import List, Dict +from typing import Dict, List import aiortc diff --git a/teleoprtc/info.py b/teleoprtc_repo/teleoprtc/info.py similarity index 99% rename from teleoprtc/info.py rename to teleoprtc_repo/teleoprtc/info.py index b47ec3ff1f..fb786feda1 100644 --- a/teleoprtc/info.py +++ b/teleoprtc_repo/teleoprtc/info.py @@ -1,4 +1,3 @@ - import dataclasses import aiortc diff --git a/teleoprtc/stream.py b/teleoprtc_repo/teleoprtc/stream.py similarity index 97% rename from teleoprtc/stream.py rename to teleoprtc_repo/teleoprtc/stream.py index a928c14897..7b4172098e 100644 --- a/teleoprtc/stream.py +++ b/teleoprtc_repo/teleoprtc/stream.py @@ -2,7 +2,7 @@ import abc import asyncio import dataclasses import logging -from typing import Callable, Awaitable, Dict, List, Any, Optional +from typing import Any, Awaitable, Callable, Dict, List, Optional import aiortc from aiortc.contrib.media import MediaRelay @@ -199,7 +199,7 @@ class WebRTCBaseStream(abc.ABC): def is_connected_and_ready(self) -> bool: return self.peer_connection is not None and \ self.peer_connection.connectionState == "connected" and \ - self.expected_number_of_incoming_media != 0 and self.incoming_media_ready_event.is_set() + (self.expected_number_of_incoming_media == 0 or self.incoming_media_ready_event.is_set()) async def wait_for_connection(self): assert self.is_started @@ -212,7 +212,7 @@ class WebRTCBaseStream(abc.ABC): await self.messaging_channel_ready_event.wait() async def wait_for_disconnection(self): - assert self.is_connected_and_ready + assert self.is_connected_and_ready, "Stream is not connected/ready yet (make sure wait_for_connection was awaited)" await self.connection_stopped_event.wait() async def stop(self): diff --git a/teleoprtc/tracks.py b/teleoprtc_repo/teleoprtc/tracks.py similarity index 98% rename from teleoprtc/tracks.py rename to teleoprtc_repo/teleoprtc/tracks.py index 96d9c35249..2e583666a9 100644 --- a/teleoprtc/tracks.py +++ b/teleoprtc_repo/teleoprtc/tracks.py @@ -2,7 +2,7 @@ import asyncio import logging import time import fractions -from typing import Optional, Tuple, Any +from typing import Any, Optional, Tuple import aiortc from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE diff --git a/teleoprtc_repo/tests/test_info.py b/teleoprtc_repo/tests/test_info.py new file mode 100755 index 0000000000..893d8a426b --- /dev/null +++ b/teleoprtc_repo/tests/test_info.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +import unittest + +from teleoprtc.info import parse_info_from_offer + + +def lf2crlf(x): + return x.replace("\n", "\r\n") + + +class TestStream(unittest.TestCase): + def test_double_video_tracks(self): + sdp = """v=0 +o=- 3910210993 3910210993 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 1 +a=msid-semantic:WMS * +m=video 9 UDP/TLS/RTP/SAVPF 97 98 99 100 101 102 +c=IN IP4 0.0.0.0 +a=recvonly +a=extmap:1 urn:ietf:params:rtp-hdrext:sdes:mid +a=extmap:2 http://www.webrtc.org/experiments/rtp-hdrext/abs-send-time +a=mid:0 +a=msid:e123f852-010c-4b7b-8761-71b72fbfd013 2b75cb0e-6b34-48d6-8bf9-21b809f2e08e +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc-group:FID 1048118556 4149054509 +a=ssrc:1048118556 cname:61992fce-bab5-42a0-ab8c-7112adfb1857 +a=ssrc:4149054509 cname:61992fce-bab5-42a0-ab8c-7112adfb1857 +a=rtpmap:97 VP8/90000 +a=rtcp-fb:97 nack +a=rtcp-fb:97 nack pli +a=rtcp-fb:97 goog-remb +a=rtpmap:98 rtx/90000 +a=fmtp:98 apt=97 +a=rtpmap:99 H264/90000 +a=rtcp-fb:99 nack +a=rtcp-fb:99 nack pli +a=rtcp-fb:99 goog-remb +a=fmtp:99 level-asymmetry-allowed=1;packetization-mode=1;profile-level-id=42001f +a=rtpmap:100 rtx/90000 +a=fmtp:100 apt=99 +a=rtpmap:101 H264/90000 +a=rtcp-fb:101 nack +a=rtcp-fb:101 nack pli +a=rtcp-fb:101 goog-remb +a=fmtp:101 level-asymmetry-allowed=1;packetization-mode=1;profile-level-id=42e01f +a=rtpmap:102 rtx/90000 +a=fmtp:102 apt=101 +a=ice-ufrag:jxQW +a=ice-pwd:KpJ0tfaY2RxnIYpTHqPSSv +a=fingerprint:sha-256 70:3A:2D:37:3C:52:96:0E:10:F6:4D:7A:EB:18:38:1B:FD:CA:A5:90:D7:6C:DA:A9:39:76:C9:2F:FB:FF:56:0C +a=setup:actpass +m=video 9 UDP/TLS/RTP/SAVPF 97 98 99 100 101 102 +c=IN IP4 0.0.0.0 +a=recvonly +a=extmap:1 urn:ietf:params:rtp-hdrext:sdes:mid +a=extmap:2 http://www.webrtc.org/experiments/rtp-hdrext/abs-send-time +a=mid:1 +a=msid:e123f852-010c-4b7b-8761-71b72fbfd013 311db759-8d51-479c-a5b4-5c8d055c43ec +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc-group:FID 4096183284 2713379498 +a=ssrc:4096183284 cname:61992fce-bab5-42a0-ab8c-7112adfb1857 +a=ssrc:2713379498 cname:61992fce-bab5-42a0-ab8c-7112adfb1857 +a=rtpmap:97 VP8/90000 +a=rtcp-fb:97 nack +a=rtcp-fb:97 nack pli +a=rtcp-fb:97 goog-remb +a=rtpmap:98 rtx/90000 +a=fmtp:98 apt=97 +a=rtpmap:99 H264/90000 +a=rtcp-fb:99 nack +a=rtcp-fb:99 nack pli +a=rtcp-fb:99 goog-remb +a=fmtp:99 level-asymmetry-allowed=1;packetization-mode=1;profile-level-id=42001f +a=rtpmap:100 rtx/90000 +a=fmtp:100 apt=99 +a=rtpmap:101 H264/90000 +a=rtcp-fb:101 nack +a=rtcp-fb:101 nack pli +a=rtcp-fb:101 goog-remb +a=fmtp:101 level-asymmetry-allowed=1;packetization-mode=1;profile-level-id=42e01f +a=rtpmap:102 rtx/90000 +a=fmtp:102 apt=101 +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 70:3A:2D:37:3C:52:96:0E:10:F6:4D:7A:EB:18:38:1B:FD:CA:A5:90:D7:6C:DA:A9:39:76:C9:2F:FB:FF:56:0C +a=setup:actpass""" + info = parse_info_from_offer(lf2crlf(sdp)) + self.assertEqual(info.n_expected_camera_tracks, 2) + self.assertFalse(info.expected_audio_track) + self.assertFalse(info.incoming_audio_track) + self.assertFalse(info.incoming_datachannel) + + def test_recvonly_audio(self): + sdp = """v=0 +o=- 3910210904 3910210904 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 +a=msid-semantic:WMS * +m=audio 9 UDP/TLS/RTP/SAVPF 96 0 8 +c=IN IP4 0.0.0.0 +a=recvonly +a=extmap:1 urn:ietf:params:rtp-hdrext:sdes:mid +a=extmap:2 urn:ietf:params:rtp-hdrext:ssrc-audio-level +a=mid:0 +a=msid:eb1d3f1a-569a-465f-b419-319477bfded6 e44eecb2-1a04-4547-97d8-481389f50d5b +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc:1233332626 cname:ca4dede8-4994-4a6d-9ae3-923b28177ca5 +a=rtpmap:96 opus/48000/2 +a=rtpmap:0 PCMU/8000 +a=rtpmap:8 PCMA/8000 +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 40:4B:14:CF:70:B8:67:E1:B1:FF:7E:F9:22:6E:60:7D:73:B5:1E:38:4B:10:20:9C:CD:1C:47:02:52:ED:45:25 +a=setup:actpass""" + info = parse_info_from_offer(lf2crlf(sdp)) + self.assertEqual(info.n_expected_camera_tracks, 0) + self.assertTrue(info.expected_audio_track) + self.assertFalse(info.incoming_audio_track) + self.assertFalse(info.incoming_datachannel) + + def test_incoming_datachanel(self): + sdp = """v=0 +o=- 3910211092 3910211092 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 +a=msid-semantic:WMS * +m=application 9 DTLS/SCTP 5000 +c=IN IP4 0.0.0.0 +a=mid:0 +a=sctpmap:5000 webrtc-datachannel 65535 +a=max-message-size:65536 +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 9B:C0:F3:35:8E:05:A1:15:DB:F8:39:0E:B0:E0:0C:EB:82:E4:B9:26:18:A6:43:2D:B9:9A:23:96:0A:59:B6:58 +a=setup:actpass""" + info = parse_info_from_offer(lf2crlf(sdp)) + self.assertEqual(info.n_expected_camera_tracks, 0) + self.assertFalse(info.expected_audio_track) + self.assertFalse(info.incoming_audio_track) + self.assertTrue(info.incoming_datachannel) + + +if __name__ == '__main__': + unittest.main() diff --git a/teleoprtc_repo/tests/test_integration.py b/teleoprtc_repo/tests/test_integration.py new file mode 100755 index 0000000000..5377bcece9 --- /dev/null +++ b/teleoprtc_repo/tests/test_integration.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 + +import asyncio +import sys +import unittest + +from aiortc.mediastreams import AudioStreamTrack, VideoStreamTrack +from parameterized import parameterized + +from teleoprtc.builder import WebRTCOfferBuilder, WebRTCAnswerBuilder +from teleoprtc.stream import StreamingOffer +from teleoprtc.info import parse_info_from_offer + + +if sys.version_info >= (3, 11): + timeout = asyncio.timeout +else: + class Timeout: + def __init__(self, delay: float): + self._delay = delay + self._task = None + self._timeout_handle = None + + def _timeout(self): + if self._task: + self._task.cancel() + + async def __aenter__(self): + self._task = asyncio.current_task() + loop = asyncio.events.get_running_loop() + self._timeout_handle = loop.call_later(self._delay, self._timeout) + return self + + async def __aexit__(self, exc_type, exc, tb): + if self._timeout_handle: + self._timeout_handle.cancel() + if exc_type is asyncio.CancelledError and self._task and self._task.cancelled(): + raise asyncio.TimeoutError from exc + return False + + def timeout(delay): + return Timeout(delay) + + +class SimpleAnswerProvider: + def __init__(self): + self.stream = None + + async def __call__(self, offer: StreamingOffer): + assert self.stream is None, "This may only be called once" + + info = parse_info_from_offer(offer.sdp) + + builder = WebRTCAnswerBuilder(offer.sdp) + for cam in offer.video: + builder.add_video_stream(cam, VideoStreamTrack()) + if info.expected_audio_track: + builder.add_audio_stream(AudioStreamTrack()) + if info.incoming_audio_track: + builder.offer_to_receive_audio_stream() + + self.stream = builder.stream() + answer = await self.stream.start() + + return answer + + +class TestStreamIntegration(unittest.IsolatedAsyncioTestCase): + @parameterized.expand([ + # name, recv_cameras, recv_audio, messaging + ("multi_camera", ["driver", "wideRoad", "road"], False, False), + ("camera_and_audio", ["driver"], True, False), + ("camera_and__messaging", ["driver"], False, True), + ("camera_and_audio_and_messaging", ["driver", "wideRoad", "road"], True, True), + ]) + async def test_multi_camera(self, name, cameras, recv_audio, add_messaging): + simple_answerer = SimpleAnswerProvider() + offer_builder = WebRTCOfferBuilder(simple_answerer) + for cam in cameras: + offer_builder.offer_to_receive_video_stream(cam) + if recv_audio: + offer_builder.offer_to_receive_audio_stream() + if add_messaging: + offer_builder.add_messaging() + stream = offer_builder.stream() + + _ = await stream.start() + self.assertTrue(stream.is_started) + + try: + async with timeout(2): + await stream.wait_for_connection() + except TimeoutError: + self.fail("Timed out waiting for connection") + self.assertTrue(stream.is_connected_and_ready) + + self.assertEqual(stream.has_messaging_channel(), add_messaging) + if stream.has_messaging_channel(): + channel = stream.get_messaging_channel() + self.assertIsNotNone(channel) + self.assertEqual(channel.readyState, "open") + + self.assertEqual(stream.has_incoming_audio_track(), recv_audio) + if stream.has_incoming_audio_track(): + track = stream.get_incoming_audio_track(False) + self.assertIsNotNone(track) + self.assertEqual(track.readyState, "live") + self.assertEqual(track.kind, "audio") + # test audio recv + try: + async with timeout(1): + await track.recv() + except TimeoutError: + self.fail("Timed out waiting for audio frame") + + for cam in cameras: + self.assertTrue(stream.has_incoming_video_track(cam)) + if stream.has_incoming_video_track(cam): + track = stream.get_incoming_video_track(cam, False) + self.assertIsNotNone(track) + self.assertEqual(track.readyState, "live") + self.assertEqual(track.kind, "video") + # test video recv + try: + async with timeout(1): + await stream.get_incoming_video_track(cam, False).recv() + except TimeoutError: + self.fail("Timed out waiting for video frame") + + await stream.stop() + await simple_answerer.stream.stop() + self.assertFalse(stream.is_started) + self.assertFalse(stream.is_connected_and_ready) + + +if __name__ == '__main__': + unittest.main() diff --git a/teleoprtc_repo/tests/test_stream.py b/teleoprtc_repo/tests/test_stream.py new file mode 100755 index 0000000000..7882eed9c9 --- /dev/null +++ b/teleoprtc_repo/tests/test_stream.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 + +import unittest + +import aiortc +from aiortc.mediastreams import AudioStreamTrack + +from teleoprtc.builder import WebRTCOfferBuilder, WebRTCAnswerBuilder +from teleoprtc.info import parse_info_from_offer +from teleoprtc.tracks import TiciVideoStreamTrack + + +class OfferCapture: + def __init__(self): + self.offer = None + + async def __call__(self, offer): + self.offer = offer + raise Exception("Offer captured") + + +class DummyH264VideoStreamTrack(TiciVideoStreamTrack): + kind = "video" + + async def recv(self): + raise NotImplementedError() + + def codec_preference(self): + return "H264" + + +class TestOfferStream(unittest.IsolatedAsyncioTestCase): + async def test_offer_stream_sdp_recvonly_audio(self): + capture = OfferCapture() + builder = WebRTCOfferBuilder(capture) + builder.offer_to_receive_audio_stream() + stream = builder.stream() + + try: + _ = await stream.start() + except Exception: + pass + + info = parse_info_from_offer(capture.offer.sdp) + self.assertTrue(info.expected_audio_track) + self.assertFalse(info.incoming_audio_track) + + async def test_offer_stream_sdp_sendonly_audio(self): + capture = OfferCapture() + builder = WebRTCOfferBuilder(capture) + builder.add_audio_stream(AudioStreamTrack()) + stream = builder.stream() + + try: + _ = await stream.start() + except Exception: + pass + + info = parse_info_from_offer(capture.offer.sdp) + self.assertFalse(info.expected_audio_track) + self.assertTrue(info.incoming_audio_track) + + async def test_offer_stream_sdp_channel(self): + capture = OfferCapture() + builder = WebRTCOfferBuilder(capture) + builder.add_messaging() + stream = builder.stream() + + try: + _ = await stream.start() + except Exception: + pass + + info = parse_info_from_offer(capture.offer.sdp) + self.assertTrue(info.incoming_datachannel) + + +class TestAnswerStream(unittest.IsolatedAsyncioTestCase): + async def test_codec_preference(self): + offer_sdp = """v=0 +o=- 3910274679 3910274679 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 +a=msid-semantic:WMS * +m=video 1337 UDP/TLS/RTP/SAVPF 97 98 99 100 101 102 +c=IN IP4 0.0.0.0 +a=recvonly +a=mid:0 +a=msid:34803878-98f8-4245-b45c-f773e5f926df 881dbc20-356a-499c-b4e8-695303bb901d +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc-group:FID 1303546896 3784011659 +a=ssrc:1303546896 cname:a59185ac-c115-48d3-b39b-db7d615a6966 +a=ssrc:3784011659 cname:a59185ac-c115-48d3-b39b-db7d615a6966 +a=rtpmap:97 VP8/90000 +a=rtcp-fb:97 nack +a=rtcp-fb:97 nack pli +a=rtcp-fb:97 goog-remb +a=rtpmap:99 H264/90000 +a=rtcp-fb:99 nack +a=rtcp-fb:99 nack pli +a=rtcp-fb:99 goog-remb +a=fmtp:99 level-asymmetry-allowed=1;packetization-mode=1;profile-level-id=42001f +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 15:F3:F0:23:67:44:EE:2C:AA:8C:D9:50:95:26:42:7C:67:EA:1F:D2:92:C5:97:01:7B:2E:57:C9:A3:13:00:4A +a=setup:actpass""" + + builder = WebRTCAnswerBuilder(offer_sdp) + builder.add_video_stream("road", DummyH264VideoStreamTrack("road", 0.05)) + stream = builder.stream() + answer = await stream.start() + + sdp_desc = aiortc.sdp.SessionDescription.parse(answer.sdp) + video_desc = [m for m in sdp_desc.media if m.kind == "video"][0] + codecs = video_desc.rtp.codecs + self.assertEqual(codecs[0].mimeType, "video/H264") + + async def test_fail_if_preferred_codec_not_in_offer(self): + offer_sdp = """v=0 +o=- 3910274679 3910274679 IN IP4 0.0.0.0 +s=- +t=0 0 +a=group:BUNDLE 0 +a=msid-semantic:WMS * +m=video 1337 UDP/TLS/RTP/SAVPF 97 98 99 100 101 102 +c=IN IP4 0.0.0.0 +a=recvonly +a=mid:0 +a=msid:34803878-98f8-4245-b45c-f773e5f926df 881dbc20-356a-499c-b4e8-695303bb901d +a=rtcp:9 IN IP4 0.0.0.0 +a=rtcp-mux +a=ssrc-group:FID 1303546896 3784011659 +a=ssrc:1303546896 cname:a59185ac-c115-48d3-b39b-db7d615a6966 +a=ssrc:3784011659 cname:a59185ac-c115-48d3-b39b-db7d615a6966 +a=rtpmap:97 VP8/90000 +a=rtcp-fb:97 nack +a=rtcp-fb:97 nack pli +a=rtcp-fb:97 goog-remb +a=ice-ufrag:1234 +a=ice-pwd:1234 +a=fingerprint:sha-256 15:F3:F0:23:67:44:EE:2C:AA:8C:D9:50:95:26:42:7C:67:EA:1F:D2:92:C5:97:01:7B:2E:57:C9:A3:13:00:4A +a=setup:actpass""" + + builder = WebRTCAnswerBuilder(offer_sdp) + builder.add_video_stream("road", DummyH264VideoStreamTrack("road", 0.05)) + stream = builder.stream() + + with self.assertRaises(ValueError): + _ = await stream.start() + + +if __name__=="__main__": + unittest.main() \ No newline at end of file diff --git a/teleoprtc_repo/tests/test_track.py b/teleoprtc_repo/tests/test_track.py new file mode 100755 index 0000000000..3c8fcbb274 --- /dev/null +++ b/teleoprtc_repo/tests/test_track.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 + +import unittest + +import aiortc + +from teleoprtc.tracks import video_track_id, parse_video_track_id, TiciVideoStreamTrack, TiciTrackWrapper + + +class TestTracks(unittest.TestCase): + def test_track_id(self): + expected_camera_type, expected_track_id = "driver", "test" + track_id = video_track_id(expected_camera_type, expected_track_id) + camera_type, track_id = parse_video_track_id(track_id) + self.assertEqual(expected_camera_type, camera_type) + self.assertEqual(expected_track_id, track_id) + + def test_track_id_invalid(self): + with self.assertRaises(ValueError): + parse_video_track_id("test") + + def test_tici_track_id(self): + class VideoStream(TiciVideoStreamTrack): + async def recv(self): + raise NotImplementedError() + + track = VideoStream("driver", 0.1) + camera_type, _ = parse_video_track_id(track.id) + self.assertEqual("driver", camera_type) + + def test_tici_wrapper_id(self): + track = TiciTrackWrapper("driver", aiortc.mediastreams.VideoStreamTrack()) + camera_type, _ = parse_video_track_id(track.id) + self.assertEqual("driver", camera_type) + + +if __name__ == '__main__': + unittest.main() diff --git a/third_party/acados/aarch64 b/third_party/acados/aarch64 new file mode 120000 index 0000000000..062c65e8d9 --- /dev/null +++ b/third_party/acados/aarch64 @@ -0,0 +1 @@ +larch64/ \ No newline at end of file diff --git a/third_party/acados/acados_template/.gitignore b/third_party/acados/acados_template/.gitignore new file mode 100644 index 0000000000..63b741bb9d --- /dev/null +++ b/third_party/acados/acados_template/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ + +# Cython intermediates +*_pyx.c +*_pyx.o +*_pyx.so diff --git a/third_party/acados/acados_template/gnsf/__init__.py b/third_party/acados/acados_template/gnsf/__init__.py index 8b13789179..e69de29bb2 100644 --- a/third_party/acados/acados_template/gnsf/__init__.py +++ b/third_party/acados/acados_template/gnsf/__init__.py @@ -1 +0,0 @@ - diff --git a/third_party/acados/acados_template/gnsf/matlab to python.md b/third_party/acados/acados_template/gnsf/matlab to python.md deleted file mode 100644 index 53a0ed971e..0000000000 --- a/third_party/acados/acados_template/gnsf/matlab to python.md +++ /dev/null @@ -1,43 +0,0 @@ -# matlab to python - -% -> # - -; -> - -from casadi import * --> -from casadi import * - - -print\('(.*)'\) -print('$1') - -print\(\['(.*)'\]\) -print(f'$1') - -keyboard -import pdb; pdb.set_trace() - - -range((([^))]*)) -range($1) - -\s*end --> -nothing - - -if (.*) -if $1: - -else -else: - -num2str -str - -for ([a-z_]*) = -for $1 in - -length\( -len( \ No newline at end of file diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh new file mode 100755 index 0000000000..b45c167b16 --- /dev/null +++ b/third_party/acados/build.sh @@ -0,0 +1,56 @@ +#!/usr/bin/env bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + +ARCHNAME="x86_64" +BLAS_TARGET="X64_AUTOMATIC" +if [ -f /TICI ]; then + ARCHNAME="larch64" + BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" +fi + +ACADOS_FLAGS="-DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET" + +if [[ "$OSTYPE" == "darwin"* ]]; then + ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64 -DCMAKE_MACOSX_RPATH=1" + ARCHNAME="Darwin" +fi + +if [ ! -d acados_repo/ ]; then + git clone https://github.com/acados/acados.git $DIR/acados_repo + # git clone https://github.com/commaai/acados.git $DIR/acados_repo +fi +cd acados_repo +git fetch --all +git checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2 +git submodule update --depth=1 --recursive --init + +# build +mkdir -p build +cd build +cmake $ACADOS_FLAGS .. +make -j20 install + +INSTALL_DIR="$DIR/$ARCHNAME" +rm -rf $INSTALL_DIR +mkdir -p $INSTALL_DIR + +rm $DIR/acados_repo/lib/*.json + +rm -rf $DIR/include $DIR/acados_template +cp -r $DIR/acados_repo/include $DIR +cp -r $DIR/acados_repo/lib $INSTALL_DIR +cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/ +#pip3 install -e $DIR/acados/interfaces/acados_template + +# build tera +cd $DIR/acados_repo/interfaces/acados_template/tera_renderer/ +if [[ "$OSTYPE" == "darwin"* ]]; then + cargo build --verbose --release --target aarch64-apple-darwin + cargo build --verbose --release --target x86_64-apple-darwin + lipo -create -output target/release/t_renderer target/x86_64-apple-darwin/release/t_renderer target/aarch64-apple-darwin/release/t_renderer +else + cargo build --verbose --release +fi +cp target/release/t_renderer $INSTALL_DIR/ diff --git a/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore new file mode 100644 index 0000000000..ac06c0cf85 --- /dev/null +++ b/third_party/bootstrap/.gitignore @@ -0,0 +1 @@ +/icons/ diff --git a/third_party/libyuv/.gitignore b/third_party/libyuv/.gitignore new file mode 100644 index 0000000000..450712e47d --- /dev/null +++ b/third_party/libyuv/.gitignore @@ -0,0 +1 @@ +libyuv/ diff --git a/third_party/libyuv/aarch64 b/third_party/libyuv/aarch64 new file mode 120000 index 0000000000..062c65e8d9 --- /dev/null +++ b/third_party/libyuv/aarch64 @@ -0,0 +1 @@ +larch64/ \ No newline at end of file diff --git a/third_party/libyuv/build.sh b/third_party/libyuv/build.sh new file mode 100755 index 0000000000..b960f60ef5 --- /dev/null +++ b/third_party/libyuv/build.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + +ARCHNAME=$(uname -m) +if [ -f /TICI ]; then + ARCHNAME="larch64" +fi + +if [[ "$OSTYPE" == "darwin"* ]]; then + ARCHNAME="Darwin" +fi + +cd $DIR +if [ ! -d libyuv ]; then + git clone --single-branch https://chromium.googlesource.com/libyuv/libyuv +fi + +cd libyuv +git checkout 4a14cb2e81235ecd656e799aecaaf139db8ce4a2 + +# build +cmake . +make -j$(nproc) + +INSTALL_DIR="$DIR/$ARCHNAME" +rm -rf $INSTALL_DIR +mkdir -p $INSTALL_DIR + +rm -rf $DIR/include +mkdir -p $INSTALL_DIR/lib +cp $DIR/libyuv/libyuv.a $INSTALL_DIR/lib +cp -r $DIR/libyuv/include $DIR + +## To create universal binary on Darwin: +## ``` +## lipo -create -output Darwin/libyuv.a path-to-x64/libyuv.a path-to-arm64/libyuv.a +## ``` diff --git a/third_party/maplibre-native-qt/.gitignore b/third_party/maplibre-native-qt/.gitignore new file mode 100644 index 0000000000..9adc6681c0 --- /dev/null +++ b/third_party/maplibre-native-qt/.gitignore @@ -0,0 +1 @@ +/maplibre/ diff --git a/third_party/maplibre-native-qt/build.sh b/third_party/maplibre-native-qt/build.sh index c64f0fc649..a368026f0f 100755 --- a/third_party/maplibre-native-qt/build.sh +++ b/third_party/maplibre-native-qt/build.sh @@ -3,35 +3,33 @@ set -e DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" -ARCHNAME="x86_64" +ARCHNAME=$(uname -m) MAPLIBRE_FLAGS="-DMLN_QT_WITH_LOCATION=OFF" -if [ -f /AGNOS ]; then +if [ -f /TICI ]; then ARCHNAME="larch64" #MAPLIBRE_FLAGS="$MAPLIBRE_FLAGS -DCMAKE_SYSTEM_NAME=Android -DANDROID_ABI=arm64-v8a" fi cd $DIR if [ ! -d maplibre ]; then - git clone git@github.com:maplibre/maplibre-native-qt.git $DIR/maplibre + git clone --single-branch https://github.com/maplibre/maplibre-native-qt.git $DIR/maplibre fi cd maplibre -git fetch --all git checkout 3726266e127c1f94ad64837c9dbe03d238255816 git submodule update --depth=1 --recursive --init # build mkdir -p build cd build -set -x cmake $MAPLIBRE_FLAGS $DIR/maplibre -make -j$(nproc) || make -j2 || make -j1 +make -j$(nproc) INSTALL_DIR="$DIR/$ARCHNAME" rm -rf $INSTALL_DIR mkdir -p $INSTALL_DIR -rm -rf $INSTALL_DIR/lib $DIR/include +rm -rf $DIR/include mkdir -p $INSTALL_DIR/lib $INSTALL_DIR/include $DIR/include cp -r $DIR/maplibre/build/src/core/*.so* $INSTALL_DIR/lib cp -r $DIR/maplibre/build/src/core/include/* $INSTALL_DIR/include diff --git a/third_party/snpe/x86_64 b/third_party/snpe/x86_64 new file mode 120000 index 0000000000..700025efab --- /dev/null +++ b/third_party/snpe/x86_64 @@ -0,0 +1 @@ +x86_64-linux-clang \ No newline at end of file diff --git a/third_party/snpe/x86_64/libSNPE.so b/third_party/snpe/x86_64/libSNPE.so deleted file mode 100644 index 0cee9ac3af..0000000000 Binary files a/third_party/snpe/x86_64/libSNPE.so and /dev/null differ diff --git a/third_party/snpe/x86_64/libomp.so b/third_party/snpe/x86_64/libomp.so deleted file mode 100755 index 567e6ae594..0000000000 Binary files a/third_party/snpe/x86_64/libomp.so and /dev/null differ diff --git a/tinygrad b/tinygrad new file mode 120000 index 0000000000..cb003823c6 --- /dev/null +++ b/tinygrad @@ -0,0 +1 @@ +tinygrad_repo/tinygrad \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/renderer/cuda.py b/tinygrad_repo/tinygrad/renderer/cuda.py new file mode 100644 index 0000000000..9e6d0d6007 --- /dev/null +++ b/tinygrad_repo/tinygrad/renderer/cuda.py @@ -0,0 +1,25 @@ +import functools +from tinygrad.renderer.cstyle import uops_to_cstyle, CStyleLanguage + +class CUDALanguage(CStyleLanguage): + kernel_prefix = "__global__ " + smem_prefix = "__shared__ " + smem_prefix_for_cast = False + arg_int_prefix = "const int" + barrier = "__syncthreads();" + float4 = "make_float4" + gid = [f'blockIdx.{chr(120+i)}' for i in range(3)] + lid = [f'threadIdx.{chr(120+i)}' for i in range(3)] + xid = [f'(blockIdx.{chr(120+i)}*blockDim.{chr(120+i)}+threadIdx.{chr(120+i)})' for i in range(3)] + half_prekernel = """ + #include + #include + using namespace nvcuda; + struct __align__(8) half4 { + half2 x, y; + __device__ __forceinline__ explicit half4(const float4& a): x(make_half2(__float2half(a.x), __float2half(a.y))), y(make_half2(__float2half(a.z),__float2half(a.w))) {} + __device__ __forceinline__ explicit operator float4() const {return make_float4(__half2float(x.x), __half2float(x.y), __half2float(y.x), __half2float(y.y)); } + }; + """ # if not getenv("PTX") else fromimport("tinygrad.renderer.assembly_ptx", "uops_to_ptx_asm") # assembly_ptx currently isn't supported + +CUDARenderer = functools.partial(uops_to_cstyle, CUDALanguage()) \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/renderer/llvmir.py b/tinygrad_repo/tinygrad/renderer/llvmir.py new file mode 100644 index 0000000000..f0fc1e2339 --- /dev/null +++ b/tinygrad_repo/tinygrad/renderer/llvmir.py @@ -0,0 +1,147 @@ +from typing import Final, Dict, Callable, Any, List, Optional, Tuple +from llvmlite import ir # type: ignore +from tinygrad.codegen.linearizer import UOps, UOp +from tinygrad.helpers import dtypes +from tinygrad.ops import Op, UnaryOps, BinaryOps, TernaryOps + +LLVM_FAST_MATH_FLAGS = ('nsz', 'arcp', 'contract', 'afn', 'reassoc') # All from fast math, but nnan and ninf + +code_for_op: Final[Dict[Op, Callable]] = { + UnaryOps.NEG: lambda builder,x: builder.neg(x) if isinstance(x.type, ir.IntType) else builder.fneg(x, flags=LLVM_FAST_MATH_FLAGS), + UnaryOps.EXP2: lambda builder,x: builder.call(builder._block.module.declare_intrinsic('llvm.exp2', [ir.FloatType()]), [x], fastmath=LLVM_FAST_MATH_FLAGS), + UnaryOps.LOG2: lambda builder,x: builder.call(builder._block.module.declare_intrinsic('llvm.log2', [ir.FloatType()]), [x], fastmath=LLVM_FAST_MATH_FLAGS), + UnaryOps.SIN: lambda builder,x: builder.call(builder._block.module.declare_intrinsic('llvm.sin', [ir.FloatType()]), [x], fastmath=LLVM_FAST_MATH_FLAGS), + UnaryOps.SQRT: lambda builder,x: builder.call(builder._block.module.declare_intrinsic('llvm.sqrt', [ir.FloatType()]), [x], fastmath=LLVM_FAST_MATH_FLAGS), + BinaryOps.ADD: lambda builder,x,y: builder.add(x,y) if isinstance(x.type, ir.IntType) else builder.fadd(x,y, flags=LLVM_FAST_MATH_FLAGS), + BinaryOps.SUB: lambda builder,x,y: builder.sub(x,y) if isinstance(x.type, ir.IntType) else builder.fsub(x,y, flags=LLVM_FAST_MATH_FLAGS), + BinaryOps.MUL: lambda builder,x,y: builder.mul(x,y) if isinstance(x.type, ir.IntType) else builder.fmul(x,y, flags=LLVM_FAST_MATH_FLAGS), + BinaryOps.DIV: lambda builder,x,y: builder.sdiv(x,y) if isinstance(x.type, ir.IntType) else builder.fdiv(x,y, flags=LLVM_FAST_MATH_FLAGS), + # TODO: this should be casted + BinaryOps.CMPLT: lambda builder,x,y: builder.zext(builder.icmp_signed("<", x, y),ir.IntType(32)) if isinstance(x.type, ir.IntType) else builder.uitofp(builder.fcmp_ordered("<", x, y, flags=LLVM_FAST_MATH_FLAGS), ir.FloatType()), + BinaryOps.MAX: lambda builder,x,y: builder.select(builder.fcmp_unordered(">", x, y, flags=LLVM_FAST_MATH_FLAGS), x, y, flags=LLVM_FAST_MATH_FLAGS), + BinaryOps.MOD: lambda builder,x,y: builder.srem(x,y), + TernaryOps.MULACC: lambda builder,x,y,z: builder.fadd(builder.fmul(x,y, flags=LLVM_FAST_MATH_FLAGS), z, flags=LLVM_FAST_MATH_FLAGS), + TernaryOps.WHERE: lambda builder,x,y,z: builder.select(builder.fcmp_unordered("!=", x, ir.Constant(ir.FloatType(), 0), flags=LLVM_FAST_MATH_FLAGS) if isinstance(x.type, ir.FloatType) else builder.trunc(x, ir.IntType(1)), y, z, flags=LLVM_FAST_MATH_FLAGS), +} + +dtype_to_llvm_dtype = {dtypes.float64:ir.DoubleType(), dtypes.float16:ir.HalfType(), dtypes.bfloat16:ir.IntType(16), dtypes.float32:ir.FloatType(), dtypes.int8:ir.IntType(8), dtypes.uint8:ir.IntType(8), dtypes.bool: ir.IntType(1), dtypes.int64: ir.IntType(64), dtypes.int32: ir.IntType(32), dtypes._arg_int32: ir.IntType(32), dtypes.int16:ir.IntType(16), dtypes.uint16:ir.IntType(16), dtypes.uint32:ir.IntType(32), dtypes.uint64:ir.IntType(64)} + +def cast(bb, val, input_type, output_type): + if input_type == output_type: return val + + if output_type == dtypes.float32: + if dtypes.is_int(input_type) or input_type == dtypes.bool: + val = bb[-1].uitofp(val, ir.FloatType()) if dtypes.is_unsigned(input_type) or input_type == dtypes.bool else bb[-1].sitofp(val, ir.FloatType()) + elif input_type == dtypes.bfloat16: + val = bb[-1].sext(val, ir.IntType(32)) + val = bb[-1].shl(val, ir.Constant(ir.IntType(32), 16)) + val = bb[-1].bitcast(val, ir.FloatType()) + elif input_type == dtypes.float64: + val = bb[-1].fptrunc(val, ir.FloatType()) + else: + val = bb[-1].fpext(val, ir.FloatType()) + return val + + if input_type == dtypes.float32: + if dtypes.is_int(output_type) or output_type == dtypes.bool: + if dtypes.is_unsigned(output_type): val = bb[-1].fptoui(val, dtype_to_llvm_dtype[output_type]) + elif output_type == dtypes.bool: val = bb[-1].fcmp_ordered("!=", val, ir.Constant(ir.FloatType(), 0), flags=LLVM_FAST_MATH_FLAGS) + else: val = bb[-1].fptosi(val, dtype_to_llvm_dtype[output_type]) + elif output_type == dtypes.bfloat16: + val = bb[-1].bitcast(val, ir.IntType(32)) + val = bb[-1].lshr(val, ir.Constant(ir.IntType(32), 16)) + val = bb[-1].trunc(val, ir.IntType(16)) + elif output_type == dtypes.float64: + val = bb[-1].fpext(val, ir.DoubleType()) + else: + val = bb[-1].fptrunc(val, dtype_to_llvm_dtype[output_type]) + return val + + raise NotImplementedError(f"cast from {input_type} -> {output_type} not implemented") + +def uops_to_llvm_ir(function_name:str, uops:List[UOp]) -> Tuple[str, Dict]: + # all llvm stuff goes into a module + module = ir.Module(name=__file__) + + # extract global buffers + buf_to_dtype = {args[0]:args[1] for uop,_,_,args,_ in uops if uop == UOps.DEFINE_GLOBAL} + buf_index = {x:i for i,x in enumerate(buf_to_dtype.keys())} + + # create llvm function + func_dtypes = [(dtype_to_llvm_dtype[dtype],dtype) for dtype in buf_to_dtype.values()] + func = ir.Function(module, ir.FunctionType(ir.VoidType(), [x.as_pointer() if dt!=dtypes._arg_int32 else x for x,dt in func_dtypes]), name=function_name) + for a in func.args: + if a.type.is_pointer: a.add_attribute("noalias") + + # add the function attribute "no-nans-fp-math"="true", which informs llvm that it allowed to use vectorization optimizations + func.attributes._known = func.attributes._known.union(frozenset(['"no-nans-fp-math"="true"'])) + func.attributes.add('"no-nans-fp-math"="true"') + + bb = [ir.IRBuilder(func.append_basic_block("entry"))] + loop_blocks: List = [] + reduce_phis: List = [] + # TODO: newvar probably shouldn't be optional + lvars: Dict[Optional[UOp], Any] = {} # this Any is an llvm type + + for bufname,dtype in buf_to_dtype.items(): + if dtype == dtypes._arg_int32: lvars[bufname] = bb[-1].sext(func.args[buf_index[bufname]], ir.IntType(32)) + + for u in uops: + uop,dtype,vin,args,_ = u + if uop == UOps.LOOP: + bb.append(ir.IRBuilder(func.append_basic_block(f"loop_body_{len(loop_blocks)}"))) + bb[-2].branch(bb[-1]._block) + + phis = [] + for rp in reduce_phis: + incoming = lvars[rp] + lvars[rp] = bb[-1].phi(ir.FloatType()) + lvars[rp].add_incoming(incoming, bb[-2]._block) + phis.append((rp, lvars[rp])) + + lvars[u] = bb[-1].phi(ir.IntType(32), name=f"loop{len(loop_blocks)}") + lvars[u].add_incoming(lvars[vin[0]], bb[-2]._block) + loop_blocks.append((bb[-1], phis)) + if uop == UOps.END: + block, phis = loop_blocks.pop() + idx_p1 = bb[-1].add(lvars[vin[0]], ir.Constant(ir.IntType(32), 1)) + lvars[vin[0]].add_incoming(idx_p1, bb[-1]._block) + for n,phi in phis: phi.add_incoming(lvars[n], bb[-1]._block) + bb.append(ir.IRBuilder(func.append_basic_block(f"loop_exit_{len(loop_blocks)}"))) + bb[-2].cbranch(bb[-2].icmp_unsigned("<", idx_p1, lvars[vin[0].vin[1]]), block._block, bb[-1]._block) + if uop == UOps.DEFINE_GLOBAL: + lvars[u] = func.args[buf_index[args[0]]] + if uop == UOps.DEFINE_ACC: + lvars[u] = ir.Constant(dtype_to_llvm_dtype[dtype], args) + reduce_phis.append(u) + if uop == UOps.SPECIAL: + lvars[u] = lvars[args.expr] + if uop == UOps.CONST: + value = int(args) if dtypes.is_int(dtype) else bool(args) if dtype == dtypes.bool else args + lvars[u] = ir.Constant(dtype_to_llvm_dtype[dtype], value) + if uop == UOps.LOAD: + assert dtype is not None + if len(vin) > 2: + gate = bb[-1].trunc(lvars[vin[2]], ir.IntType(1)) + aug_idx = bb[-1].select(gate, lvars[vin[1]], ir.Constant(ir.IntType(32), 0)) + val = bb[-1].load(bb[-1].gep(lvars[vin[0]], [aug_idx], inbounds=True)) + val = cast(bb, val, vin[0].dtype, dtype) + val = bb[-1].select(gate, val, lvars[vin[3]]) + else: + val = bb[-1].load(bb[-1].gep(lvars[vin[0]], [lvars[vin[1]]], inbounds=True)) + val = cast(bb, val, vin[0].dtype, dtype) + lvars[u] = val + if uop == UOps.PHI: + lvars[u] = lvars[vin[1]] + # PHI UOps can link to other PHI Uops, backtrace this to DEFINE_ACC + backward = vin[0] + while backward.uop == UOps.PHI: backward = backward.vin[0] + lvars[backward] = lvars[u] + if uop == UOps.STORE: + element = cast(bb, lvars[vin[2]], vin[2].dtype, vin[0].dtype) + bb[-1].store(element, bb[-1].gep(lvars[vin[0]], [lvars[vin[1]]], inbounds=True)) + if uop == UOps.ALU: + lvars[u] = code_for_op[args](bb[-1], *[lvars[x] for x in vin]) + + bb[-1].ret_void() + return str(module), {} diff --git a/tinygrad_repo/tinygrad/renderer/metal.py b/tinygrad_repo/tinygrad/renderer/metal.py new file mode 100644 index 0000000000..5b033a408b --- /dev/null +++ b/tinygrad_repo/tinygrad/renderer/metal.py @@ -0,0 +1,16 @@ +import functools +from tinygrad.renderer.cstyle import uops_to_cstyle, CStyleLanguage + +class MetalLanguage(CStyleLanguage): + kernel_prefix = "#include \nusing namespace metal;\nkernel " + buffer_prefix = "device " + smem_prefix = "threadgroup " + arg_int_prefix = "constant int&" + barrier = "threadgroup_barrier(mem_flags::mem_threadgroup);" + float4 = "float4" + uses_ptr_arithmetic=True + gid = [f"gid.{chr(120+i)}" for i in range(3)] + lid = [f"lid.{chr(120+i)}" for i in range(3)] + extra_args = ['uint3 gid [[threadgroup_position_in_grid]]', 'uint3 lid [[thread_position_in_threadgroup]]'] + +MetalRenderer = functools.partial(uops_to_cstyle, MetalLanguage()) diff --git a/tinygrad_repo/tinygrad/renderer/triton.py b/tinygrad_repo/tinygrad/renderer/triton.py new file mode 100644 index 0000000000..944626b6b9 --- /dev/null +++ b/tinygrad_repo/tinygrad/renderer/triton.py @@ -0,0 +1,130 @@ +from typing import Dict, List, Final, Callable, DefaultDict +from collections import defaultdict +from tinygrad.ops import UnaryOps, BinaryOps, TernaryOps, Op +from tinygrad.helpers import DType, dtypes, ImageDType, DEBUG, getenv +from tinygrad.codegen.linearizer import UOp, UOps +from triton.compiler import compile as triton_compile # type: ignore +import linecache +import math +import re + +triton_dtypes = {dtypes.double: "tl.float64", dtypes.float32: "tl.float32", dtypes.float16: "tl.float16", dtypes.bool: "tl.int1", dtypes.int8: "tl.int8", dtypes.uint8: "tl.uint8", dtypes.int32: "tl.int32", dtypes.int64: "tl.int64", dtypes.uint32: "tl.uint32", dtypes.uint64: "tl.uint64", dtypes.int16: "tl.int16", dtypes.uint16: "tl.uint16"} +signature_dtypes = {dtypes.double: "*fp64",dtypes.float32: "*fp32", dtypes.float16: "*fp16", dtypes.bool: "*i8", dtypes.int8: "*i1", dtypes.uint8: "*u8", dtypes._arg_int32: "i32", dtypes.int32: "*i32", dtypes.int64: "*i64", dtypes.uint32: "*u32", dtypes.uint64: "*u64", dtypes.int16: "*i16", dtypes.uint16: "*u16"} + +def next_power_of_2(x): + return 1 << (x - 1).bit_length() + +def render_valid(valid): + return '(' * (len(valid) -1) + ') and '.join(valid) if len(valid) else 'True' + +#NOTE Triton requires matching dimensions for load/store, disable this and see TestOps::test_output_padded_conv_transpose2d fail to compile +def fill_dims_for_idx(idx, dims): + return "(" + idx + "+ (" + (f"0*({'+'.join(d for d in dims)})))") if len(dims) else idx + +def get_max(var): + if isinstance(var, int): return var + return re.sub(r'\[(.*?)\]', '', str(var))[1:-1] + +#NOTE can be removed after https://github.com/gpuocelot/gpuocelot/issues/8 gets resolved +def remove_single_scalar_curly_braces(ptx_code): + return '\n'.join([re.sub(r'\{\s*(%\w+)\s*\}', r'\1', line) for line in ptx_code.split('\n')]) + +def render_const(args): + return (('-' if args<0 else '') + 'tl.where(1,float("inf"),0)') if math.isinf(args) else ('tl.where(1,float("nan"),0)' if math.isnan(args) else str(args)) + +def render_cast(x:str, dtype:DType): + return f"{x}.to({triton_dtypes[dtype]})" + +def define_scalar(local_size, dtype, args): + if len(local_size) > 0: return f"tl.full(({','.join([str(next_power_of_2(x)) for x in local_size])},),{render_const(args)}, dtype={triton_dtypes[dtype]})" + return render_const(args) + +def uops_to_triton(function_name:str, uops:List[UOp]): + local_size: List[int] = [] + depth = 1 + signatures, dims, bufs, kernel, valid = [], [], [], [], [] #type: ignore + + c: DefaultDict[str, int] = defaultdict(int) + r: Dict[UOp, str] = {} + def ssa(u, prefix="t"): + nonlocal c, r + c[prefix] += 1 + r[u]=f"{prefix}{c[prefix]-1}" + return r[u] + + child_count: DefaultDict[UOp, int] = defaultdict(int) + for ru in uops: + for v in ru.vin: + child_count[v] += 1 + + def kk(s): kernel.append(" "*depth+s) + code_for_op: Final[Dict[Op, Callable]] = { + UnaryOps.EXP2: lambda x,: f"tl.math.exp2({x})", + UnaryOps.LOG2: lambda x,: f"tl.math.log2({x})", + UnaryOps.SIN: lambda x,: f"tl.sin({x})", + UnaryOps.SQRT: lambda x,: f"tl.sqrt({x})", + UnaryOps.NEG: lambda x,: f"-{x}", + BinaryOps.ADD: lambda x,y,: f"({x}+{y})", BinaryOps.SUB: lambda x,y,: f"({x}-{y})", + BinaryOps.MUL: lambda x,y,: f"({x}*{y})", BinaryOps.DIV: lambda x,y,: f"({x}/{y})" if y != '0.0' else f"{x}*tl.where({x}==0.0, float('nan'), float('inf'))", + BinaryOps.MAX: lambda x,y,: f"tl.maximum({x},{y})", + BinaryOps.CMPLT: lambda x,y,: f"({x}<{y})", + BinaryOps.MOD: lambda x,y,: f"tl.abs({x})%tl.abs({y})*tl.where({x}<0,-1,1)", + TernaryOps.MULACC: lambda x,y,z,: f"(({x}*{y})+{z})", + TernaryOps.WHERE: lambda x,y,z,: f"tl.where({x},{y},{z})", + } + def int_div(x,y): return f"({x}//{y})" if y != '0' else f"{x}*tl.where({x}==0, float('nan'), float('inf'))" + for u in uops: + uop,dtype,vin,args,_ = u + if uop == UOps.LOOP: + kk(f"for {ssa(u, 'ridx')} in range({vin[0].arg}, {r[vin[1]]}):") + depth += 1 + elif uop == UOps.END: depth -= 1 + elif uop == UOps.ALU: + assert dtype is not None + val = code_for_op[args](*[r[x] for x in vin]) + if child_count[u] <=1 or dtypes.is_int(dtype): r[u] = int_div(*[r[x] for x in vin]) if args == BinaryOps.DIV and dtypes.is_int(dtype) else val + else: kk(f"{ssa(u, 'alu')} = ({val})") + elif uop == UOps.LOAD: + assert dtype is not None + if len(vin) == 2: kk(f"{ssa(u, 'val')} = {render_cast(f'tl.load({r[vin[0]]} + { fill_dims_for_idx(r[vin[1]], dims)}, mask = {render_valid(valid)})', dtype)}") + else: kk(f"{ssa(u, 'val')} = {render_cast(f'tl.where({r[vin[2]]}, tl.load({r[vin[0]]}+{fill_dims_for_idx(r[vin[1]],dims)} , mask={render_valid(valid+[r[vin[2]]])}), 0.0)', dtype)}") + elif uop == UOps.DEFINE_ACC: kk(f"{ssa(u, 'acc')} = {define_scalar(local_size, dtype, args).replace('//', '/')}") + elif uop == UOps.CONST: r[u] = define_scalar([], dtype, args) + elif uop == UOps.PHI: + kk(f"{r[vin[0]]} = {r[vin[1]].replace('//', '/')}") + r[u] = r[vin[0]] + elif uop == UOps.STORE: + assert not isinstance(dtype, ImageDType), "unimplemented: image store" + kk(f"tl.store({r[vin[0]]} + {r[vin[1]]}, {r[vin[2]].replace('//', '/')}, mask = {render_valid(valid)}) ") + elif uop == UOps.DEFINE_GLOBAL: + bufs.append(args) + signatures.append(signature_dtypes[args[1]]) + r[u] = args[0] + elif uop == UOps.SPECIAL: + dims.append(args[1]) + valid.append(f"{args[1]}<{get_max(args[2])}") + if args[1].startswith("g"): kk(f"{args[1]} = tl.program_id({args[0]}) # {args[2]}") + elif args[1].startswith("l"): + kk(f"{args[1]} = tl.arange({0}, {next_power_of_2(args[2])})") + local_size.append(args[2]) + r[u] = args[1] + else: raise NotImplementedError(f"unimplemented: {uop}") + + prg = f"import triton\nimport triton.language as tl\ntl.core.TRITON_MAX_TENSOR_NUMEL = float('inf')\n@triton.jit\ndef {function_name}("+','.join(f"{buf[0]}" for buf in bufs)+"):\n" + for i, line in enumerate(list(filter(lambda line: "tl.arange" in line, kernel))): kernel[kernel.index(line)] += f"[{', '.join([':' if i == j else 'None' for j in range(len(local_size))])}]" + prg += "\n".join(kernel) + + acc_local_size = 1 + for x in local_size: acc_local_size *= next_power_of_2(x) + local_size = [acc_local_size] + [1] * (len(local_size) - 1) + + if DEBUG >= 4: print(prg) + getlines = linecache.getlines + linecache.getlines = lambda filename, module_globals=None: prg.splitlines(keepends=True) if "" == filename else getlines(filename, module_globals) + exec(compile(prg, "", "exec"), globals()) # pylint: disable=W0122\ + compiled = triton_compile(globals()[function_name], signature=",".join(signatures), device_type="cuda", debug=False, cc=(35 if getenv("CUDACPU", 0) else None)) + prg = remove_single_scalar_curly_braces(compiled.asm["ptx"].split(".file")[0].split(".visible .func")[0]) + max_local_size = [int(x) for x in prg.split(".maxntid ")[1].split("\n")[0].split(", ")] + for i in range(len(local_size)): local_size[i] = min(local_size[i], max_local_size[i]) + + return prg, {"shared":compiled.metadata["shared"], "local_size":local_size + [1]*(3-len(local_size))} diff --git a/tinygrad_repo/tinygrad/renderer/wgsl.py b/tinygrad_repo/tinygrad/renderer/wgsl.py new file mode 100644 index 0000000000..feb5215d0b --- /dev/null +++ b/tinygrad_repo/tinygrad/renderer/wgsl.py @@ -0,0 +1,62 @@ +from tinygrad.helpers import dtypes, DType +from tinygrad.renderer.cstyle import CStyleLanguage +from typing import List, Union +from tinygrad.ops import UnaryOps, BinaryOps, TernaryOps +import math +from typing import Tuple + +type_map = {dtypes.float: "f32", dtypes.half: "f16", dtypes.int32: "i32", dtypes.uint32: "u32", dtypes.bool: "bool"} +class WGSLLanguage(CStyleLanguage): + gid = [f"i32(gindex.{'xyz'[x]})" for x in range(3)] + lid = [f"i32(lindex.{'xyz'[x]})" for x in range(3)] + size_prefix = "let" + barrier="workgroupBarrier();" + generic_var_prefix = "var " + external_local_bufs = True + code_for_op = { + UnaryOps.NEG: lambda x: f"(-{x})", + UnaryOps.EXP2: lambda x: f"exp2({x})", UnaryOps.LOG2: lambda x: f"log2({x})", + UnaryOps.SIN: lambda x: f"sin({x})", UnaryOps.SQRT: lambda x: f"sqrt({x})", + BinaryOps.ADD: lambda x,y: f"({x}+{y})", BinaryOps.SUB: lambda x,y: f"({x}-{y})", BinaryOps.MUL: lambda x,y: f"({x}*{y})", + BinaryOps.DIV: lambda x,y: f"({x}/{y})", BinaryOps.MOD: lambda x,y: f"({x}%{y})", + BinaryOps.MAX: lambda x,y: f"max({x},{y})", BinaryOps.CMPLT: lambda x,y: f"f32({x}<{y})", + TernaryOps.MULACC: lambda x,y,z: f"fma({x},{y},{z})", TernaryOps.WHERE: lambda a,b,c: f"select({c},{b},{a}!=0.)" + } + + def render_local(self, name: str, size: int): + return f"var {name}: array;" + + def render_const(self, x:Union[float,int], var_dtype) -> str: + if math.isnan(x): val = "nan()" + elif math.isinf(x): val = ("-" if x < 0 else "") + "0x1.fffffep+127f" + else: val = f"({x}" + ("" if dtypes.is_int(var_dtype) else "f") + ")" + return self.render_cast([val]*var_dtype.sz, var_dtype) if var_dtype.sz > 1 else val + + def render_kernel(self, function_name:str, kernel:List[str], bufs:List[Tuple[str,DType]], local_size:List[int], prekernel:List[str]) -> str: + local_size = local_size[::-1] if local_size else [1] + bind_it = iter(range(len(bufs))) + prg = "fn nan() -> f32 { let bits = 0xffffffffu; return bitcast(bits); }\n" + prg += "\n".join(prekernel+[f"@group(0) @binding({next(bind_it)}) var {name}: array<{type_map[dtype]}>;" for name,dtype in bufs]) + prg += f"\n@compute @workgroup_size({','.join([str(x) for x in local_size])}) fn {function_name}(@builtin(workgroup_id) gindex: vec3, @builtin(local_invocation_id) lindex: vec3) {{\n" + "\n".join(kernel) + "\n}" + return prg + + def render_for(self, expr:str, _min:Union[int,str], _max:Union[int,str]) -> str: + return f"for(var {expr} = {_min}; {expr} < {_max}; {expr}++) {{" + + def render_if(self, cond: str): + return f"if (bool({cond})) {{" + + def render_conditional(self, cond:str, x:str, y:str) -> str: + return f"select(f32({y}), {x}, bool({cond}))" + + def render_cast(self, x:List[str], var_dtype:DType) -> str: + if type_map[var_dtype]: return f"{type_map[var_dtype]}({x[0]})" + raise NotImplementedError(f"no cast for {var_dtype}") + + def render_load(self, output_dtype, buf_name, buf_dtype, idx, local=False) -> str: + return f"f32({super().render_load(output_dtype, buf_name, buf_dtype, idx, local)})" + + def render_store(self, buf_name:str, buf_dtype:DType, var_name:str, var_dtype:DType, idx, local=False) -> str: + if buf_dtype != var_dtype: + var_name = f"{type_map[buf_dtype]}({var_name})" + return f"{buf_name}[{idx}] = {var_name};" \ No newline at end of file diff --git a/tinygrad_repo/tinygrad/runtime/ops_clang.py b/tinygrad_repo/tinygrad/runtime/ops_clang.py new file mode 100644 index 0000000000..59116a9333 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_clang.py @@ -0,0 +1,36 @@ +import time, ctypes, subprocess, platform, functools, pathlib, tempfile +from typing import Any +from tinygrad.ops import Compiled +from tinygrad.helpers import diskcache +from tinygrad.runtime.lib import RawMallocBuffer +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.renderer.cstyle import uops_to_cstyle, CStyleLanguage + +args = { + 'Linux': {'cflags':'-lm -fPIC --rtlib=compiler-rt ', 'ext':'so'}, + 'Darwin': {'cflags':'-lm -fPIC --rtlib=compiler-rt ', 'ext':'dylib'} +}[platform.system()] + +CLANG_PROGRAM_HEADER = '#include \n#define max(x,y) ((x>y)?x:y)\n#define int64 long\n#define half __fp16\n#define uchar unsigned char\n#include \n' + +@diskcache +def compile_clang(prg:str, header:str=CLANG_PROGRAM_HEADER) -> bytes: + # TODO: remove file write. sadly clang doesn't like the use of /dev/stdout here + with tempfile.NamedTemporaryFile(delete=True) as output_file: + subprocess.check_output(args=('clang -shared -O2 -Wall -Werror -x c '+args['cflags']+' - -o '+str(output_file.name)).split(), input=(header+prg).encode('utf-8')) + return pathlib.Path(output_file.name).read_bytes() + +class ClangProgram: + def __init__(self, name:str, prg:bytes): + # write to disk so we can load it + with tempfile.NamedTemporaryFile(delete=True) as cached_file_path: + pathlib.Path(cached_file_path.name).write_bytes(prg) + self.fxn: Any = ctypes.CDLL(str(cached_file_path.name))[name] + + def __call__(self, *args, wait=False): + if wait: st = time.perf_counter() + self.fxn(*[x._buf if isinstance(x, RawMallocBuffer) else x for x in args]) + if wait: return time.perf_counter()-st + +renderer = functools.partial(uops_to_cstyle, CStyleLanguage(buffer_suffix=" restrict", arg_int_prefix="const int")) +ClangBuffer = Compiled(RawMallocBuffer, LinearizerOptions(supports_float4=False, has_local=False), renderer, compile_clang, ClangProgram) diff --git a/tinygrad_repo/tinygrad/runtime/ops_cuda.py b/tinygrad_repo/tinygrad/runtime/ops_cuda.py new file mode 100644 index 0000000000..73a3e5b4af --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_cuda.py @@ -0,0 +1,93 @@ +import subprocess, time, re, hashlib, tempfile +from pathlib import Path +from typing import Optional, Tuple +import numpy as np +from pycuda.compiler import compile as cuda_compile # type: ignore +from tinygrad.helpers import DEBUG, getenv, colored, diskcache +from tinygrad.ops import Compiled +from tinygrad.runtime.lib import RawBufferCopyInOut, RawMallocBuffer, LRUAllocator +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.renderer.cuda import CUDARenderer + +def pretty_ptx(s): + # all expressions match `` and replace it with `color()` + s = re.sub(r'([!@<\[\s,\+\-;\n])((?:[_%$][\w%\$_]+(?:\.[xyz])?\:?)|(?:buf\d+))([<>\]\s,\+\-;\n\)])', lambda m:m[1]+colored(m[2], "blue")+m[3], s, flags=re.M) # identifiers + s = re.sub(r'(.)((?:b|s|u|f)(?:8|16|32|64)|pred)([\.\s])', lambda m:m[1]+colored(m[2], "green")+m[3], s, flags=re.M) # types + s = re.sub(r'^(\s*)([\w]+)(.*?;$)', lambda m:m[1]+colored(m[2], "yellow")+m[3], s, flags=re.M) # instructions + s = re.sub(r'([<>\[\]\s,\+\-;])((?:0[fF][0-9a-fA-F]{8})|(?:[0-9]+)|(?:0[xX][0-9a-fA-F]+))([<>\[\]\s,\+\-;])', lambda m:m[1]+colored(m[2], "yellow")+m[3], s, flags=re.M) # numbers + s = re.sub(r'(\.)(param|reg|global)', lambda m:m[1]+colored(m[2], "magenta"), s, flags=re.M) # space + s = re.sub(r'(\.)(version|target|address_size|visible|entry)', lambda m:m[1]+colored(m[2], "magenta"), s, flags=re.M) # derivatives + return s +def arch(): return "sm_" + "".join([str(x) for x in pycuda.driver.Context.get_device().compute_capability()]) + +if getenv("CUDACPU", 0) == 1: + import ctypes, ctypes.util + lib = ctypes.CDLL(ctypes.util.find_library("gpuocelot")) + lib.ptx_run.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.POINTER(ctypes.c_void_p), ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int, ctypes.c_int] + class cuda: + class module: + def __init__(self, src): self.src = src + def get_function(self, _): return self + def __call__(self, *args, block, grid, shared): lib.ptx_run(self.src, len(args), (ctypes.c_void_p * len(args))(*[ctypes.cast(x, ctypes.c_void_p) for x in args]), *block, *grid, shared) + module_from_buffer = lambda src: cuda.module(src) # pylint: disable=unnecessary-lambda # noqa: E731 + class Event: + def __init__(self): pass + def record(self): self.start = time.perf_counter() + def time_till(self, other): return other.start - self.start + def synchronize(self): pass + class Context: + synchronize = lambda:0 # noqa: E731 + CompileError = Exception + class context: + class device: + compute_capability = lambda: (3,5) # pylint: disable=unnecessary-lambda # noqa: E731 + get_device = lambda: context.device # pylint: disable=unnecessary-lambda # noqa: E731 + import pycuda.driver # type: ignore + pycuda.driver.Context = context + RawCUDABuffer = RawMallocBuffer +else: + import pycuda.autoprimaryctx # type: ignore # pylint: disable=unused-import # noqa: F401 + import pycuda.driver as cuda # type: ignore + class CUDAAllocator(LRUAllocator): + def _do_alloc(self, size, dtype, device, **kwargs): return cuda.mem_alloc(size * dtype.itemsize) # type: ignore + def _cached_bufkey(self, size, dtype, device): return (device, size*dtype.itemsize) # Buffers of the same length could be reused, no matter what dtype. + CUDAAlloc = CUDAAllocator(pycuda.driver.Context.get_device().total_memory()) + class RawCUDABuffer(RawBufferCopyInOut): # type: ignore + def __init__(self, size, dtype): super().__init__(size, dtype, allocator=CUDAAlloc) + def _copyin(self, x:np.ndarray, stream:Optional[cuda.Stream]=None): cuda.memcpy_htod_async(self._buf, x.ravel(), stream) # type: ignore + def _copyout(self, x:np.ndarray): cuda.memcpy_dtoh(x, self._buf) # type: ignore + +@diskcache +def compile_cuda(prg) -> bytes: return cuda_compile(prg, target="ptx", no_extern_c=True, options=['-Wno-deprecated-gpu-targets']) + +class CUDAProgram: + def __init__(self, name:str, _prg:bytes): + prg = _prg.decode('utf-8') + if DEBUG >= 5: print(pretty_ptx(prg)) + if DEBUG >= 6: + try: + fn = (Path(tempfile.gettempdir()) / f"tinycuda_{hashlib.md5(prg.encode('utf-8')).hexdigest()}").as_posix() + with open(fn + ".ptx", "wb") as f: f.write(prg.encode('utf-8')) + subprocess.run(["ptxas", f"-arch={arch()}", "-o", fn, fn+".ptx"], check=True) + print(subprocess.check_output(['nvdisasm', fn]).decode('utf-8')) + except Exception as e: print("failed to generate SASS", str(e)) + # TODO: name is wrong, so we get it from the ptx using hacks + self.prg = cuda.module_from_buffer(prg.encode('utf-8')).get_function(prg.split(".visible .entry ")[1].split("(")[0]) + + def __call__(self, *args, global_size:Tuple[int,int,int], local_size:Tuple[int,int,int], shared:int=0, wait=False): + if wait: + start, end = cuda.Event(), cuda.Event() + start.record() + self.prg(*[x._buf if isinstance(x, RawCUDABuffer) else np.int32(x) if (isinstance(x, int) and not getenv("CUDACPU")) else x for x in args], block=tuple(local_size), grid=tuple(global_size), shared=shared) + if wait: + end.record() + end.synchronize() + return start.time_till(end)*1e-3 + +if getenv("TRITON") == 1: + from tinygrad.renderer.triton import uops_to_triton + CUDABuffer = Compiled(RawCUDABuffer, LinearizerOptions(supports_float4=False, supports_float4_alu=False, global_max = [65535, 65535, 2147483647], local_max = [64, 1024, 1024], has_shared=False), + uops_to_triton, lambda x: x.encode('utf-8'), CUDAProgram, cuda.Context.synchronize) +else: + CUDABuffer = Compiled(RawCUDABuffer, LinearizerOptions(supports_float4=False if getenv("PTX") else True, supports_float4_alu=False, global_max = [65535, 65535, 2147483647], local_max = [64, 1024, 1024]), + CUDARenderer, compile_cuda, CUDAProgram, cuda.Context.synchronize) diff --git a/tinygrad_repo/tinygrad/runtime/ops_hip.py b/tinygrad_repo/tinygrad/runtime/ops_hip.py new file mode 100644 index 0000000000..7420406550 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_hip.py @@ -0,0 +1,104 @@ +import numpy as np +import ctypes, functools +import extra.hip_wrapper as hip +from typing import Tuple +from tinygrad.helpers import DEBUG, getenv, diskcache +from tinygrad.ops import Compiled +from tinygrad.runtime.lib import RawBufferCopyInOut, LRUAllocator, RawBufferTransfer +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.renderer.cstyle import uops_to_cstyle, CStyleLanguage + +# TODO: if you fork and exit the child process after creating anything with cl on AMD, it hangs on e.wait() +if DEBUG >= 6: + from extra.helpers import enable_early_exec + early_exec = enable_early_exec() + +# The default HIP stream is used for everything. + +class HIPAllocator(LRUAllocator): + def _do_alloc(self, size, dtype, device, **kwargs): + hip.hipSetDevice(device) + return hip.hipMalloc(size * dtype.itemsize) + def _do_free(self, buf): hip.hipFree(buf) + def _cached_bufkey(self, size, dtype, device): return (device, size*dtype.itemsize) # Buffers of the same length could be reused, no matter what dtype. + +class _HIP: + def __init__(self, device=None): + self.default_device = device or getenv("HIP_DEFAULT_DEVICE") + hip.hipSetDevice(self.default_device) + self.device_count = hip.hipGetDeviceCount() + self.allocator = HIPAllocator(hip.hipGetDeviceProperties(self.default_device).totalGlobalMem) +HIP = _HIP() + +class RawHIPBuffer(RawBufferCopyInOut, RawBufferTransfer): + def __init__(self, size, dtype, device=HIP.default_device, buf=None, allocator=HIP.allocator): super().__init__(size, dtype, buf=buf, allocator=allocator, **{'device': int(device)}) + def _copyin(self, x:np.ndarray): + hip.hipSetDevice(self._device) + hip.hipMemcpyAsync(self._buf, np.require(x, requirements='C').ctypes.data_as(ctypes.c_void_p), self.size * self.dtype.itemsize, hip.hipMemcpyHostToDevice, 0) + def _copyout(self, x:np.ndarray): + hip.hipSetDevice(self._device) + hip.hipMemcpy(x.ctypes.data, self._buf, self.size * self.dtype.itemsize, hip.hipMemcpyDeviceToHost) + def _transfer(self, x): + hip.hipSetDevice(x._device) + hip.hipMemcpy(self._buf, x._buf, self.size * self.dtype.itemsize, hip.hipMemcpyDeviceToDevice) + +@diskcache +def compile_hip(prg) -> bytes: + prog = hip.hiprtcCreateProgram(prg, "", [], []) + hip.hiprtcCompileProgram(prog, [f'--offload-arch={hip.hipGetDeviceProperties(HIP.default_device).gcnArchName}']) + return hip.hiprtcGetCode(prog) + +class HIPProgram: + def __init__(self, name:str, prg:bytes): + self.modules, self.prgs = [], [] + + if DEBUG >= 6: + asm = early_exec((["/opt/rocm/llvm/bin/llvm-objdump", '-d', '-'], prg)) + print('\n'.join([x for x in asm.decode('utf-8').split("\n") if 's_code_end' not in x])) + + for i in range(HIP.device_count): + hip.hipSetDevice(i) + self.modules.append(hip.hipModuleLoadData(prg)) + self.prgs.append(hip.hipModuleGetFunction(self.modules[-1], name)) + + def __call__(self, *args, global_size:Tuple[int,int,int], local_size:Tuple[int,int,int], wait=False): + hip.hipSetDevice(args[0]._device) + if wait: + start, end = hip.hipEventCreate(), hip.hipEventCreate() + hip.hipEventRecord(start) + class PackageStruct(ctypes.Structure): + _fields_ = [(f'field{idx}', ctypes.c_void_p if not isinstance(args[idx], int) else ctypes.c_int) for idx in range(len(args))] + struct = PackageStruct(*[data._buf if not isinstance(data, int) else np.int32(data) for data in args]) + hip.hipModuleLaunchKernel(self.prgs[args[0]._device], global_size[0], global_size[1], global_size[2], local_size[0], local_size[1], local_size[2], 0, 0, struct) + if wait: + hip.hipEventRecord(end) + hip.hipEventSynchronize(end) + return hip.hipEventElapsedTime(start, end)*1e-3 + + def __del__(self): + for module in self.modules: hip.hipModuleUnload(module) + +renderer = functools.partial(uops_to_cstyle, CStyleLanguage( + kernel_prefix = "#include \n#define INFINITY (__builtin_inff())\n#define NAN (__builtin_nanf(\"\"))" + """ +__device__ float4 max(float4 x, float4 y) { return float4(max(x.x, y.x), max(x.y, y.y), max(x.z, y.z), max(x.w, y.w)); } +__device__ float4 pow(float x, float4 y) { return float4(pow(x, y.x), pow(x, y.y), pow(x, y.z), pow(x, y.w)); } +__device__ float4 pow(float4 x, float4 y) { return float4(pow(x.x, y.x), pow(x.y, y.y), pow(x.z, y.z), pow(x.w, y.w)); } +__device__ float4 log2(float4 x) { return float4(log2(x.x), log2(x.y), log2(x.z), log2(x.w)); } +__device__ float4 exp2(float4 x) { return float4(exp2(x.x), exp2(x.y), exp2(x.z), exp2(x.w)); } +__device__ float4 sin(float4 x) { return float4(sin(x.x), sin(x.y), sin(x.z), sin(x.w)); } +typedef float float8 __attribute__((ext_vector_type(8))); +typedef _Float16 half16 __attribute__((ext_vector_type(16))); +extern "C" __global__ + """, launch_bounds=True, + smem_prefix = "__shared__ ", smem_prefix_for_cast=False, barrier = "__syncthreads();", float4 = "make_float4", uses_vload=True, uses_ptr_arithmetic=True, arg_int_prefix = "const int", + half_prekernel = "#include \nusing half4 = HIP_vector_type;" + """ +__device__ float vload_half(size_t offset, const half *p) { return (float)*(p + offset); } +__device__ float2 vload_half2(size_t offset, const half *p) { return make_float2((float)*(p + offset*2), (float)*(p + offset*2 + 1)); } +__device__ float4 vload_half4(size_t offset, const half *p) { return make_float4((float)*(p + offset*4), (float)*(p + offset*4 + 1), (float)*(p + offset*4 + 2), (float)*(p + offset*4 + 3)); } +__device__ void vstore_half(float data, size_t offset, half *p) { *(p + offset) = (half)data; } +__device__ void vstore_half2(float2 data, size_t offset, half *p) { *(p + offset*2) = (half)data.x; *(p + offset*2 + 1) = (half)data.y; } +__device__ void vstore_half4(float4 data, size_t offset, half *p) { *(p + offset*4) = (half)data.x; *(p + offset*4 + 1) = (half)data.y; *(p + offset*4 + 2) = (half)data.z; *(p + offset*4 + 3) = (half)data.w; } + """, + gid = [f'blockIdx.{chr(120+i)}' for i in range(3)], + lid = [f'threadIdx.{chr(120+i)}' for i in range(3)])) +HIPBuffer = Compiled(RawHIPBuffer, LinearizerOptions(device="HIP"), renderer, compile_hip, HIPProgram, hip.hipDeviceSynchronize) diff --git a/tinygrad_repo/tinygrad/runtime/ops_llvm.py b/tinygrad_repo/tinygrad/runtime/ops_llvm.py new file mode 100644 index 0000000000..9cfaa45a28 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_llvm.py @@ -0,0 +1,68 @@ +import time, ctypes +from typing import ClassVar +from tinygrad.ops import Compiled +from tinygrad.helpers import getenv, DEBUG, diskcache +from ctypes import CFUNCTYPE +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.renderer.llvmir import uops_to_llvm_ir +from tinygrad.runtime.lib import RawMallocBuffer + +import llvmlite.binding as llvm # type: ignore + +LLVMOPT = bool(getenv("LLVMOPT")) + +class LLVM: + target_machine: ClassVar[llvm.targets.TargetMachine] = None + engine: ClassVar[llvm.executionengine.ExecutionEngine] = None + optimizer: ClassVar[llvm.passmanagers.ModulePassManager] = None + + def __init__(self): + if LLVM.engine is not None: return + llvm.initialize() + llvm.initialize_native_target() + llvm.initialize_native_asmprinter() + llvm.initialize_native_asmparser() + target = llvm.Target.from_triple(llvm.get_process_triple()) + LLVM.optimizer = llvm.create_module_pass_manager() + LLVM.target_machine = target.create_target_machine(opt=2) # this opt actually can change things. ex: opt=3 means no FMA, opt=2 means FMA + LLVM.target_machine.add_analysis_passes(LLVM.optimizer) + + # TODO: this makes compile times so much faster + if LLVMOPT: + llvm.set_option(str(), '-force-vector-interleave=4') # this makes sum the same speed as torch, it also doubles the (slow) conv speed + if DEBUG >= 4: llvm.set_option(str(), '--debug-only=loop-vectorize') + #llvm.set_option(str(), '--debug') + + # does this do anything? + builder = llvm.create_pass_manager_builder() + builder.opt_level = 3 + builder.size_level = 0 + builder.loop_vectorize = True + builder.slp_vectorize = True + builder.populate(LLVM.optimizer) + + LLVM.target_machine.set_asm_verbosity(True) + backing_mod = llvm.parse_assembly(str()) + backing_mod.triple = llvm.get_process_triple() + LLVM.engine = llvm.create_mcjit_compiler(backing_mod, LLVM.target_machine) + +@diskcache +def compile_llvm(prg, llvmopt=LLVMOPT) -> bytes: + mod = llvm.parse_assembly(prg) + mod.verify() + LLVM().optimizer.run(mod) + if DEBUG >= 5: print(LLVM.target_machine.emit_assembly(mod)) + return LLVM.target_machine.emit_object(mod) + +class LLVMProgram: + def __init__(self, name:str, lib:bytes): + LLVM().engine.add_object_file(llvm.object_file.ObjectFileRef.from_data(lib)) + self.fxn = LLVM.engine.get_function_address(name) + + def __call__(self, *bufs, wait=False): + cfunc = CFUNCTYPE(ctypes.c_int, *[ctypes.c_void_p for _ in bufs])(self.fxn) + if wait: st = time.perf_counter() + cfunc(*[x._buf if not isinstance(x, int) else x for x in bufs]) + if wait: return time.perf_counter()-st + +LLVMBuffer = Compiled(RawMallocBuffer, LinearizerOptions(supports_float4=False, has_local=False, has_shared=False), uops_to_llvm_ir, compile_llvm, LLVMProgram) diff --git a/tinygrad_repo/tinygrad/runtime/ops_metal.py b/tinygrad_repo/tinygrad/runtime/ops_metal.py new file mode 100644 index 0000000000..584576b6cb --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_metal.py @@ -0,0 +1,83 @@ +# pip3 install pyobjc-framework-Metal pyobjc-framework-Cocoa pyobjc-framework-libdispatch +import os, subprocess, pathlib, ctypes, tempfile +import Metal, Cocoa, libdispatch # type: ignore +from typing import List, Any, Tuple +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.helpers import prod, getenv, DEBUG, DType, dtypes, diskcache +from tinygrad.ops import Compiled +from tinygrad.renderer.metal import MetalRenderer +from tinygrad.runtime.lib import RawBufferMapped, LRUAllocator + +class MetalAllocator(LRUAllocator): + def _do_alloc(self, size, dtype, device, **kwargs): return METAL.device.newBufferWithLength_options_(size*dtype.itemsize, Metal.MTLResourceStorageModeShared) + def _do_free(self, buf): buf.release() + def _cached_bufkey(self, size, dtype, device): return (device, size*dtype.itemsize) # Buffers of the same length could be reused, no matter what dtype. + +class _METAL: + def __init__(self): + self.mtl_buffers_in_flight: List[Any] = [] + self.device = Metal.MTLCreateSystemDefaultDevice() + self.mtl_queue = self.device.newCommandQueueWithMaxCommandBufferCount_(1024) + self.allocator = MetalAllocator(self.device.dedicatedMemorySize() or self.device.sharedMemorySize()) + # TODO: is there a better way to do this? + def synchronize(self): + for cbuf in self.mtl_buffers_in_flight: cbuf.waitUntilCompleted() + self.mtl_buffers_in_flight.clear() +METAL = _METAL() + +class RawMetalBuffer(RawBufferMapped): + def __init__(self, size:int, dtype:DType): + assert dtype != dtypes.double, f"METAL does not support {dtype.name}" + super().__init__(size, dtype, allocator=METAL.allocator) + def _buffer(self): + METAL.synchronize() + return self._buf.contents().as_buffer(self._buf.length()) + +def unwrap(x): + ret, err = x + assert err is None, str(err) + return ret + +@diskcache +def compile_metal(prg, use_xcode=bool(getenv("METAL_XCODE"))) -> bytes: + if use_xcode: + # NOTE: if you run llvm-dis on "air" you can see the llvm bytecode + air = subprocess.check_output(['xcrun', '-sdk', 'macosx', 'metal', '-x', 'metal', '-c', '-', '-o', '-'], input=prg.encode('utf-8')) + return subprocess.check_output(['xcrun', '-sdk', 'macosx', 'metallib', '-', '-o', '-'], input=air) + options = Metal.MTLCompileOptions.alloc().init() + library = unwrap(METAL.device.newLibraryWithSource_options_error_(prg, options, None)) + # TODO: avoid file write here? + with tempfile.NamedTemporaryFile(delete=True) as output_file: + unwrap(library.serializeToURL_error_(Cocoa.NSURL.URLWithString_(f"file://{output_file.name}"), None)) + return pathlib.Path(output_file.name).read_bytes() + +class MetalProgram: + def __init__(self, name:str, lib:bytes): + data = libdispatch.dispatch_data_create(lib, len(lib), None, None) + self.library = unwrap(METAL.device.newLibraryWithData_error_(data, None)) + self.fxn = self.library.newFunctionWithName_(name) + if DEBUG >= 5: + with tempfile.NamedTemporaryFile(delete=True) as shader: + shader.write(lib) + shader.flush() + os.system(f"cd {pathlib.Path(__file__).parents[2]}/disassemblers/applegpu && python3 compiler_explorer.py {shader.name}") + self.pipeline_state = unwrap(METAL.device.newComputePipelineStateWithFunction_error_(self.fxn, None)) + + def __call__(self, *bufs, global_size:Tuple[int,int,int], local_size:Tuple[int,int,int], wait=False): + assert prod(local_size) <= self.pipeline_state.maxTotalThreadsPerThreadgroup(), f"local size {local_size} bigger than {self.pipeline_state.maxTotalThreadsPerThreadgroup()} with exec width {self.pipeline_state.threadExecutionWidth()} memory length {self.pipeline_state.staticThreadgroupMemoryLength()}" + command_buffer = METAL.mtl_queue.commandBuffer() + encoder = command_buffer.computeCommandEncoder() + encoder.setComputePipelineState_(self.pipeline_state) + for i,a in enumerate(bufs): + if isinstance(a, RawMetalBuffer): encoder.setBuffer_offset_atIndex_(a._buf, 0, i) + elif isinstance(a, int): encoder.setBytes_length_atIndex_((arg:=ctypes.c_int32(a)), ctypes.sizeof(arg), i) + else: raise RuntimeError(f"arg at index {i} has unsupported type {type(a)}") + encoder.dispatchThreadgroups_threadsPerThreadgroup_(Metal.MTLSize(*global_size), Metal.MTLSize(*local_size)) + encoder.endEncoding() + command_buffer.commit() + if wait: + command_buffer.waitUntilCompleted() + return command_buffer.GPUEndTime() - command_buffer.GPUStartTime() + METAL.mtl_buffers_in_flight.append(command_buffer) + +MetalBuffer = Compiled(RawMetalBuffer, LinearizerOptions(device="METAL"), MetalRenderer, compile_metal, MetalProgram, METAL.synchronize) diff --git a/tinygrad_repo/tinygrad/runtime/ops_shm.py b/tinygrad_repo/tinygrad/runtime/ops_shm.py new file mode 100644 index 0000000000..10e8652d14 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_shm.py @@ -0,0 +1,35 @@ +import os, mmap +try: import _posixshmem # type: ignore +except Exception: pass +from typing import Callable, Dict +from tinygrad.helpers import DType, OSX +from tinygrad.runtime.lib import RawBufferMapped +from tinygrad.ops import Interpreted, Op, UnaryOps, MovementOps, BufferOps + +SHM_CACHE: Dict[str, mmap.mmap] = {} +class RawShmBuffer(RawBufferMapped): + def __init__(self, size, dtype:DType, device:str): + device, self.cache_id = device.split(",")[0], None if "," not in device else device.split(",")[1] + + if self.cache_id is not None and self.cache_id in SHM_CACHE: shm = SHM_CACHE[self.cache_id] + else: + if OSX: + with open(f"/tmp/shm_{device}", "w+b") as f: + f.truncate(size * dtype.itemsize) + shm = mmap.mmap(f.fileno(), size * dtype.itemsize, flags=mmap.MAP_SHARED) + else: + fd = _posixshmem.shm_open(device, os.O_RDWR, 0o600) + # TODO: these flags are somewhat platform specific, but python doesn't expose the ones we need + shm = mmap.mmap(fd, size * dtype.itemsize, flags=mmap.MAP_SHARED | 0x2000 | 0x008000) + shm.madvise(mmap.MADV_HUGEPAGE) # type: ignore + os.close(fd) + if self.cache_id is not None: SHM_CACHE[self.cache_id] = shm + + super().__init__(size, dtype, shm) + def __del__(self): + if self.cache_id is None: self._buf.close() + def _buffer(self): return memoryview(self._buf) + +# TODO: is this wrong? +shm_fxn_for_op: Dict[Op, Callable] = { BufferOps.MEM: lambda x: x, UnaryOps.NOOP: lambda x:x, MovementOps.RESHAPE: lambda x,_:x, MovementOps.AS_STRIDED: lambda x,_:x } +ShmBuffer = Interpreted(RawShmBuffer, shm_fxn_for_op, from_underlying=lambda x:x) diff --git a/tinygrad_repo/tinygrad/runtime/ops_torch.py b/tinygrad_repo/tinygrad/runtime/ops_torch.py new file mode 100644 index 0000000000..7122eb54b2 --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_torch.py @@ -0,0 +1,41 @@ +import torch +import numpy as np +from typing import Dict, Callable, Optional +from tinygrad.ops import BufferOps, UnaryOps, BinaryOps, MovementOps, TernaryOps, Op, Interpreted +from tinygrad.helpers import getenv, dtypes, prod, DType +from tinygrad.runtime.ops_cpu import base_fxn_for_op, einsum_mulacc +from tinygrad.runtime.lib import RawBuffer + +device = torch.device("cuda:0" if torch.cuda.is_available() else ("mps" if getenv("MPS", 0) else "cpu")) +type_map = {torch.float64: dtypes.float64, torch.float16: dtypes.float16, torch.float32: dtypes.float32, torch.int8: dtypes.int8, torch.int32: dtypes.int32, torch.int64: dtypes.int64, torch.uint8: dtypes.uint8, torch.bool: dtypes.bool} +inverse_type_map = {v:k for k,v in type_map.items()} + +def as_strided(x, arg): + if any(i < 0 for i in arg[1]): + return torch.as_strided(x.contiguous(), arg[0], tuple(abs(i) for i in arg[1]), + arg[2] + sum((s-1)*a if a < 0 else 0 for (s,a) in zip(arg[0], arg[1]))).flip([i for i,a in enumerate(arg[1]) if a < 0]) + return torch.as_strided(x.contiguous(), arg[0], arg[1], arg[2]) + +torch_fxn_for_op: Dict[Op, Callable] = {**base_fxn_for_op, **{ + # TODO: torch.tensor should work here + #BufferOps.CONST: lambda val, dtype: torch.tensor(val, dtype=inverse_type_map[dtype]), + BufferOps.CONST: lambda val, dtype: torch.from_numpy(np.array(val, dtype=dtype.np)), + UnaryOps.NOOP: lambda x: x.contiguous(), UnaryOps.SQRT: lambda x: x.sqrt(), UnaryOps.EXP2: lambda x: x.exp2(), UnaryOps.LOG2: lambda x: x.log2(), UnaryOps.SIN: torch.sin, + UnaryOps.CAST: lambda x,y: (x.view if y[1] else x.type)(next(k for k,v in type_map.items() if v==y[0])), + BinaryOps.MAX: torch.maximum, BinaryOps.CMPLT: lambda x,y: (x=0 for s in x.strides) else x.copy()).requires_grad_(False).to(device) + return cls(prod(x.shape), type_map[buf.dtype], buf) + def toCPU(self): return self._buf.cpu().numpy() +TorchBuffer = Interpreted(RawTorchBuffer, torch_fxn_for_op, from_underlying=lambda x: RawTorchBuffer(prod(x.shape), type_map[x.dtype], x)) diff --git a/tinygrad_repo/tinygrad/runtime/ops_webgpu.py b/tinygrad_repo/tinygrad/runtime/ops_webgpu.py new file mode 100644 index 0000000000..b186bfe0dc --- /dev/null +++ b/tinygrad_repo/tinygrad/runtime/ops_webgpu.py @@ -0,0 +1,45 @@ +import numpy as np +import functools +from wgpu.utils._device import get_default_device # type: ignore +from tinygrad.runtime.lib import RawBufferCopyIn, LRUAllocator +from tinygrad.helpers import dtypes, DType +from tinygrad.ops import Compiled +from tinygrad.codegen.kernel import LinearizerOptions +from tinygrad.renderer.cstyle import uops_to_cstyle +from tinygrad.renderer.wgsl import WGSLLanguage +import wgpu # type: ignore + +wgpu_device = get_default_device() + +class WebGPUProgram: + def __init__(self, name: str, prg: str): self.name,self.prg = name,wgpu_device.create_shader_module(code=prg) + def __call__(self, *bufs, global_size, local_size, wait=False): + assert len(bufs) <= 8, "WEBGPU only supports 8 buffers" + binding_layouts = [{"binding": i, "visibility": wgpu.ShaderStage.COMPUTE, "buffer": {"type": wgpu.BufferBindingType.storage}} for i in range(len(bufs))] + bindings = [{"binding": i, "resource": {"buffer": x._buf, "offset": 0, "size": x._buf.size}} for i, x in enumerate(bufs)] + bind_group_layout = wgpu_device.create_bind_group_layout(entries=binding_layouts) + pipeline_layout = wgpu_device.create_pipeline_layout(bind_group_layouts=[bind_group_layout]) + bind_group = wgpu_device.create_bind_group(layout=bind_group_layout, entries=bindings) + compute_pipeline = wgpu_device.create_compute_pipeline(layout=pipeline_layout,compute={"module": self.prg, "entry_point": self.name},) + command_encoder = wgpu_device.create_command_encoder() + compute_pass = command_encoder.begin_compute_pass() + compute_pass.set_pipeline(compute_pipeline) + compute_pass.set_bind_group(0, bind_group, [], 0, 999999) # last 2 not used + compute_pass.dispatch_workgroups(*global_size) # x y z + compute_pass.end() + wgpu_device.queue.submit([command_encoder.finish()]) + +class RawWebGPUAllocator(LRUAllocator): + def _do_alloc(self, size, dtype, device, **kwargs): return wgpu_device.create_buffer(size=size*dtype.itemsize, usage=wgpu.BufferUsage.STORAGE | wgpu.BufferUsage.COPY_DST | wgpu.BufferUsage.COPY_SRC) + def _cached_bufkey(self, size, dtype, device): return (device, size*dtype.itemsize) # Buffers of the same length could be reused, no matter what dtype. +WebGPUAlloc = RawWebGPUAllocator(wgpu_device.limits['max_buffer_size']) + +class RawWebGPUBuffer(RawBufferCopyIn): + def __init__(self, size:int, dtype:DType): + assert dtype not in [dtypes.int8,dtypes.uint8,dtypes.int64,dtypes.uint64,dtypes.double], f"dtype {dtype} not supported on WEBGPU" + super().__init__(size, dtype, allocator=WebGPUAlloc) + def _copyin(self, x:np.ndarray): wgpu_device.queue.write_buffer(self._buf, 0, np.ascontiguousarray(x)) + def toCPU(self) -> np.ndarray: return np.frombuffer(wgpu_device.queue.read_buffer(self._buf, 0), dtype=np.dtype(self.dtype.np, metadata={"backing": self})) # type: ignore + +renderer = functools.partial(uops_to_cstyle, WGSLLanguage()) +WebGpuBuffer = Compiled(RawWebGPUBuffer, LinearizerOptions(device="WEBGPU", supports_float4=False, local_max=[256, 256, 64], global_max=[65535, 65535, 65535]), renderer, lambda x: x, WebGPUProgram) diff --git a/tools/bodyteleop/.gitignore b/tools/bodyteleop/.gitignore new file mode 100644 index 0000000000..adeab99a95 --- /dev/null +++ b/tools/bodyteleop/.gitignore @@ -0,0 +1,4 @@ +av +av-10.0.0/* +key.pem +cert.pem \ No newline at end of file diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py index b1fb9525db..fd8f691d19 100644 --- a/tools/bodyteleop/web.py +++ b/tools/bodyteleop/web.py @@ -56,7 +56,7 @@ def create_ssl_cert(cert_path: str, key_path: str): try: proc = subprocess.run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \ -days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"', - stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) + capture_output=True, shell=True) proc.check_returncode() except subprocess.CalledProcessError as ex: raise ValueError(f"Error creating SSL certificate:\n[stdout]\n{proc.stdout.decode()}\n[stderr]\n{proc.stderr.decode()}") from ex @@ -77,7 +77,7 @@ def create_ssl_context(): ## ENDPOINTS async def index(request: 'web.Request'): - with open(os.path.join(TELEOPDIR, "static", "index.html"), "r") as f: + with open(os.path.join(TELEOPDIR, "static", "index.html")) as f: content = f.read() return web.Response(content_type="text/html", text=content) diff --git a/tools/joystick/README.md b/tools/joystick/README.md deleted file mode 100644 index aea93dad53..0000000000 --- a/tools/joystick/README.md +++ /dev/null @@ -1,53 +0,0 @@ -# Joystick - -**Hardware needed**: device running openpilot, laptop, joystick (optional) - -With joystickd, you can connect your laptop to your comma device over the network and debug controls using a joystick or keyboard. -joystickd uses [inputs](https://pypi.org/project/inputs) which supports many common gamepads and joysticks. - -## Usage - -The car must be off, and openpilot must be offroad before starting `joystickd`. - -### Using a keyboard - -SSH into your comma device and start joystickd with the following command: - -```shell -tools/joystick/joystickd.py --keyboard -``` - -The available buttons and axes will print showing their key mappings. In general, the WASD keys control gas and brakes and steering torque in 5% increments. - -### Joystick on your comma three - -Plug the joystick into your comma three aux USB-C port. Then, SSH into the device and start `joystickd.py`. - -### Joystick on your laptop - -In order to use a joystick over the network, we need to run joystickd locally from your laptop and have it send `testJoystick` packets over the network to the comma device. - -1. Connect a joystick to your PC. -2. Connect your laptop to your comma device's hotspot and open a new SSH shell. Since joystickd is being run on your laptop, we need to write a parameter to let controlsd know to start in joystick debug mode: - ```shell - # on your comma device - echo -n "1" > /data/params/d/JoystickDebugMode - ``` -3. Run bridge with your laptop's IP address. This republishes the `testJoystick` packets sent from your laptop so that openpilot can receive them: - ```shell - # on your comma device - cereal/messaging/bridge {LAPTOP_IP} testJoystick - ``` -4. Start joystickd on your laptop in ZMQ mode. - ```shell - # on your laptop - export ZMQ=1 - tools/joystick/joystickd.py - ``` - ---- -Now start your car and openpilot should go into joystick mode with an alert on startup! The status of the axes will display on the alert, while button statuses print in the shell. - -Make sure the conditions are met in the panda to allow controls (e.g. cruise control engaged). You can also make a modification to the panda code to always allow controls. - -![](https://github.com/commaai/openpilot/assets/8762862/e640cbca-cb7a-4dcb-abce-b23b036ad8e7) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py deleted file mode 100755 index 176ff25be6..0000000000 --- a/tools/joystick/joystickd.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python -import os -import argparse -import threading -from inputs import get_gamepad - -import cereal.messaging as messaging -from openpilot.common.realtime import Ratekeeper -from openpilot.common.numpy_fast import interp, clip -from openpilot.common.params import Params -from openpilot.tools.lib.kbhit import KBHit - - -class Keyboard: - def __init__(self): - self.kb = KBHit() - self.axis_increment = 0.05 # 5% of full actuation each key press - self.axes_map = {'w': 'gb', 's': 'gb', - 'a': 'steer', 'd': 'steer'} - self.axes_values = {'gb': 0., 'steer': 0.} - self.axes_order = ['gb', 'steer'] - self.cancel = False - - def update(self): - key = self.kb.getch().lower() - self.cancel = False - if key == 'r': - self.axes_values = {ax: 0. for ax in self.axes_values} - elif key == 'c': - self.cancel = True - elif key in self.axes_map: - axis = self.axes_map[key] - incr = self.axis_increment if key in ['w', 'a'] else -self.axis_increment - self.axes_values[axis] = clip(self.axes_values[axis] + incr, -1, 1) - else: - return False - return True - - -class Joystick: - def __init__(self, gamepad=False): - # TODO: find a way to get this from API, perhaps "inputs" doesn't support it - if gamepad: - self.cancel_button = 'BTN_NORTH' # (BTN_NORTH=X, ABS_RZ=Right Trigger) - accel_axis = 'ABS_Y' - steer_axis = 'ABS_RX' - else: - self.cancel_button = 'BTN_TRIGGER' - accel_axis = 'ABS_Y' - steer_axis = 'ABS_RX' - self.min_axis_value = {accel_axis: 0., steer_axis: 0.} - self.max_axis_value = {accel_axis: 255., steer_axis: 255.} - self.axes_values = {accel_axis: 0., steer_axis: 0.} - self.axes_order = [accel_axis, steer_axis] - self.cancel = False - - def update(self): - joystick_event = get_gamepad()[0] - event = (joystick_event.code, joystick_event.state) - if event[0] == self.cancel_button: - if event[1] == 1: - self.cancel = True - elif event[1] == 0: # state 0 is falling edge - self.cancel = False - elif event[0] in self.axes_values: - self.max_axis_value[event[0]] = max(event[1], self.max_axis_value[event[0]]) - self.min_axis_value[event[0]] = min(event[1], self.min_axis_value[event[0]]) - - norm = -interp(event[1], [self.min_axis_value[event[0]], self.max_axis_value[event[0]]], [-1., 1.]) - self.axes_values[event[0]] = norm if abs(norm) > 0.05 else 0. # center can be noisy, deadzone of 5% - else: - return False - return True - - -def send_thread(joystick): - joystick_sock = messaging.pub_sock('testJoystick') - rk = Ratekeeper(100, print_delay_threshold=None) - while 1: - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = [joystick.axes_values[a] for a in joystick.axes_order] - dat.testJoystick.buttons = [joystick.cancel] - joystick_sock.send(dat.to_bytes()) - print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) - rk.keep_time() - -def joystick_thread(joystick): - Params().put_bool('JoystickDebugMode', True) - threading.Thread(target=send_thread, args=(joystick,), daemon=True).start() - while True: - joystick.update() - -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Publishes events from your joystick to control your car.\n' + - 'openpilot must be offroad before starting joysticked.', - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - parser.add_argument('--keyboard', action='store_true', help='Use your keyboard instead of a joystick') - parser.add_argument('--gamepad', action='store_true', help='Use gamepad configuration instead of joystick') - args = parser.parse_args() - - if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: - print("The car must be off before running joystickd.") - exit() - - print() - if args.keyboard: - print('Gas/brake control: `W` and `S` keys') - print('Steering control: `A` and `D` keys') - print('Buttons') - print('- `R`: Resets axes') - print('- `C`: Cancel cruise control') - else: - print('Using joystick, make sure to run cereal/messaging/bridge on your device if running over the network!') - - joystick = Keyboard() if args.keyboard else Joystick(args.gamepad) - joystick_thread(joystick) diff --git a/tools/lib/README.md b/tools/lib/README.md index a0c4a0863b..af1ad0de22 100644 --- a/tools/lib/README.md +++ b/tools/lib/README.md @@ -34,10 +34,16 @@ for msg in lr: ### Segment Ranges -We also support a new format called a "segment range", where you can specify which segments from a route to load. +We also support a new format called a "segment range": -```python +``` +344c5c15b34f2d8a / 2024-01-03--09-37-12 / 2:6 / q +[ dongle id ] [ timestamp ] [ selector ] [ query type] +``` +you can specify which segments from a route to load + +```python lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4") # 4th segment lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/4:6") # 4th and 5th segment lr = LogReader("a2a0ccea32023010|2023-07-27--13-01-19/-1") # last segment diff --git a/tools/lib/api.py b/tools/lib/api.py index 6ff9242f29..92a75d2a3b 100644 --- a/tools/lib/api.py +++ b/tools/lib/api.py @@ -2,7 +2,7 @@ import os import requests API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') -class CommaApi(): +class CommaApi: def __init__(self, token=None): self.session = requests.Session() self.session.headers['User-agent'] = 'OpenpilotTools' diff --git a/tools/lib/auth.py b/tools/lib/auth.py index 997d1f860d..5988397d0a 100755 --- a/tools/lib/auth.py +++ b/tools/lib/auth.py @@ -26,7 +26,7 @@ import sys import pprint import webbrowser from http.server import BaseHTTPRequestHandler, HTTPServer -from typing import Any, Dict +from typing import Any from urllib.parse import parse_qs, urlencode from openpilot.tools.lib.api import APIError, CommaApi, UnauthorizedError @@ -36,7 +36,7 @@ PORT = 3000 class ClientRedirectServer(HTTPServer): - query_params: Dict[str, Any] = {} + query_params: dict[str, Any] = {} class ClientRedirectHandler(BaseHTTPRequestHandler): diff --git a/tools/lib/azure_container.py b/tools/lib/azure_container.py index 7d9550266d..f5a3a8bfb1 100644 --- a/tools/lib/azure_container.py +++ b/tools/lib/azure_container.py @@ -2,7 +2,7 @@ import os from datetime import datetime, timedelta from functools import lru_cache from pathlib import Path -from typing import IO, Union +from typing import IO TOKEN_PATH = Path("/data/azure_token") @@ -57,18 +57,18 @@ class AzureContainer: ext = "hevc" if log_type.endswith('camera') else "bz2" return self.BASE_URL + f"{route_name.replace('|', '/')}/{segment_num}/{log_type}.{ext}" - def upload_bytes(self, data: Union[bytes, IO], blob_name: str) -> str: + def upload_bytes(self, data: bytes | IO, blob_name: str, overwrite=False) -> str: from azure.storage.blob import BlobClient blob = BlobClient( account_url=self.ACCOUNT_URL, container_name=self.CONTAINER, blob_name=blob_name, credential=get_azure_credential(), - overwrite=False, + overwrite=overwrite, ) - blob.upload_blob(data) + blob.upload_blob(data, overwrite=overwrite) return self.BASE_URL + blob_name - def upload_file(self, path: Union[str, os.PathLike], blob_name: str) -> str: + def upload_file(self, path: str | os.PathLike, blob_name: str, overwrite=False) -> str: with open(path, "rb") as f: - return self.upload_bytes(f, blob_name) + return self.upload_bytes(f, blob_name, overwrite) diff --git a/tools/lib/bootlog.py b/tools/lib/bootlog.py index 827ef1eefc..208ddc19c2 100644 --- a/tools/lib/bootlog.py +++ b/tools/lib/bootlog.py @@ -1,6 +1,5 @@ import functools import re -from typing import List, Optional from openpilot.tools.lib.auth_config import get_token from openpilot.tools.lib.api import CommaApi @@ -44,7 +43,7 @@ class Bootlog: return False return self.id < b.id -def get_bootlog_from_id(bootlog_id: str) -> Optional[Bootlog]: +def get_bootlog_from_id(bootlog_id: str) -> Bootlog | None: # TODO: implement an API endpoint for this bl = Bootlog(bootlog_id) for b in get_bootlogs(bl.dongle_id): @@ -52,7 +51,7 @@ def get_bootlog_from_id(bootlog_id: str) -> Optional[Bootlog]: return b return None -def get_bootlogs(dongle_id: str) -> List[Bootlog]: +def get_bootlogs(dongle_id: str) -> list[Bootlog]: api = CommaApi(get_token()) r = api.get(f'v1/devices/{dongle_id}/bootlogs') return [Bootlog(b) for b in r] diff --git a/tools/lib/comma_car_segments.py b/tools/lib/comma_car_segments.py index 9027fec637..78825504e6 100644 --- a/tools/lib/comma_car_segments.py +++ b/tools/lib/comma_car_segments.py @@ -1,13 +1,22 @@ import os import requests + # Forks with additional car support can fork the commaCarSegments repo on huggingface or host the LFS files themselves COMMA_CAR_SEGMENTS_REPO = os.environ.get("COMMA_CAR_SEGMENTS_REPO", "https://huggingface.co/datasets/commaai/commaCarSegments") COMMA_CAR_SEGMENTS_BRANCH = os.environ.get("COMMA_CAR_SEGMENTS_BRANCH", "main") COMMA_CAR_SEGMENTS_LFS_INSTANCE = os.environ.get("COMMA_CAR_SEGMENTS_LFS_INSTANCE", COMMA_CAR_SEGMENTS_REPO) def get_comma_car_segments_database(): - return requests.get(get_repo_raw_url("database.json")).json() + from openpilot.selfdrive.car.fingerprints import MIGRATION + + database = requests.get(get_repo_raw_url("database.json")).json() + + ret = {} + for platform in database: + ret[MIGRATION.get(platform, platform)] = database[platform] + + return ret # Helpers related to interfacing with the commaCarSegments repository, which contains a collection of public segments for users to perform validation on. diff --git a/tools/lib/filereader.py b/tools/lib/filereader.py index e9b8b4b2ce..3bfdc75feb 100644 --- a/tools/lib/filereader.py +++ b/tools/lib/filereader.py @@ -10,10 +10,11 @@ DATA_ENDPOINT = os.getenv("DATA_ENDPOINT", "http://data-raw.comma.internal/") def internal_source_available(): try: hostname = urlparse(DATA_ENDPOINT).hostname - if hostname: - socket.gethostbyname(hostname) - return True - except socket.gaierror: + port = urlparse(DATA_ENDPOINT).port or 80 + with socket.socket(socket.AF_INET,socket.SOCK_STREAM) as s: + s.connect((hostname, port)) + return True + except (socket.gaierror, ConnectionRefusedError): pass return False diff --git a/tools/lib/helpers.py b/tools/lib/helpers.py index f0af3d03c5..e9b4b85e70 100644 --- a/tools/lib/helpers.py +++ b/tools/lib/helpers.py @@ -1,7 +1,4 @@ import bz2 -import datetime - -TIME_FMT = "%Y-%m-%d--%H-%M-%S" # regex patterns @@ -9,25 +6,18 @@ class RE: DONGLE_ID = r'(?P[a-f0-9]{16})' TIMESTAMP = r'(?P[0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2})' LOG_ID_V2 = r'(?P[a-f0-9]{8})--(?P[a-z0-9]{10})' - LOG_ID = r'(?P(?:{}|{}))'.format(TIMESTAMP, LOG_ID_V2) - ROUTE_NAME = r'(?P{}[|_/]{})'.format(DONGLE_ID, LOG_ID) - SEGMENT_NAME = r'{}(?:--|/)(?P[0-9]+)'.format(ROUTE_NAME) + LOG_ID = fr'(?P(?:{TIMESTAMP}|{LOG_ID_V2}))' + ROUTE_NAME = fr'(?P{DONGLE_ID}[|_/]{LOG_ID})' + SEGMENT_NAME = fr'{ROUTE_NAME}(?:--|/)(?P[0-9]+)' INDEX = r'-?[0-9]+' - SLICE = r'(?P{})?:?(?P{})?:?(?P{})?'.format(INDEX, INDEX, INDEX) - SEGMENT_RANGE = r'{}(?:(--|/)(?P({})))?(?:/(?P([qras])))?'.format(ROUTE_NAME, SLICE) + SLICE = fr'(?P{INDEX})?:?(?P{INDEX})?:?(?P{INDEX})?' + SEGMENT_RANGE = fr'{ROUTE_NAME}(?:(--|/)(?P({SLICE})))?(?:/(?P([qras])))?' BOOTLOG_NAME = ROUTE_NAME - EXPLORER_FILE = r'^(?P{})--(?P[a-z]+\.[a-z0-9]+)$'.format(SEGMENT_NAME) - OP_SEGMENT_DIR = r'^(?P{})$'.format(SEGMENT_NAME) - - -def timestamp_to_datetime(t: str) -> datetime.datetime: - """ - Convert an openpilot route timestamp to a python datetime - """ - return datetime.datetime.strptime(t, TIME_FMT) + EXPLORER_FILE = fr'^(?P{SEGMENT_NAME})--(?P[a-z]+\.[a-z0-9]+)$' + OP_SEGMENT_DIR = fr'^(?P{SEGMENT_NAME})$' def save_log(dest, log_msgs, compress=True): diff --git a/tools/lib/live_logreader.py b/tools/lib/live_logreader.py index 0678fd1d00..6a7ecee6fd 100644 --- a/tools/lib/live_logreader.py +++ b/tools/lib/live_logreader.py @@ -1,5 +1,4 @@ import os -from typing import List from cereal import log as capnp_log, messaging from cereal.services import SERVICE_LIST @@ -8,7 +7,7 @@ from openpilot.tools.lib.logreader import LogIterable, RawLogIterable ALL_SERVICES = list(SERVICE_LIST.keys()) -def raw_live_logreader(services: List[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> RawLogIterable: +def raw_live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> RawLogIterable: if addr != "127.0.0.1": os.environ["ZMQ"] = "1" messaging.context = messaging.Context() @@ -25,7 +24,7 @@ def raw_live_logreader(services: List[str] = ALL_SERVICES, addr: str = '127.0.0. yield msg -def live_logreader(services: List[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> LogIterable: +def live_logreader(services: list[str] = ALL_SERVICES, addr: str = '127.0.0.1') -> LogIterable: for m in raw_live_logreader(services, addr): with capnp_log.Event.from_bytes(m) as evt: yield evt diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py index 7957712aff..669c1520db 100755 --- a/tools/lib/logreader.py +++ b/tools/lib/logreader.py @@ -11,7 +11,7 @@ import tqdm import urllib.parse import warnings -from typing import Callable, Dict, Iterable, Iterator, List, Optional, Type +from collections.abc import Callable, Iterable, Iterator from urllib.parse import parse_qs, urlparse from cereal import log as capnp_log @@ -21,7 +21,7 @@ from openpilot.tools.lib.openpilotci import get_url from openpilot.tools.lib.filereader import FileReader, file_exists, internal_source_available from openpilot.tools.lib.route import Route, SegmentRange -LogMessage = Type[capnp._DynamicStructReader] +LogMessage = type[capnp._DynamicStructReader] LogIterable = Iterable[LogMessage] RawLogIterable = Iterable[bytes] @@ -76,8 +76,8 @@ class ReadMode(enum.StrEnum): AUTO_INTERACTIVE = "i" # default to rlogs, fallback to qlogs with a prompt from the user -LogPath = Optional[str] -LogPaths = List[LogPath] +LogPath = str | None +LogPaths = list[LogPath] ValidFileCallable = Callable[[LogPath], bool] Source = Callable[[SegmentRange, ReadMode], LogPaths] @@ -89,7 +89,7 @@ def default_valid_file(fn: LogPath) -> bool: def auto_strategy(rlog_paths: LogPaths, qlog_paths: LogPaths, interactive: bool, valid_file: ValidFileCallable) -> LogPaths: # auto select logs based on availability - if any(rlog is None or not valid_file(rlog) for rlog in rlog_paths): + if any(rlog is None or not valid_file(rlog) for rlog in rlog_paths) and all(qlog is not None and valid_file(qlog) for qlog in qlog_paths): if interactive: if input("Some rlogs were not found, would you like to fallback to qlogs for those segments? (y/n) ").lower() != "y": return rlog_paths @@ -170,8 +170,17 @@ def auto_source(sr: SegmentRange, mode=ReadMode.RLOG) -> LogPaths: if mode == ReadMode.SANITIZED: return comma_car_segments_source(sr, mode) - SOURCES: List[Source] = [internal_source, openpilotci_source, comma_api_source, comma_car_segments_source,] + SOURCES: list[Source] = [internal_source, openpilotci_source, comma_api_source, comma_car_segments_source,] exceptions = [] + + # for automatic fallback modes, auto_source needs to first check if rlogs exist for any source + if mode in [ReadMode.AUTO, ReadMode.AUTO_INTERACTIVE]: + for source in SOURCES: + try: + return check_source(source, sr, ReadMode.RLOG) + except Exception: + pass + # Automatically determine viable source for source in SOURCES: try: @@ -212,7 +221,7 @@ def parse_indirect(identifier: str): class LogReader: - def _parse_identifiers(self, identifier: str | List[str]): + def _parse_identifiers(self, identifier: str | list[str]): if isinstance(identifier, list): return [i for j in identifier for i in self._parse_identifiers(j)] @@ -234,7 +243,7 @@ class LogReader: are uploaded or auto fallback to qlogs with '/a' selector at the end of the route name." return identifiers - def __init__(self, identifier: str | List[str], default_mode: ReadMode = ReadMode.RLOG, + def __init__(self, identifier: str | list[str], default_mode: ReadMode = ReadMode.RLOG, default_source=auto_source, sort_by_time=False, only_union_types=False): self.default_mode = default_mode self.default_source = default_source @@ -243,12 +252,12 @@ are uploaded or auto fallback to qlogs with '/a' selector at the end of the rout self.sort_by_time = sort_by_time self.only_union_types = only_union_types - self.__lrs: Dict[int, _LogFileReader] = {} + self.__lrs: dict[int, _LogFileReader] = {} self.reset() def _get_lr(self, i): if i not in self.__lrs: - self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i]) + self.__lrs[i] = _LogFileReader(self.logreader_identifiers[i], sort_by_time=self.sort_by_time, only_union_types=self.only_union_types) return self.__lrs[i] def __iter__(self): diff --git a/tools/lib/route.py b/tools/lib/route.py index 47ebdc7a51..d5fd41108b 100644 --- a/tools/lib/route.py +++ b/tools/lib/route.py @@ -4,7 +4,6 @@ from functools import cache from urllib.parse import urlparse from collections import defaultdict from itertools import chain -from typing import Optional, cast from openpilot.tools.lib.auth_config import get_token from openpilot.tools.lib.api import CommaApi @@ -231,7 +230,7 @@ class SegmentName: def route_name(self) -> RouteName: return self._route_name @property - def data_dir(self) -> Optional[str]: return self._data_dir + def data_dir(self) -> str | None: return self._data_dir def __str__(self) -> str: return self._canonical_name @@ -240,7 +239,9 @@ class SegmentName: def get_max_seg_number_cached(sr: 'SegmentRange') -> int: try: api = CommaApi(get_token()) - return cast(int, api.get("/v1/route/" + sr.route_name.replace("/", "|"))["segment_numbers"][-1]) + max_seg_number = api.get("/v1/route/" + sr.route_name.replace("/", "|"))["maxqlog"] + assert isinstance(max_seg_number, int) + return max_seg_number except Exception as e: raise Exception("unable to get max_segment_number. ensure you have access to this route or the route is public.") from e @@ -263,6 +264,10 @@ class SegmentRange: def timestamp(self) -> str: return self.m.group("timestamp") + @property + def log_id(self) -> str: + return self.m.group("log_id") + @property def slice(self) -> str: return self.m.group("slice") or "" @@ -291,7 +296,7 @@ class SegmentRange: return list(range(end + 1))[s] def __str__(self) -> str: - return f"{self.dongle_id}/{self.timestamp}" + (f"/{self.slice}" if self.slice else "") + (f"/{self.selector}" if self.selector else "") + return f"{self.dongle_id}/{self.log_id}" + (f"/{self.slice}" if self.slice else "") + (f"/{self.selector}" if self.selector else "") def __repr__(self) -> str: return self.__str__() diff --git a/tools/lib/tests/test_caching.py b/tools/lib/tests/test_caching.py old mode 100755 new mode 100644 index 4b73e0a301..2bb63b4dce --- a/tools/lib/tests/test_caching.py +++ b/tools/lib/tests/test_caching.py @@ -1,12 +1,11 @@ -#!/usr/bin/env python3 -from functools import partial import http.server import os -import unittest - -from parameterized import parameterized -from openpilot.selfdrive.athena.tests.helpers import with_http_server +import shutil +import socket +import pytest +from openpilot.selfdrive.test.helpers import http_server_context +from openpilot.system.hardware.hw import Paths from openpilot.tools.lib.url_file import URLFile @@ -15,7 +14,7 @@ class CachingTestRequestHandler(http.server.BaseHTTPRequestHandler): def do_GET(self): if self.FILE_EXISTS: - self.send_response(200, b'1234') + self.send_response(206 if "Range" in self.headers else 200, b'1234') else: self.send_response(404) self.end_headers() @@ -29,10 +28,39 @@ class CachingTestRequestHandler(http.server.BaseHTTPRequestHandler): self.end_headers() -with_caching_server = partial(with_http_server, handler=CachingTestRequestHandler) - - -class TestFileDownload(unittest.TestCase): +@pytest.fixture +def host(): + with http_server_context(handler=CachingTestRequestHandler) as (host, port): + yield f"http://{host}:{port}" + +class TestFileDownload: + + def test_pipeline_defaults(self, host): + # TODO: parameterize the defaults so we don't rely on hard-coded values in xx + + assert URLFile.pool_manager().pools._maxsize == 10# PoolManager num_pools param + pool_manager_defaults = { + "maxsize": 100, + "socket_options": [(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1),], + } + for k, v in pool_manager_defaults.items(): + assert URLFile.pool_manager().connection_pool_kw.get(k) == v + + retry_defaults = { + "total": 5, + "backoff_factor": 0.5, + "status_forcelist": [409, 429, 503, 504], + } + for k, v in retry_defaults.items(): + assert getattr(URLFile.pool_manager().connection_pool_kw["retries"], k) == v + + # ensure caching off by default and cache dir doesn't get created + os.environ.pop("FILEREADER_CACHE", None) + if os.path.exists(Paths.download_cache_root()): + shutil.rmtree(Paths.download_cache_root()) + URLFile(f"{host}/test.txt").get_length() + URLFile(f"{host}/test.txt").read() + assert not os.path.exists(Paths.download_cache_root()) def compare_loads(self, url, start=0, length=None): """Compares range between cached and non cached version""" @@ -42,21 +70,21 @@ class TestFileDownload(unittest.TestCase): file_cached.seek(start) file_downloaded.seek(start) - self.assertEqual(file_cached.get_length(), file_downloaded.get_length()) - self.assertLessEqual(length + start if length is not None else 0, file_downloaded.get_length()) + assert file_cached.get_length() == file_downloaded.get_length() + assert length + start if length is not None else 0 <= file_downloaded.get_length() response_cached = file_cached.read(ll=length) response_downloaded = file_downloaded.read(ll=length) - self.assertEqual(response_cached, response_downloaded) + assert response_cached == response_downloaded # Now test with cache in place file_cached = URLFile(url, cache=True) file_cached.seek(start) response_cached = file_cached.read(ll=length) - self.assertEqual(file_cached.get_length(), file_downloaded.get_length()) - self.assertEqual(response_cached, response_downloaded) + assert file_cached.get_length() == file_downloaded.get_length() + assert response_cached == response_downloaded def test_small_file(self): # Make sure we don't force cache @@ -87,22 +115,16 @@ class TestFileDownload(unittest.TestCase): self.compare_loads(large_file_url, length - 100, 100) self.compare_loads(large_file_url) - @parameterized.expand([(True, ), (False, )]) - @with_caching_server - def test_recover_from_missing_file(self, cache_enabled, host): + @pytest.mark.parametrize("cache_enabled", [True, False]) + def test_recover_from_missing_file(self, host, cache_enabled): os.environ["FILEREADER_CACHE"] = "1" if cache_enabled else "0" file_url = f"{host}/test.png" CachingTestRequestHandler.FILE_EXISTS = False length = URLFile(file_url).get_length() - self.assertEqual(length, -1) + assert length == -1 CachingTestRequestHandler.FILE_EXISTS = True length = URLFile(file_url).get_length() - self.assertEqual(length, 4) - - - -if __name__ == "__main__": - unittest.main() + assert length == 4 diff --git a/tools/lib/tests/test_comma_car_segments.py b/tools/lib/tests/test_comma_car_segments.py index b355b0fe60..b9a4def75f 100644 --- a/tools/lib/tests/test_comma_car_segments.py +++ b/tools/lib/tests/test_comma_car_segments.py @@ -1,14 +1,13 @@ - - -import unittest - +import pytest import requests +from openpilot.selfdrive.car.fingerprints import MIGRATION from openpilot.tools.lib.comma_car_segments import get_comma_car_segments_database, get_url from openpilot.tools.lib.logreader import LogReader from openpilot.tools.lib.route import SegmentRange -class TestCommaCarSegments(unittest.TestCase): +@pytest.mark.skip(reason="huggingface is flaky, run this test manually to check for issues") +class TestCommaCarSegments: def test_database(self): database = get_comma_car_segments_database() @@ -19,7 +18,7 @@ class TestCommaCarSegments(unittest.TestCase): def test_download_segment(self): database = get_comma_car_segments_database() - fp = "SUBARU FORESTER 2019" + fp = "SUBARU_FORESTER" segment = database[fp][0] @@ -28,14 +27,8 @@ class TestCommaCarSegments(unittest.TestCase): url = get_url(sr.route_name, sr.slice) resp = requests.get(url) - self.assertEqual(resp.status_code, 200) + assert resp.status_code == 200 lr = LogReader(url) - CP = lr.first("carParams") - - self.assertEqual(CP.carFingerprint, fp) - - -if __name__ == "__main__": - unittest.main() + assert MIGRATION.get(CP.carFingerprint, CP.carFingerprint) == fp diff --git a/tools/lib/tests/test_logreader.py b/tools/lib/tests/test_logreader.py old mode 100755 new mode 100644 index 2141915b87..6bc7ba8773 --- a/tools/lib/tests/test_logreader.py +++ b/tools/lib/tests/test_logreader.py @@ -1,16 +1,15 @@ -#!/usr/bin/env python3 +import capnp import contextlib import io import shutil import tempfile import os -import unittest import pytest import requests from parameterized import parameterized -from unittest import mock +from cereal import log as capnp_log from openpilot.tools.lib.logreader import LogIterable, LogReader, comma_api_source, parse_indirect, ReadMode, InternalUnavailableException from openpilot.tools.lib.route import SegmentRange from openpilot.tools.lib.url_file import URLFileException @@ -26,24 +25,22 @@ def noop(segment: LogIterable): @contextlib.contextmanager -def setup_source_scenario(is_internal=False): - with ( - mock.patch("openpilot.tools.lib.logreader.internal_source") as internal_source_mock, - mock.patch("openpilot.tools.lib.logreader.openpilotci_source") as openpilotci_source_mock, - mock.patch("openpilot.tools.lib.logreader.comma_api_source") as comma_api_source_mock, - ): - if is_internal: - internal_source_mock.return_value = [QLOG_FILE] - else: - internal_source_mock.side_effect = InternalUnavailableException +def setup_source_scenario(mocker, is_internal=False): + internal_source_mock = mocker.patch("openpilot.tools.lib.logreader.internal_source") + openpilotci_source_mock = mocker.patch("openpilot.tools.lib.logreader.openpilotci_source") + comma_api_source_mock = mocker.patch("openpilot.tools.lib.logreader.comma_api_source") + if is_internal: + internal_source_mock.return_value = [QLOG_FILE] + else: + internal_source_mock.side_effect = InternalUnavailableException - openpilotci_source_mock.return_value = [None] - comma_api_source_mock.return_value = [QLOG_FILE] + openpilotci_source_mock.return_value = [None] + comma_api_source_mock.return_value = [QLOG_FILE] - yield + yield -class TestLogReader(unittest.TestCase): +class TestLogReader: @parameterized.expand([ (f"{TEST_ROUTE}", ALL_SEGS), (f"{TEST_ROUTE.replace('/', '|')}", ALL_SEGS), @@ -72,7 +69,7 @@ class TestLogReader(unittest.TestCase): def test_indirect_parsing(self, identifier, expected): parsed, _, _ = parse_indirect(identifier) sr = SegmentRange(parsed) - self.assertListEqual(list(sr.seg_idxs), expected, identifier) + assert list(sr.seg_idxs) == expected, identifier @parameterized.expand([ (f"{TEST_ROUTE}", f"{TEST_ROUTE}"), @@ -84,11 +81,11 @@ class TestLogReader(unittest.TestCase): ]) def test_canonical_name(self, identifier, expected): sr = SegmentRange(identifier) - self.assertEqual(str(sr), expected) + assert str(sr) == expected - @parameterized.expand([(True,), (False,)]) - @mock.patch("openpilot.tools.lib.logreader.file_exists") - def test_direct_parsing(self, cache_enabled, file_exists_mock): + @pytest.mark.parametrize("cache_enabled", [True, False]) + def test_direct_parsing(self, mocker, cache_enabled): + file_exists_mock = mocker.patch("openpilot.tools.lib.logreader.file_exists") os.environ["FILEREADER_CACHE"] = "1" if cache_enabled else "0" qlog = tempfile.NamedTemporaryFile(mode='wb', delete=False) @@ -98,13 +95,13 @@ class TestLogReader(unittest.TestCase): for f in [QLOG_FILE, qlog.name]: l = len(list(LogReader(f))) - self.assertGreater(l, 100) + assert l > 100 - with self.assertRaises(URLFileException) if not cache_enabled else self.assertRaises(AssertionError): + with pytest.raises(URLFileException) if not cache_enabled else pytest.raises(AssertionError): l = len(list(LogReader(QLOG_FILE.replace("/3/", "/200/")))) # file_exists should not be called for direct files - self.assertEqual(file_exists_mock.call_count, 0) + assert file_exists_mock.call_count == 0 @parameterized.expand([ (f"{TEST_ROUTE}///",), @@ -119,103 +116,136 @@ class TestLogReader(unittest.TestCase): (f"{TEST_ROUTE}--3a",), ]) def test_bad_ranges(self, segment_range): - with self.assertRaises(AssertionError): + with pytest.raises(AssertionError): _ = SegmentRange(segment_range).seg_idxs - @parameterized.expand([ + @pytest.mark.parametrize("segment_range, api_call", [ (f"{TEST_ROUTE}/0", False), (f"{TEST_ROUTE}/:2", False), (f"{TEST_ROUTE}/0:", True), (f"{TEST_ROUTE}/-1", True), (f"{TEST_ROUTE}", True), ]) - def test_slicing_api_call(self, segment_range, api_call): - with mock.patch("openpilot.tools.lib.route.get_max_seg_number_cached") as max_seg_mock: - max_seg_mock.return_value = NUM_SEGS - _ = SegmentRange(segment_range).seg_idxs - self.assertEqual(api_call, max_seg_mock.called) + def test_slicing_api_call(self, mocker, segment_range, api_call): + max_seg_mock = mocker.patch("openpilot.tools.lib.route.get_max_seg_number_cached") + max_seg_mock.return_value = NUM_SEGS + _ = SegmentRange(segment_range).seg_idxs + assert api_call == max_seg_mock.called @pytest.mark.slow def test_modes(self): qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0", ReadMode.QLOG))) rlog_len = len(list(LogReader(f"{TEST_ROUTE}/0", ReadMode.RLOG))) - self.assertLess(qlog_len * 6, rlog_len) + assert qlog_len * 6 < rlog_len @pytest.mark.slow def test_modes_from_name(self): qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/q"))) rlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/r"))) - self.assertLess(qlog_len * 6, rlog_len) + assert qlog_len * 6 < rlog_len @pytest.mark.slow def test_list(self): qlog_len = len(list(LogReader(f"{TEST_ROUTE}/0/q"))) qlog_len_2 = len(list(LogReader([f"{TEST_ROUTE}/0/q", f"{TEST_ROUTE}/0/q"]))) - self.assertEqual(qlog_len * 2, qlog_len_2) + assert qlog_len * 2 == qlog_len_2 @pytest.mark.slow - @mock.patch("openpilot.tools.lib.logreader._LogFileReader") - def test_multiple_iterations(self, init_mock): + def test_multiple_iterations(self, mocker): + init_mock = mocker.patch("openpilot.tools.lib.logreader._LogFileReader") lr = LogReader(f"{TEST_ROUTE}/0/q") qlog_len1 = len(list(lr)) qlog_len2 = len(list(lr)) # ensure we don't create multiple instances of _LogFileReader, which means downloading the files twice - self.assertEqual(init_mock.call_count, 1) + assert init_mock.call_count == 1 - self.assertEqual(qlog_len1, qlog_len2) + assert qlog_len1 == qlog_len2 @pytest.mark.slow def test_helpers(self): lr = LogReader(f"{TEST_ROUTE}/0/q") - self.assertEqual(lr.first("carParams").carFingerprint, "SUBARU OUTBACK 6TH GEN") - self.assertTrue(0 < len(list(lr.filter("carParams"))) < len(list(lr))) + assert lr.first("carParams").carFingerprint == "SUBARU OUTBACK 6TH GEN" + assert 0 < len(list(lr.filter("carParams"))) < len(list(lr)) @parameterized.expand([(True,), (False,)]) @pytest.mark.slow def test_run_across_segments(self, cache_enabled): os.environ["FILEREADER_CACHE"] = "1" if cache_enabled else "0" lr = LogReader(f"{TEST_ROUTE}/0:4") - self.assertEqual(len(lr.run_across_segments(4, noop)), len(list(lr))) + assert len(lr.run_across_segments(4, noop)) == len(list(lr)) @pytest.mark.slow - def test_auto_mode(self): + def test_auto_mode(self, subtests, mocker): lr = LogReader(f"{TEST_ROUTE}/0/q") qlog_len = len(list(lr)) - with mock.patch("openpilot.tools.lib.route.Route.log_paths") as log_paths_mock: - log_paths_mock.return_value = [None] * NUM_SEGS - # Should fall back to qlogs since rlogs are not available - - with self.subTest("interactive_yes"): - with mock.patch("sys.stdin", new=io.StringIO("y\n")): - lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, default_source=comma_api_source) - log_len = len(list(lr)) - self.assertEqual(qlog_len, log_len) - - with self.subTest("interactive_no"): - with mock.patch("sys.stdin", new=io.StringIO("n\n")): - with self.assertRaises(AssertionError): - lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, default_source=comma_api_source) - - with self.subTest("non_interactive"): - lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO, default_source=comma_api_source) - log_len = len(list(lr)) - self.assertEqual(qlog_len, log_len) + log_paths_mock = mocker.patch("openpilot.tools.lib.route.Route.log_paths") + log_paths_mock.return_value = [None] * NUM_SEGS + # Should fall back to qlogs since rlogs are not available - @parameterized.expand([(True,), (False,)]) + with subtests.test("interactive_yes"): + mocker.patch("sys.stdin", new=io.StringIO("y\n")) + lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, default_source=comma_api_source) + log_len = len(list(lr)) + assert qlog_len == log_len + + with subtests.test("interactive_no"): + mocker.patch("sys.stdin", new=io.StringIO("n\n")) + with pytest.raises(AssertionError): + lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO_INTERACTIVE, default_source=comma_api_source) + + with subtests.test("non_interactive"): + lr = LogReader(f"{TEST_ROUTE}/0", default_mode=ReadMode.AUTO, default_source=comma_api_source) + log_len = len(list(lr)) + assert qlog_len == log_len + + @pytest.mark.parametrize("is_internal", [True, False]) @pytest.mark.slow - def test_auto_source_scenarios(self, is_internal): + def test_auto_source_scenarios(self, mocker, is_internal): lr = LogReader(QLOG_FILE) qlog_len = len(list(lr)) - with setup_source_scenario(is_internal=is_internal): + with setup_source_scenario(mocker, is_internal=is_internal): lr = LogReader(f"{TEST_ROUTE}/0/q") log_len = len(list(lr)) - self.assertEqual(qlog_len, log_len) + assert qlog_len == log_len - -if __name__ == "__main__": - unittest.main() + @pytest.mark.slow + def test_sort_by_time(self): + msgs = list(LogReader(f"{TEST_ROUTE}/0/q")) + assert msgs != sorted(msgs, key=lambda m: m.logMonoTime) + + msgs = list(LogReader(f"{TEST_ROUTE}/0/q", sort_by_time=True)) + assert msgs == sorted(msgs, key=lambda m: m.logMonoTime) + + def test_only_union_types(self): + with tempfile.NamedTemporaryFile() as qlog: + # write valid Event messages + num_msgs = 100 + with open(qlog.name, "wb") as f: + f.write(b"".join(capnp_log.Event.new_message().to_bytes() for _ in range(num_msgs))) + + msgs = list(LogReader(qlog.name)) + assert len(msgs) == num_msgs + [m.which() for m in msgs] + + # append non-union Event message + event_msg = capnp_log.Event.new_message() + non_union_bytes = bytearray(event_msg.to_bytes()) + non_union_bytes[event_msg.total_size.word_count * 8] = 0xff # set discriminant value out of range using Event word offset + with open(qlog.name, "ab") as f: + f.write(non_union_bytes) + + # ensure new message is added, but is not a union type + msgs = list(LogReader(qlog.name)) + assert len(msgs) == num_msgs + 1 + with pytest.raises(capnp.KjException): + [m.which() for m in msgs] + + # should not be added when only_union_types=True + msgs = list(LogReader(qlog.name, only_union_types=True)) + assert len(msgs) == num_msgs + [m.which() for m in msgs] diff --git a/tools/lib/tests/test_readers.py b/tools/lib/tests/test_readers.py old mode 100755 new mode 100644 index 1f24ae5c8e..624531a1a8 --- a/tools/lib/tests/test_readers.py +++ b/tools/lib/tests/test_readers.py @@ -1,5 +1,4 @@ -#!/usr/bin/env python -import unittest +import pytest import requests import tempfile @@ -9,16 +8,16 @@ from openpilot.tools.lib.framereader import FrameReader from openpilot.tools.lib.logreader import LogReader -class TestReaders(unittest.TestCase): - @unittest.skip("skip for bandwidth reasons") +class TestReaders: + @pytest.mark.skip("skip for bandwidth reasons") def test_logreader(self): def _check_data(lr): hist = defaultdict(int) for l in lr: hist[l.which()] += 1 - self.assertEqual(hist['carControl'], 6000) - self.assertEqual(hist['logMessage'], 6857) + assert hist['carControl'] == 6000 + assert hist['logMessage'] == 6857 with tempfile.NamedTemporaryFile(suffix=".bz2") as fp: r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true", timeout=10) @@ -31,15 +30,15 @@ class TestReaders(unittest.TestCase): lr_url = LogReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true") _check_data(lr_url) - @unittest.skip("skip for bandwidth reasons") + @pytest.mark.skip("skip for bandwidth reasons") def test_framereader(self): def _check_data(f): - self.assertEqual(f.frame_count, 1200) - self.assertEqual(f.w, 1164) - self.assertEqual(f.h, 874) + assert f.frame_count == 1200 + assert f.w == 1164 + assert f.h == 874 frame_first_30 = f.get(0, 30) - self.assertEqual(len(frame_first_30), 30) + assert len(frame_first_30) == 30 print(frame_first_30[15]) @@ -62,6 +61,3 @@ class TestReaders(unittest.TestCase): fr_url = FrameReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true") _check_data(fr_url) - -if __name__ == "__main__": - unittest.main() diff --git a/tools/lib/tests/test_route_library.py b/tools/lib/tests/test_route_library.py old mode 100755 new mode 100644 index 7977f17be2..491bb81327 --- a/tools/lib/tests/test_route_library.py +++ b/tools/lib/tests/test_route_library.py @@ -1,10 +1,8 @@ -#!/usr/bin/env python -import unittest from collections import namedtuple from openpilot.tools.lib.route import SegmentName -class TestRouteLibrary(unittest.TestCase): +class TestRouteLibrary: def test_segment_name_formats(self): Case = namedtuple('Case', ['input', 'expected_route', 'expected_segment_num', 'expected_data_dir']) @@ -21,12 +19,9 @@ class TestRouteLibrary(unittest.TestCase): s = SegmentName(route_or_segment_name, allow_route_name=True) - self.assertEqual(str(s.route_name), case.expected_route) - self.assertEqual(s.segment_num, case.expected_segment_num) - self.assertEqual(s.data_dir, case.expected_data_dir) + assert str(s.route_name) == case.expected_route + assert s.segment_num == case.expected_segment_num + assert s.data_dir == case.expected_data_dir for case in cases: _validate(case) - -if __name__ == "__main__": - unittest.main() diff --git a/tools/lib/vidindex.py b/tools/lib/vidindex.py index 8156faba6b..f2e4e9ca45 100755 --- a/tools/lib/vidindex.py +++ b/tools/lib/vidindex.py @@ -3,7 +3,6 @@ import argparse import os import struct from enum import IntEnum -from typing import Tuple from openpilot.tools.lib.filereader import FileReader @@ -120,7 +119,7 @@ HEVC_CODED_SLICE_SEGMENT_NAL_UNITS = ( class VideoFileInvalid(Exception): pass -def get_ue(dat: bytes, start_idx: int, skip_bits: int) -> Tuple[int, int]: +def get_ue(dat: bytes, start_idx: int, skip_bits: int) -> tuple[int, int]: prefix_val = 0 prefix_len = 0 suffix_val = 0 @@ -184,7 +183,7 @@ def get_hevc_nal_unit_type(dat: bytes, nal_unit_start: int) -> HevcNalUnitType: print(" nal_unit_type:", nal_unit_type.name, f"({nal_unit_type.value})") return nal_unit_type -def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalUnitType) -> Tuple[int, bool]: +def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalUnitType) -> tuple[int, bool]: # 7.3.2.9 Slice segment layer RBSP syntax # slice_segment_layer_rbsp( ) { # slice_segment_header( ) @@ -259,7 +258,7 @@ def get_hevc_slice_type(dat: bytes, nal_unit_start: int, nal_unit_type: HevcNalU raise VideoFileInvalid("slice_type must be 0, 1, or 2") return slice_type, is_first_slice -def hevc_index(hevc_file_name: str, allow_corrupt: bool=False) -> Tuple[list, int, bytes]: +def hevc_index(hevc_file_name: str, allow_corrupt: bool=False) -> tuple[list, int, bytes]: with FileReader(hevc_file_name) as f: dat = f.read() diff --git a/tools/replay/camera.cc b/tools/replay/camera.cc deleted file mode 100644 index 49f3010c6c..0000000000 --- a/tools/replay/camera.cc +++ /dev/null @@ -1,103 +0,0 @@ -#include "tools/replay/camera.h" - -#include -#include - -#include "third_party/linux/include/msm_media_info.h" -#include "tools/replay/util.h" - -std::tuple get_nv12_info(int width, int height) { - int nv12_width = VENUS_Y_STRIDE(COLOR_FMT_NV12, width); - int nv12_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height); - assert(nv12_width == VENUS_UV_STRIDE(COLOR_FMT_NV12, width)); - assert(nv12_height / 2 == VENUS_UV_SCANLINES(COLOR_FMT_NV12, height)); - size_t nv12_buffer_size = 2346 * nv12_width; // comes from v4l2_format.fmt.pix_mp.plane_fmt[0].sizeimage - return {nv12_width, nv12_height, nv12_buffer_size}; -} - -CameraServer::CameraServer(std::pair camera_size[MAX_CAMERAS]) { - for (int i = 0; i < MAX_CAMERAS; ++i) { - std::tie(cameras_[i].width, cameras_[i].height) = camera_size[i]; - } - startVipcServer(); -} - -CameraServer::~CameraServer() { - for (auto &cam : cameras_) { - if (cam.thread.joinable()) { - cam.queue.push({}); - cam.thread.join(); - } - } - vipc_server_.reset(nullptr); -} - -void CameraServer::startVipcServer() { - vipc_server_.reset(new VisionIpcServer("camerad")); - for (auto &cam : cameras_) { - if (cam.width > 0 && cam.height > 0) { - rInfo("camera[%d] frame size %dx%d", cam.type, cam.width, cam.height); - auto [nv12_width, nv12_height, nv12_buffer_size] = get_nv12_info(cam.width, cam.height); - vipc_server_->create_buffers_with_sizes(cam.stream_type, YUV_BUFFER_COUNT, false, cam.width, cam.height, - nv12_buffer_size, nv12_width, nv12_width * nv12_height); - if (!cam.thread.joinable()) { - cam.thread = std::thread(&CameraServer::cameraThread, this, std::ref(cam)); - } - } - } - vipc_server_->start_listener(); -} - -void CameraServer::cameraThread(Camera &cam) { - auto read_frame = [&](FrameReader *fr, int frame_id) { - VisionBuf *yuv_buf = vipc_server_->get_buffer(cam.stream_type); - assert(yuv_buf); - bool ret = fr->get(frame_id, yuv_buf); - return ret ? yuv_buf : nullptr; - }; - - while (true) { - const auto [fr, eidx] = cam.queue.pop(); - if (!fr) break; - - const int id = eidx.getSegmentId(); - bool prefetched = (id == cam.cached_id && eidx.getSegmentNum() == cam.cached_seg); - auto yuv = prefetched ? cam.cached_buf : read_frame(fr, id); - if (yuv) { - VisionIpcBufExtra extra = { - .frame_id = eidx.getFrameId(), - .timestamp_sof = eidx.getTimestampSof(), - .timestamp_eof = eidx.getTimestampEof(), - }; - yuv->set_frame_id(eidx.getFrameId()); - vipc_server_->send(yuv, &extra); - } else { - rError("camera[%d] failed to get frame: %lu", cam.type, eidx.getSegmentId()); - } - - cam.cached_id = id + 1; - cam.cached_seg = eidx.getSegmentNum(); - cam.cached_buf = read_frame(fr, cam.cached_id); - - --publishing_; - } -} - -void CameraServer::pushFrame(CameraType type, FrameReader *fr, const cereal::EncodeIndex::Reader &eidx) { - auto &cam = cameras_[type]; - if (cam.width != fr->width || cam.height != fr->height) { - cam.width = fr->width; - cam.height = fr->height; - waitForSent(); - startVipcServer(); - } - - ++publishing_; - cam.queue.push({fr, eidx}); -} - -void CameraServer::waitForSent() { - while (publishing_ > 0) { - std::this_thread::yield(); - } -} diff --git a/tools/replay/camera.h b/tools/replay/camera.h deleted file mode 100644 index 9f43c5a362..0000000000 --- a/tools/replay/camera.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include "cereal/visionipc/visionipc_server.h" -#include "common/queue.h" -#include "tools/replay/framereader.h" -#include "tools/replay/logreader.h" - -std::tuple get_nv12_info(int width, int height); - -class CameraServer { -public: - CameraServer(std::pair camera_size[MAX_CAMERAS] = nullptr); - ~CameraServer(); - void pushFrame(CameraType type, FrameReader* fr, const cereal::EncodeIndex::Reader& eidx); - void waitForSent(); - -protected: - struct Camera { - CameraType type; - VisionStreamType stream_type; - int width; - int height; - std::thread thread; - SafeQueue> queue; - int cached_id = -1; - int cached_seg = -1; - VisionBuf * cached_buf; - }; - void startVipcServer(); - void cameraThread(Camera &cam); - - Camera cameras_[MAX_CAMERAS] = { - {.type = RoadCam, .stream_type = VISION_STREAM_ROAD}, - {.type = DriverCam, .stream_type = VISION_STREAM_DRIVER}, - {.type = WideRoadCam, .stream_type = VISION_STREAM_WIDE_ROAD}, - }; - std::atomic publishing_ = 0; - std::unique_ptr vipc_server_; -}; diff --git a/tools/replay/consoleui.cc b/tools/replay/consoleui.cc deleted file mode 100644 index 54056c6cd5..0000000000 --- a/tools/replay/consoleui.cc +++ /dev/null @@ -1,374 +0,0 @@ -#include "tools/replay/consoleui.h" - -#include -#include -#include -#include - -#include - -#include "common/util.h" -#include "common/version.h" - -namespace { - -const int BORDER_SIZE = 3; - -const std::initializer_list> keyboard_shortcuts[] = { - { - {"s", "+10s"}, - {"shift+s", "-10s"}, - {"m", "+60s"}, - {"shift+m", "-60s"}, - {"space", "Pause/Resume"}, - {"e", "Next Engagement"}, - {"d", "Next Disengagement"}, - {"t", "Next User Tag"}, - {"i", "Next Info"}, - {"w", "Next Warning"}, - {"c", "Next Critical"}, - }, - { - {"enter", "Enter seek request"}, - {"+/-", "Playback speed"}, - {"q", "Exit"}, - }, -}; - -enum Color { - Default, - Debug, - Yellow, - Green, - Red, - Cyan, - BrightWhite, - Engaged, - Disengaged, -}; - -void add_str(WINDOW *w, const char *str, Color color = Color::Default, bool bold = false) { - if (color != Color::Default) wattron(w, COLOR_PAIR(color)); - if (bold) wattron(w, A_BOLD); - waddstr(w, str); - if (bold) wattroff(w, A_BOLD); - if (color != Color::Default) wattroff(w, COLOR_PAIR(color)); -} - -} // namespace - -ConsoleUI::ConsoleUI(Replay *replay, QObject *parent) : replay(replay), sm({"carState", "liveParameters"}), QObject(parent) { - // Initialize curses - initscr(); - clear(); - curs_set(false); - cbreak(); // Line buffering disabled. pass on everything - noecho(); - keypad(stdscr, true); - nodelay(stdscr, true); // non-blocking getchar() - - // Initialize all the colors. https://www.ditig.com/256-colors-cheat-sheet - start_color(); - init_pair(Color::Debug, 246, COLOR_BLACK); // #949494 - init_pair(Color::Yellow, 184, COLOR_BLACK); - init_pair(Color::Red, COLOR_RED, COLOR_BLACK); - init_pair(Color::Cyan, COLOR_CYAN, COLOR_BLACK); - init_pair(Color::BrightWhite, 15, COLOR_BLACK); - init_pair(Color::Disengaged, COLOR_BLUE, COLOR_BLUE); - init_pair(Color::Engaged, 28, 28); - init_pair(Color::Green, 34, COLOR_BLACK); - - initWindows(); - - qRegisterMetaType("uint64_t"); - qRegisterMetaType("ReplyMsgType"); - installMessageHandler([this](ReplyMsgType type, const std::string msg) { - emit logMessageSignal(type, QString::fromStdString(msg)); - }); - installDownloadProgressHandler([this](uint64_t cur, uint64_t total, bool success) { - emit updateProgressBarSignal(cur, total, success); - }); - - QObject::connect(replay, &Replay::streamStarted, this, &ConsoleUI::updateSummary); - QObject::connect(¬ifier, SIGNAL(activated(int)), SLOT(readyRead())); - QObject::connect(this, &ConsoleUI::updateProgressBarSignal, this, &ConsoleUI::updateProgressBar); - QObject::connect(this, &ConsoleUI::logMessageSignal, this, &ConsoleUI::logMessage); - - sm_timer.callOnTimeout(this, &ConsoleUI::updateStatus); - sm_timer.start(100); - getch_timer.start(1000, this); - readyRead(); -} - -ConsoleUI::~ConsoleUI() { - endwin(); -} - -void ConsoleUI::initWindows() { - getmaxyx(stdscr, max_height, max_width); - w.fill(nullptr); - w[Win::Title] = newwin(1, max_width, 0, 0); - w[Win::Stats] = newwin(2, max_width - 2 * BORDER_SIZE, 2, BORDER_SIZE); - w[Win::Timeline] = newwin(4, max_width - 2 * BORDER_SIZE, 5, BORDER_SIZE); - w[Win::TimelineDesc] = newwin(1, 100, 10, BORDER_SIZE); - w[Win::CarState] = newwin(3, 100, 12, BORDER_SIZE); - w[Win::DownloadBar] = newwin(1, 100, 16, BORDER_SIZE); - if (int log_height = max_height - 27; log_height > 4) { - w[Win::LogBorder] = newwin(log_height, max_width - 2 * (BORDER_SIZE - 1), 17, BORDER_SIZE - 1); - box(w[Win::LogBorder], 0, 0); - w[Win::Log] = newwin(log_height - 2, max_width - 2 * BORDER_SIZE, 18, BORDER_SIZE); - scrollok(w[Win::Log], true); - } - w[Win::Help] = newwin(5, max_width - (2 * BORDER_SIZE), max_height - 6, BORDER_SIZE); - - // set the title bar - wbkgd(w[Win::Title], A_REVERSE); - mvwprintw(w[Win::Title], 0, 3, "openpilot replay %s", COMMA_VERSION); - - // show windows on the real screen - refresh(); - displayTimelineDesc(); - displayHelp(); - updateSummary(); - updateTimeline(); - for (auto win : w) { - if (win) wrefresh(win); - } -} - -void ConsoleUI::timerEvent(QTimerEvent *ev) { - if (ev->timerId() != getch_timer.timerId()) return; - - if (is_term_resized(max_height, max_width)) { - for (auto win : w) { - if (win) delwin(win); - } - endwin(); - clear(); - refresh(); - initWindows(); - rWarning("resize term %dx%d", max_height, max_width); - } - updateTimeline(); -} - -void ConsoleUI::updateStatus() { - auto write_item = [this](int y, int x, const char *key, const std::string &value, const std::string &unit, - bool bold = false, Color color = Color::BrightWhite) { - auto win = w[Win::CarState]; - wmove(win, y, x); - add_str(win, key); - add_str(win, value.c_str(), color, bold); - add_str(win, unit.c_str()); - }; - static const std::pair status_text[] = { - {"loading...", Color::Red}, - {"playing", Color::Green}, - {"paused...", Color::Yellow}, - }; - - sm.update(0); - - if (status != Status::Paused) { - auto events = replay->events(); - uint64_t current_mono_time = replay->routeStartTime() + replay->currentSeconds() * 1e9; - bool playing = !events->empty() && events->back()->mono_time > current_mono_time; - status = playing ? Status::Playing : Status::Waiting; - } - auto [status_str, status_color] = status_text[status]; - write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); - std::string current_segment = " - " + std::to_string((int)(replay->currentSeconds() / 60)); - write_item(0, 25, "TIME: ", replay->currentDateTime().toString("ddd MMMM dd hh:mm:ss").toStdString(), current_segment, true); - - auto p = sm["liveParameters"].getLiveParameters(); - write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); - write_item(1, 25, "SPEED: ", util::string_format("%.2f", sm["carState"].getCarState().getVEgo()), " m/s"); - write_item(2, 0, "STEER RATIO: ", util::string_format("%.2f", p.getSteerRatio()), ""); - auto angle_offsets = util::string_format("%.2f|%.2f", p.getAngleOffsetAverageDeg(), p.getAngleOffsetDeg()); - write_item(2, 25, "ANGLE OFFSET(AVG|INSTANT): ", angle_offsets, " deg"); - - wrefresh(w[Win::CarState]); -} - -void ConsoleUI::displayHelp() { - for (int i = 0; i < std::size(keyboard_shortcuts); ++i) { - wmove(w[Win::Help], i * 2, 0); - for (auto &[key, desc] : keyboard_shortcuts[i]) { - wattron(w[Win::Help], A_REVERSE); - waddstr(w[Win::Help], (' ' + key + ' ').c_str()); - wattroff(w[Win::Help], A_REVERSE); - waddstr(w[Win::Help], (' ' + desc + ' ').c_str()); - } - } - wrefresh(w[Win::Help]); -} - -void ConsoleUI::displayTimelineDesc() { - std::tuple indicators[]{ - {Color::Engaged, " Engaged ", false}, - {Color::Disengaged, " Disengaged ", false}, - {Color::Green, " Info ", true}, - {Color::Yellow, " Warning ", true}, - {Color::Red, " Critical ", true}, - {Color::Cyan, " User Tag ", true}, - }; - for (auto [color, name, bold] : indicators) { - add_str(w[Win::TimelineDesc], "__", color, bold); - add_str(w[Win::TimelineDesc], name); - } -} - -void ConsoleUI::logMessage(ReplyMsgType type, const QString &msg) { - if (auto win = w[Win::Log]) { - Color color = Color::Default; - if (type == ReplyMsgType::Debug) { - color = Color::Debug; - } else if (type == ReplyMsgType::Warning) { - color = Color::Yellow; - } else if (type == ReplyMsgType::Critical) { - color = Color::Red; - } - add_str(win, qPrintable(msg + "\n"), color); - wrefresh(win); - } -} - -void ConsoleUI::updateProgressBar(uint64_t cur, uint64_t total, bool success) { - werase(w[Win::DownloadBar]); - if (success && cur < total) { - const int width = 35; - const float progress = cur / (double)total; - const int pos = width * progress; - wprintw(w[Win::DownloadBar], "Downloading [%s>%s] %d%% %s", std::string(pos, '=').c_str(), - std::string(width - pos, ' ').c_str(), int(progress * 100.0), formattedDataSize(total).c_str()); - } - wrefresh(w[Win::DownloadBar]); -} - -void ConsoleUI::updateSummary() { - const auto &route = replay->route(); - mvwprintw(w[Win::Stats], 0, 0, "Route: %s, %lu segments", qPrintable(route->name()), route->segments().size()); - mvwprintw(w[Win::Stats], 1, 0, "Car Fingerprint: %s", replay->carFingerprint().c_str()); - wrefresh(w[Win::Stats]); -} - -void ConsoleUI::updateTimeline() { - auto win = w[Win::Timeline]; - int width = getmaxx(win); - werase(win); - - wattron(win, COLOR_PAIR(Color::Disengaged)); - mvwhline(win, 1, 0, ' ', width); - mvwhline(win, 2, 0, ' ', width); - wattroff(win, COLOR_PAIR(Color::Disengaged)); - - const int total_sec = replay->totalSeconds(); - for (auto [begin, end, type] : replay->getTimeline()) { - int start_pos = (begin / total_sec) * width; - int end_pos = (end / total_sec) * width; - if (type == TimelineType::Engaged) { - mvwchgat(win, 1, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); - mvwchgat(win, 2, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); - } else if (type == TimelineType::UserFlag) { - mvwchgat(win, 3, start_pos, end_pos - start_pos + 1, ACS_S3, Color::Cyan, NULL); - } else { - auto color_id = Color::Green; - if (type != TimelineType::AlertInfo) { - color_id = type == TimelineType::AlertWarning ? Color::Yellow : Color::Red; - } - mvwchgat(win, 3, start_pos, end_pos - start_pos + 1, ACS_S3, color_id, NULL); - } - } - - int cur_pos = ((double)replay->currentSeconds() / total_sec) * width; - wattron(win, COLOR_PAIR(Color::BrightWhite)); - mvwaddch(win, 0, cur_pos, ACS_VLINE); - mvwaddch(win, 3, cur_pos, ACS_VLINE); - wattroff(win, COLOR_PAIR(Color::BrightWhite)); - wrefresh(win); -} - -void ConsoleUI::readyRead() { - int c; - while ((c = getch()) != ERR) { - handleKey(c); - } -} - -void ConsoleUI::pauseReplay(bool pause) { - replay->pause(pause); - status = pause ? Status::Paused : Status::Waiting; -} - -void ConsoleUI::handleKey(char c) { - if (c == '\n') { - // pause the replay and blocking getchar() - pauseReplay(true); - updateStatus(); - getch_timer.stop(); - curs_set(true); - nodelay(stdscr, false); - - // Wait for user input - rWarning("Waiting for input..."); - int y = getmaxy(stdscr) - 9; - move(y, BORDER_SIZE); - add_str(stdscr, "Enter seek request: ", Color::BrightWhite, true); - refresh(); - - // Seek to choice - echo(); - int choice = 0; - scanw((char *)"%d", &choice); - noecho(); - pauseReplay(false); - replay->seekTo(choice, false); - - // Clean up and turn off the blocking mode - move(y, 0); - clrtoeol(); - nodelay(stdscr, true); - curs_set(false); - refresh(); - getch_timer.start(1000, this); - - } else if (c == '+' || c == '=') { - auto it = std::upper_bound(speed_array.begin(), speed_array.end(), replay->getSpeed()); - if (it != speed_array.end()) { - rWarning("playback speed: %.1fx", *it); - replay->setSpeed(*it); - } - } else if (c == '_' || c == '-') { - auto it = std::lower_bound(speed_array.begin(), speed_array.end(), replay->getSpeed()); - if (it != speed_array.begin()) { - auto prev = std::prev(it); - rWarning("playback speed: %.1fx", *prev); - replay->setSpeed(*prev); - } - } else if (c == 'e') { - replay->seekToFlag(FindFlag::nextEngagement); - } else if (c == 'd') { - replay->seekToFlag(FindFlag::nextDisEngagement); - } else if (c == 't') { - replay->seekToFlag(FindFlag::nextUserFlag); - } else if (c == 'i') { - replay->seekToFlag(FindFlag::nextInfo); - } else if (c == 'w') { - replay->seekToFlag(FindFlag::nextWarning); - } else if (c == 'c') { - replay->seekToFlag(FindFlag::nextCritical); - } else if (c == 'm') { - replay->seekTo(+60, true); - } else if (c == 'M') { - replay->seekTo(-60, true); - } else if (c == 's') { - replay->seekTo(+10, true); - } else if (c == 'S') { - replay->seekTo(-10, true); - } else if (c == ' ') { - pauseReplay(!replay->isPaused()); - } else if (c == 'q' || c == 'Q') { - replay->stop(); - qApp->exit(); - } -} diff --git a/tools/replay/consoleui.h b/tools/replay/consoleui.h deleted file mode 100644 index 6ed44bc623..0000000000 --- a/tools/replay/consoleui.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "tools/replay/replay.h" -#include - -class ConsoleUI : public QObject { - Q_OBJECT - -public: - ConsoleUI(Replay *replay, QObject *parent = 0); - ~ConsoleUI(); - inline static const std::array speed_array = {0.2f, 0.5f, 1.0f, 2.0f, 3.0f}; - -private: - void initWindows(); - void handleKey(char c); - void displayHelp(); - void displayTimelineDesc(); - void updateTimeline(); - void updateSummary(); - void updateStatus(); - void pauseReplay(bool pause); - - enum Status { Waiting, Playing, Paused }; - enum Win { Title, Stats, Log, LogBorder, DownloadBar, Timeline, TimelineDesc, Help, CarState, Max}; - std::array w{}; - SubMaster sm; - Replay *replay; - QBasicTimer getch_timer; - QTimer sm_timer; - QSocketNotifier notifier{0, QSocketNotifier::Read, this}; - int max_width, max_height; - Status status = Status::Waiting; - -signals: - void updateProgressBarSignal(uint64_t cur, uint64_t total, bool success); - void logMessageSignal(ReplyMsgType type, const QString &msg); - -private slots: - void readyRead(); - void timerEvent(QTimerEvent *ev); - void updateProgressBar(uint64_t cur, uint64_t total, bool success); - void logMessage(ReplyMsgType type, const QString &msg); -}; diff --git a/tools/replay/filereader.cc b/tools/replay/filereader.cc deleted file mode 100644 index 22af7f5f86..0000000000 --- a/tools/replay/filereader.cc +++ /dev/null @@ -1,46 +0,0 @@ -#include "tools/replay/filereader.h" - -#include - -#include "common/util.h" -#include "system/hardware/hw.h" -#include "tools/replay/util.h" - -std::string cacheFilePath(const std::string &url) { - static std::string cache_path = [] { - const std::string comma_cache = Path::download_cache_root(); - util::create_directories(comma_cache, 0755); - return comma_cache.back() == '/' ? comma_cache : comma_cache + "/"; - }(); - - return cache_path + sha256(getUrlWithoutQuery(url)); -} - -std::string FileReader::read(const std::string &file, std::atomic *abort) { - const bool is_remote = file.find("https://") == 0; - const std::string local_file = is_remote ? cacheFilePath(file) : file; - std::string result; - - if ((!is_remote || cache_to_local_) && util::file_exists(local_file)) { - result = util::read_file(local_file); - } else if (is_remote) { - result = download(file, abort); - if (cache_to_local_ && !result.empty()) { - std::ofstream fs(local_file, std::ios::binary | std::ios::out); - fs.write(result.data(), result.size()); - } - } - return result; -} - -std::string FileReader::download(const std::string &url, std::atomic *abort) { - for (int i = 0; i <= max_retries_ && !(abort && *abort); ++i) { - if (i > 0) rWarning("download failed, retrying %d", i); - - std::string result = httpGet(url, chunk_size_, abort); - if (!result.empty()) { - return result; - } - } - return {}; -} diff --git a/tools/replay/filereader.h b/tools/replay/filereader.h deleted file mode 100644 index 34aa91e858..0000000000 --- a/tools/replay/filereader.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once - -#include -#include - -class FileReader { -public: - FileReader(bool cache_to_local, size_t chunk_size = 0, int retries = 3) - : cache_to_local_(cache_to_local), chunk_size_(chunk_size), max_retries_(retries) {} - virtual ~FileReader() {} - std::string read(const std::string &file, std::atomic *abort = nullptr); - -private: - std::string download(const std::string &url, std::atomic *abort); - size_t chunk_size_; - int max_retries_; - bool cache_to_local_; -}; - -std::string cacheFilePath(const std::string &url); diff --git a/tools/replay/framereader.cc b/tools/replay/framereader.cc deleted file mode 100644 index 303f0c60ca..0000000000 --- a/tools/replay/framereader.cc +++ /dev/null @@ -1,251 +0,0 @@ -#include "tools/replay/framereader.h" -#include "tools/replay/util.h" - -#include -#include -#include "third_party/libyuv/include/libyuv.h" - -#ifdef __APPLE__ -#define HW_DEVICE_TYPE AV_HWDEVICE_TYPE_VIDEOTOOLBOX -#define HW_PIX_FMT AV_PIX_FMT_VIDEOTOOLBOX -#else -#define HW_DEVICE_TYPE AV_HWDEVICE_TYPE_CUDA -#define HW_PIX_FMT AV_PIX_FMT_CUDA -#endif - -namespace { - -struct buffer_data { - const uint8_t *data; - int64_t offset; - size_t size; -}; - -int readPacket(void *opaque, uint8_t *buf, int buf_size) { - struct buffer_data *bd = (struct buffer_data *)opaque; - assert(bd->offset <= bd->size); - buf_size = std::min((size_t)buf_size, (size_t)(bd->size - bd->offset)); - if (!buf_size) return AVERROR_EOF; - - memcpy(buf, bd->data + bd->offset, buf_size); - bd->offset += buf_size; - return buf_size; -} - -enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat *pix_fmts) { - enum AVPixelFormat *hw_pix_fmt = reinterpret_cast(ctx->opaque); - for (const enum AVPixelFormat *p = pix_fmts; *p != -1; p++) { - if (*p == *hw_pix_fmt) return *p; - } - rWarning("Please run replay with the --no-hw-decoder flag!"); - // fallback to YUV420p - *hw_pix_fmt = AV_PIX_FMT_NONE; - return AV_PIX_FMT_YUV420P; -} - -} // namespace - -FrameReader::FrameReader() { - av_log_set_level(AV_LOG_QUIET); -} - -FrameReader::~FrameReader() { - for (AVPacket *pkt : packets) { - av_packet_free(&pkt); - } - - if (decoder_ctx) avcodec_free_context(&decoder_ctx); - if (input_ctx) avformat_close_input(&input_ctx); - if (hw_device_ctx) av_buffer_unref(&hw_device_ctx); - - if (avio_ctx_) { - av_freep(&avio_ctx_->buffer); - avio_context_free(&avio_ctx_); - } -} - -bool FrameReader::load(const std::string &url, bool no_hw_decoder, std::atomic *abort, bool local_cache, int chunk_size, int retries) { - FileReader f(local_cache, chunk_size, retries); - std::string data = f.read(url, abort); - if (data.empty()) { - rWarning("URL %s returned no data", url.c_str()); - return false; - } - - return load((std::byte *)data.data(), data.size(), no_hw_decoder, abort); -} - -bool FrameReader::load(const std::byte *data, size_t size, bool no_hw_decoder, std::atomic *abort) { - input_ctx = avformat_alloc_context(); - if (!input_ctx) { - rError("Error calling avformat_alloc_context"); - return false; - } - - struct buffer_data bd = { - .data = (const uint8_t*)data, - .offset = 0, - .size = size, - }; - const int avio_ctx_buffer_size = 64 * 1024; - unsigned char *avio_ctx_buffer = (unsigned char *)av_malloc(avio_ctx_buffer_size); - avio_ctx_ = avio_alloc_context(avio_ctx_buffer, avio_ctx_buffer_size, 0, &bd, readPacket, nullptr, nullptr); - input_ctx->pb = avio_ctx_; - - input_ctx->probesize = 10 * 1024 * 1024; // 10MB - int ret = avformat_open_input(&input_ctx, nullptr, nullptr, nullptr); - if (ret != 0) { - char err_str[1024] = {0}; - av_strerror(ret, err_str, std::size(err_str)); - rError("Error loading video - %s", err_str); - return false; - } - - ret = avformat_find_stream_info(input_ctx, nullptr); - if (ret < 0) { - rError("cannot find a video stream in the input file"); - return false; - } - - AVStream *video = input_ctx->streams[0]; - const AVCodec *decoder = avcodec_find_decoder(video->codecpar->codec_id); - if (!decoder) return false; - - decoder_ctx = avcodec_alloc_context3(decoder); - ret = avcodec_parameters_to_context(decoder_ctx, video->codecpar); - if (ret != 0) return false; - - width = (decoder_ctx->width + 3) & ~3; - height = decoder_ctx->height; - - if (has_hw_decoder && !no_hw_decoder) { - if (!initHardwareDecoder(HW_DEVICE_TYPE)) { - rWarning("No device with hardware decoder found. fallback to CPU decoding."); - } - } - - ret = avcodec_open2(decoder_ctx, decoder, nullptr); - if (ret < 0) { - rError("avcodec_open2 failed %d", ret); - return false; - } - - packets.reserve(60 * 20); // 20fps, one minute - while (!(abort && *abort)) { - AVPacket *pkt = av_packet_alloc(); - ret = av_read_frame(input_ctx, pkt); - if (ret < 0) { - av_packet_free(&pkt); - valid_ = (ret == AVERROR_EOF); - break; - } - packets.push_back(pkt); - // some stream seems to contain no keyframes - key_frames_count_ += pkt->flags & AV_PKT_FLAG_KEY; - } - valid_ = valid_ && !packets.empty(); - return valid_; -} - -bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) { - for (int i = 0;; i++) { - const AVCodecHWConfig *config = avcodec_get_hw_config(decoder_ctx->codec, i); - if (!config) { - rWarning("decoder %s does not support hw device type %s.", decoder_ctx->codec->name, - av_hwdevice_get_type_name(hw_device_type)); - return false; - } - if (config->methods & AV_CODEC_HW_CONFIG_METHOD_HW_DEVICE_CTX && config->device_type == hw_device_type) { - hw_pix_fmt = config->pix_fmt; - break; - } - } - - int ret = av_hwdevice_ctx_create(&hw_device_ctx, hw_device_type, nullptr, nullptr, 0); - if (ret < 0) { - hw_pix_fmt = AV_PIX_FMT_NONE; - has_hw_decoder = false; - rWarning("Failed to create specified HW device %d.", ret); - return false; - } - - decoder_ctx->hw_device_ctx = av_buffer_ref(hw_device_ctx); - decoder_ctx->opaque = &hw_pix_fmt; - decoder_ctx->get_format = get_hw_format; - return true; -} - -bool FrameReader::get(int idx, VisionBuf *buf) { - assert(buf != nullptr); - if (!valid_ || idx < 0 || idx >= packets.size()) { - return false; - } - return decode(idx, buf); -} - -bool FrameReader::decode(int idx, VisionBuf *buf) { - int from_idx = idx; - if (idx != prev_idx + 1 && key_frames_count_ > 1) { - // seeking to the nearest key frame - for (int i = idx; i >= 0; --i) { - if (packets[i]->flags & AV_PKT_FLAG_KEY) { - from_idx = i; - break; - } - } - } - prev_idx = idx; - - for (int i = from_idx; i <= idx; ++i) { - AVFrame *f = decodeFrame(packets[i]); - if (f && i == idx) { - return copyBuffers(f, buf); - } - } - return false; -} - -AVFrame *FrameReader::decodeFrame(AVPacket *pkt) { - int ret = avcodec_send_packet(decoder_ctx, pkt); - if (ret < 0) { - rError("Error sending a packet for decoding: %d", ret); - return nullptr; - } - - av_frame_.reset(av_frame_alloc()); - ret = avcodec_receive_frame(decoder_ctx, av_frame_.get()); - if (ret != 0) { - rError("avcodec_receive_frame error: %d", ret); - return nullptr; - } - - if (av_frame_->format == hw_pix_fmt) { - hw_frame.reset(av_frame_alloc()); - if ((ret = av_hwframe_transfer_data(hw_frame.get(), av_frame_.get(), 0)) < 0) { - rError("error transferring the data from GPU to CPU"); - return nullptr; - } - return hw_frame.get(); - } else { - return av_frame_.get(); - } -} - -bool FrameReader::copyBuffers(AVFrame *f, VisionBuf *buf) { - assert(f != nullptr && buf != nullptr); - if (hw_pix_fmt == HW_PIX_FMT) { - for (int i = 0; i < height/2; i++) { - memcpy(buf->y + (i*2 + 0)*buf->stride, f->data[0] + (i*2 + 0)*f->linesize[0], width); - memcpy(buf->y + (i*2 + 1)*buf->stride, f->data[0] + (i*2 + 1)*f->linesize[0], width); - memcpy(buf->uv + i*buf->stride, f->data[1] + i*f->linesize[1], width); - } - } else { - libyuv::I420ToNV12(f->data[0], f->linesize[0], - f->data[1], f->linesize[1], - f->data[2], f->linesize[2], - buf->y, buf->stride, - buf->uv, buf->stride, - width, height); - } - return true; -} diff --git a/tools/replay/framereader.h b/tools/replay/framereader.h deleted file mode 100644 index bb72ac8f8d..0000000000 --- a/tools/replay/framereader.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "cereal/visionipc/visionbuf.h" -#include "tools/replay/filereader.h" - -extern "C" { -#include -#include -} - -struct AVFrameDeleter { - void operator()(AVFrame* frame) const { av_frame_free(&frame); } -}; - -class FrameReader { -public: - FrameReader(); - ~FrameReader(); - bool load(const std::string &url, bool no_hw_decoder = false, std::atomic *abort = nullptr, bool local_cache = false, - int chunk_size = -1, int retries = 0); - bool load(const std::byte *data, size_t size, bool no_hw_decoder = false, std::atomic *abort = nullptr); - bool get(int idx, VisionBuf *buf); - int getYUVSize() const { return width * height * 3 / 2; } - size_t getFrameCount() const { return packets.size(); } - bool valid() const { return valid_; } - - int width = 0, height = 0; - -private: - bool initHardwareDecoder(AVHWDeviceType hw_device_type); - bool decode(int idx, VisionBuf *buf); - AVFrame * decodeFrame(AVPacket *pkt); - bool copyBuffers(AVFrame *f, VisionBuf *buf); - - std::vector packets; - std::unique_ptrav_frame_, hw_frame; - AVFormatContext *input_ctx = nullptr; - AVCodecContext *decoder_ctx = nullptr; - int key_frames_count_ = 0; - bool valid_ = false; - AVIOContext *avio_ctx_ = nullptr; - - AVPixelFormat hw_pix_fmt = AV_PIX_FMT_NONE; - AVBufferRef *hw_device_ctx = nullptr; - int prev_idx = -1; - inline static std::atomic has_hw_decoder = true; -}; diff --git a/tools/replay/logreader.cc b/tools/replay/logreader.cc deleted file mode 100644 index c92ff4753f..0000000000 --- a/tools/replay/logreader.cc +++ /dev/null @@ -1,98 +0,0 @@ -#include "tools/replay/logreader.h" - -#include -#include "tools/replay/filereader.h" -#include "tools/replay/util.h" - -Event::Event(const kj::ArrayPtr &amsg, bool frame) : reader(amsg), frame(frame) { - words = kj::ArrayPtr(amsg.begin(), reader.getEnd()); - event = reader.getRoot(); - which = event.which(); - mono_time = event.getLogMonoTime(); - - // 1) Send video data at t=timestampEof/timestampSof - // 2) Send encodeIndex packet at t=logMonoTime - if (frame) { - auto idx = capnp::AnyStruct::Reader(event).getPointerSection()[0].getAs(); - // C2 only has eof set, and some older routes have neither - uint64_t sof = idx.getTimestampSof(); - uint64_t eof = idx.getTimestampEof(); - if (sof > 0) { - mono_time = sof; - } else if (eof > 0) { - mono_time = eof; - } - } -} - -// class LogReader - -LogReader::LogReader(size_t memory_pool_block_size) { -#ifdef HAS_MEMORY_RESOURCE - const size_t buf_size = sizeof(Event) * memory_pool_block_size; - mbr_ = std::make_unique(buf_size); -#endif - events.reserve(memory_pool_block_size); -} - -LogReader::~LogReader() { - for (Event *e : events) { - delete e; - } -} - -bool LogReader::load(const std::string &url, std::atomic *abort, bool local_cache, int chunk_size, int retries) { - raw_ = FileReader(local_cache, chunk_size, retries).read(url, abort); - if (raw_.empty()) return false; - - if (url.find(".bz2") != std::string::npos) { - raw_ = decompressBZ2(raw_, abort); - if (raw_.empty()) return false; - } - return parse(abort); -} - -bool LogReader::load(const std::byte *data, size_t size, std::atomic *abort) { - raw_.assign((const char *)data, size); - return parse(abort); -} - -bool LogReader::parse(std::atomic *abort) { - try { - kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word)); - while (words.size() > 0 && !(abort && *abort)) { -#ifdef HAS_MEMORY_RESOURCE - Event *evt = new (mbr_.get()) Event(words); -#else - Event *evt = new Event(words); -#endif - // Add encodeIdx packet again as a frame packet for the video stream - if (evt->which == cereal::Event::ROAD_ENCODE_IDX || - evt->which == cereal::Event::DRIVER_ENCODE_IDX || - evt->which == cereal::Event::WIDE_ROAD_ENCODE_IDX) { - -#ifdef HAS_MEMORY_RESOURCE - Event *frame_evt = new (mbr_.get()) Event(words, true); -#else - Event *frame_evt = new Event(words, true); -#endif - - events.push_back(frame_evt); - } - - words = kj::arrayPtr(evt->reader.getEnd(), words.end()); - events.push_back(evt); - } - } catch (const kj::Exception &e) { - rWarning("failed to parse log : %s", e.getDescription().cStr()); - if (!events.empty()) { - rWarning("read %zu events from corrupt log", events.size()); - } - } - - if (!events.empty() && !(abort && *abort)) { - std::sort(events.begin(), events.end(), Event::lessThan()); - return true; - } - return false; -} diff --git a/tools/replay/logreader.h b/tools/replay/logreader.h deleted file mode 100644 index 73f822d16c..0000000000 --- a/tools/replay/logreader.h +++ /dev/null @@ -1,67 +0,0 @@ -#pragma once - -#if __has_include() -#define HAS_MEMORY_RESOURCE 1 -#include -#endif - -#include -#include -#include - -#include "cereal/gen/cpp/log.capnp.h" -#include "system/camerad/cameras/camera_common.h" - -const CameraType ALL_CAMERAS[] = {RoadCam, DriverCam, WideRoadCam}; -const int MAX_CAMERAS = std::size(ALL_CAMERAS); -const int DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE = 65000; - -class Event { -public: - Event(cereal::Event::Which which, uint64_t mono_time) : reader(kj::ArrayPtr{}) { - // construct a dummy Event for binary search, e.g std::upper_bound - this->which = which; - this->mono_time = mono_time; - } - Event(const kj::ArrayPtr &amsg, bool frame = false); - inline kj::ArrayPtr bytes() const { return words.asBytes(); } - - struct lessThan { - inline bool operator()(const Event *l, const Event *r) { - return l->mono_time < r->mono_time || (l->mono_time == r->mono_time && l->which < r->which); - } - }; - -#if HAS_MEMORY_RESOURCE - void *operator new(size_t size, std::pmr::monotonic_buffer_resource *mbr) { - return mbr->allocate(size); - } - void operator delete(void *ptr) { - // No-op. memory used by EventMemoryPool increases monotonically until the logReader is destroyed. - } -#endif - - uint64_t mono_time; - cereal::Event::Which which; - cereal::Event::Reader event; - capnp::FlatArrayMessageReader reader; - kj::ArrayPtr words; - bool frame; -}; - -class LogReader { -public: - LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE); - ~LogReader(); - bool load(const std::string &url, std::atomic *abort = nullptr, - bool local_cache = false, int chunk_size = -1, int retries = 0); - bool load(const std::byte *data, size_t size, std::atomic *abort = nullptr); - std::vector events; - -private: - bool parse(std::atomic *abort); - std::string raw_; -#ifdef HAS_MEMORY_RESOURCE - std::unique_ptr mbr_; -#endif -}; diff --git a/tools/replay/main.cc b/tools/replay/main.cc deleted file mode 100644 index a0c072438d..0000000000 --- a/tools/replay/main.cc +++ /dev/null @@ -1,83 +0,0 @@ -#include -#include - -#include "common/prefix.h" -#include "tools/replay/consoleui.h" -#include "tools/replay/replay.h" - -int main(int argc, char *argv[]) { -#ifdef __APPLE__ - // With all sockets opened, we might hit the default limit of 256 on macOS - util::set_file_descriptor_limit(1024); -#endif - - QCoreApplication app(argc, argv); - - const std::tuple flags[] = { - {"dcam", REPLAY_FLAG_DCAM, "load driver camera"}, - {"ecam", REPLAY_FLAG_ECAM, "load wide road camera"}, - {"no-loop", REPLAY_FLAG_NO_LOOP, "stop at the end of the route"}, - {"no-cache", REPLAY_FLAG_NO_FILE_CACHE, "turn off local cache"}, - {"qcam", REPLAY_FLAG_QCAMERA, "load qcamera"}, - {"no-hw-decoder", REPLAY_FLAG_NO_HW_DECODER, "disable HW video decoding"}, - {"no-vipc", REPLAY_FLAG_NO_VIPC, "do not output video"}, - {"all", REPLAY_FLAG_ALL_SERVICES, "do output all messages including uiDebug, userFlag" - ". this may causes issues when used along with UI"} - }; - - QCommandLineParser parser; - parser.setApplicationDescription("Mock openpilot components by publishing logged messages."); - parser.addHelpOption(); - parser.addPositionalArgument("route", "the drive to replay. find your drives at connect.comma.ai"); - parser.addOption({{"a", "allow"}, "whitelist of services to send", "allow"}); - parser.addOption({{"b", "block"}, "blacklist of services to send", "block"}); - parser.addOption({{"c", "cache"}, "cache segments in memory. default is 5", "n"}); - parser.addOption({{"s", "start"}, "start from ", "seconds"}); - parser.addOption({"x", QString("playback . between %1 - %2") - .arg(ConsoleUI::speed_array.front()).arg(ConsoleUI::speed_array.back()), "speed"}); - parser.addOption({"demo", "use a demo route instead of providing your own"}); - parser.addOption({"data_dir", "local directory with routes", "data_dir"}); - parser.addOption({"prefix", "set OPENPILOT_PREFIX", "prefix"}); - for (auto &[name, _, desc] : flags) { - parser.addOption({name, desc}); - } - - parser.process(app); - const QStringList args = parser.positionalArguments(); - if (args.empty() && !parser.isSet("demo")) { - parser.showHelp(); - } - - const QString route = args.empty() ? DEMO_ROUTE : args.first(); - QStringList allow = parser.value("allow").isEmpty() ? QStringList{} : parser.value("allow").split(","); - QStringList block = parser.value("block").isEmpty() ? QStringList{} : parser.value("block").split(","); - - uint32_t replay_flags = REPLAY_FLAG_NONE; - for (const auto &[name, flag, _] : flags) { - if (parser.isSet(name)) { - replay_flags |= flag; - } - } - - std::unique_ptr op_prefix; - auto prefix = parser.value("prefix"); - if (!prefix.isEmpty()) { - op_prefix.reset(new OpenpilotPrefix(prefix.toStdString())); - } - - Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); - if (!parser.value("c").isEmpty()) { - replay->setSegmentCacheLimit(parser.value("c").toInt()); - } - if (!parser.value("x").isEmpty()) { - replay->setSpeed(std::clamp(parser.value("x").toFloat(), - ConsoleUI::speed_array.front(), ConsoleUI::speed_array.back())); - } - if (!replay->load()) { - return 0; - } - - ConsoleUI console_ui(replay); - replay->start(parser.value("start").toInt()); - return app.exec(); -} diff --git a/tools/replay/replay.cc b/tools/replay/replay.cc deleted file mode 100644 index 9657375be7..0000000000 --- a/tools/replay/replay.cc +++ /dev/null @@ -1,428 +0,0 @@ -#include "tools/replay/replay.h" - -#include -#include - -#include -#include "cereal/services.h" -#include "common/params.h" -#include "common/timing.h" -#include "tools/replay/util.h" - -Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *sm_, - uint32_t flags, QString data_dir, QObject *parent) : sm(sm_), flags_(flags), QObject(parent) { - if (!(flags_ & REPLAY_FLAG_ALL_SERVICES)) { - block << "uiDebug" << "userFlag"; - } - auto event_struct = capnp::Schema::from().asStruct(); - sockets_.resize(event_struct.getUnionFields().size()); - for (const auto &[name, _] : services) { - if (!block.contains(name.c_str()) && (allow.empty() || allow.contains(name.c_str()))) { - uint16_t which = event_struct.getFieldByName(name).getProto().getDiscriminantValue(); - sockets_[which] = name.c_str(); - } - } - - std::vector s; - std::copy_if(sockets_.begin(), sockets_.end(), std::back_inserter(s), - [](const char *name) { return name != nullptr; }); - qDebug() << "services " << s; - qDebug() << "loading route " << route; - - if (sm == nullptr) { - pm = std::make_unique(s); - } - route_ = std::make_unique(route, data_dir); - events_ = std::make_unique>(); - new_events_ = std::make_unique>(); -} - -Replay::~Replay() { - stop(); -} - -void Replay::stop() { - if (!stream_thread_ && segments_.empty()) return; - - rInfo("shutdown: in progress..."); - if (stream_thread_ != nullptr) { - exit_ = updating_events_ = true; - stream_cv_.notify_one(); - stream_thread_->quit(); - stream_thread_->wait(); - stream_thread_ = nullptr; - } - camera_server_.reset(nullptr); - timeline_future.waitForFinished(); - segments_.clear(); - rInfo("shutdown: done"); -} - -bool Replay::load() { - if (!route_->load()) { - qCritical() << "failed to load route" << route_->name() - << "from" << (route_->dir().isEmpty() ? "server" : route_->dir()); - return false; - } - - for (auto &[n, f] : route_->segments()) { - bool has_log = !f.rlog.isEmpty() || !f.qlog.isEmpty(); - bool has_video = !f.road_cam.isEmpty() || !f.qcamera.isEmpty(); - if (has_log && (has_video || hasFlag(REPLAY_FLAG_NO_VIPC))) { - segments_.insert({n, nullptr}); - } - } - if (segments_.empty()) { - qCritical() << "no valid segments in route" << route_->name(); - return false; - } - rInfo("load route %s with %zu valid segments", qPrintable(route_->name()), segments_.size()); - return true; -} - -void Replay::start(int seconds) { - seekTo(route_->identifier().segment_id * 60 + seconds, false); -} - -void Replay::updateEvents(const std::function &lambda) { - // set updating_events to true to force stream thread release the lock and wait for events_updated. - updating_events_ = true; - { - std::unique_lock lk(stream_lock_); - events_updated_ = lambda(); - updating_events_ = false; - } - stream_cv_.notify_one(); -} - -void Replay::seekTo(double seconds, bool relative) { - seconds = relative ? seconds + currentSeconds() : seconds; - updateEvents([&]() { - seconds = std::max(double(0.0), seconds); - int seg = (int)seconds / 60; - if (segments_.find(seg) == segments_.end()) { - rWarning("can't seek to %d s segment %d is invalid", seconds, seg); - return true; - } - - rInfo("seeking to %d s, segment %d", (int)seconds, seg); - current_segment_ = seg; - cur_mono_time_ = route_start_ts_ + seconds * 1e9; - emit seekedTo(seconds); - return isSegmentMerged(seg); - }); - queueSegment(); -} - -void Replay::seekToFlag(FindFlag flag) { - if (auto next = find(flag)) { - seekTo(*next - 2, false); // seek to 2 seconds before next - } -} - -void Replay::buildTimeline() { - uint64_t engaged_begin = 0; - bool engaged = false; - - auto alert_status = cereal::ControlsState::AlertStatus::NORMAL; - auto alert_size = cereal::ControlsState::AlertSize::NONE; - uint64_t alert_begin = 0; - std::string alert_type; - - const TimelineType timeline_types[] = { - [(int)cereal::ControlsState::AlertStatus::NORMAL] = TimelineType::AlertInfo, - [(int)cereal::ControlsState::AlertStatus::USER_PROMPT] = TimelineType::AlertWarning, - [(int)cereal::ControlsState::AlertStatus::CRITICAL] = TimelineType::AlertCritical, - }; - - const auto &route_segments = route_->segments(); - for (auto it = route_segments.cbegin(); it != route_segments.cend() && !exit_; ++it) { - std::shared_ptr log(new LogReader()); - if (!log->load(it->second.qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; - - for (const Event *e : log->events) { - if (e->which == cereal::Event::Which::CONTROLS_STATE) { - auto cs = e->event.getControlsState(); - - if (engaged != cs.getEnabled()) { - if (engaged) { - std::lock_guard lk(timeline_lock); - timeline.push_back({toSeconds(engaged_begin), toSeconds(e->mono_time), TimelineType::Engaged}); - } - engaged_begin = e->mono_time; - engaged = cs.getEnabled(); - } - - if (alert_type != cs.getAlertType().cStr() || alert_status != cs.getAlertStatus()) { - if (!alert_type.empty() && alert_size != cereal::ControlsState::AlertSize::NONE) { - std::lock_guard lk(timeline_lock); - timeline.push_back({toSeconds(alert_begin), toSeconds(e->mono_time), timeline_types[(int)alert_status]}); - } - alert_begin = e->mono_time; - alert_type = cs.getAlertType().cStr(); - alert_size = cs.getAlertSize(); - alert_status = cs.getAlertStatus(); - } - } else if (e->which == cereal::Event::Which::USER_FLAG) { - std::lock_guard lk(timeline_lock); - timeline.push_back({toSeconds(e->mono_time), toSeconds(e->mono_time), TimelineType::UserFlag}); - } - } - std::sort(timeline.begin(), timeline.end(), [](auto &l, auto &r) { return std::get<2>(l) < std::get<2>(r); }); - emit qLogLoaded(it->first, log); - } -} - -std::optional Replay::find(FindFlag flag) { - int cur_ts = currentSeconds(); - for (auto [start_ts, end_ts, type] : getTimeline()) { - if (type == TimelineType::Engaged) { - if (flag == FindFlag::nextEngagement && start_ts > cur_ts) { - return start_ts; - } else if (flag == FindFlag::nextDisEngagement && end_ts > cur_ts) { - return end_ts; - } - } else if (start_ts > cur_ts) { - if ((flag == FindFlag::nextUserFlag && type == TimelineType::UserFlag) || - (flag == FindFlag::nextInfo && type == TimelineType::AlertInfo) || - (flag == FindFlag::nextWarning && type == TimelineType::AlertWarning) || - (flag == FindFlag::nextCritical && type == TimelineType::AlertCritical)) { - return start_ts; - } - } - } - return std::nullopt; -} - -void Replay::pause(bool pause) { - updateEvents([=]() { - rWarning("%s at %.2f s", pause ? "paused..." : "resuming", currentSeconds()); - paused_ = pause; - return true; - }); -} - -void Replay::setCurrentSegment(int n) { - if (current_segment_.exchange(n) != n) { - QMetaObject::invokeMethod(this, &Replay::queueSegment, Qt::QueuedConnection); - } -} - -void Replay::segmentLoadFinished(bool success) { - if (!success) { - Segment *seg = qobject_cast(sender()); - rWarning("failed to load segment %d, removing it from current replay list", seg->seg_num); - updateEvents([&]() { - segments_.erase(seg->seg_num); - return true; - }); - } - queueSegment(); -} - -void Replay::queueSegment() { - auto cur = segments_.lower_bound(current_segment_.load()); - if (cur == segments_.end()) return; - - auto begin = std::prev(cur, std::min(segment_cache_limit / 2, std::distance(segments_.begin(), cur))); - auto end = std::next(begin, std::min(segment_cache_limit, segments_.size())); - // load one segment at a time - auto it = std::find_if(cur, end, [](auto &it) { return !it.second || !it.second->isLoaded(); }); - if (it != end && !it->second) { - rDebug("loading segment %d...", it->first); - it->second = std::make_unique(it->first, route_->at(it->first), flags_); - QObject::connect(it->second.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); - } - - mergeSegments(begin, end); - - // free segments out of current semgnt window. - std::for_each(segments_.begin(), begin, [](auto &e) { e.second.reset(nullptr); }); - std::for_each(end, segments_.end(), [](auto &e) { e.second.reset(nullptr); }); - - // start stream thread - const auto &cur_segment = cur->second; - if (stream_thread_ == nullptr && cur_segment->isLoaded()) { - startStream(cur_segment.get()); - emit streamStarted(); - } -} - -void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end) { - std::vector segments_need_merge; - size_t new_events_size = 0; - for (auto it = begin; it != end; ++it) { - if (it->second && it->second->isLoaded()) { - segments_need_merge.push_back(it->first); - new_events_size += it->second->log->events.size(); - } - } - - if (segments_need_merge != segments_merged_) { - std::string s; - for (int i = 0; i < segments_need_merge.size(); ++i) { - s += std::to_string(segments_need_merge[i]); - if (i != segments_need_merge.size() - 1) s += ", "; - } - rDebug("merge segments %s", s.c_str()); - new_events_->clear(); - new_events_->reserve(new_events_size); - for (int n : segments_need_merge) { - size_t size = new_events_->size(); - const auto &events = segments_[n]->log->events; - std::copy_if(events.begin(), events.end(), std::back_inserter(*new_events_), - [this](auto e) { return e->which < sockets_.size() && sockets_[e->which] != nullptr; }); - std::inplace_merge(new_events_->begin(), new_events_->begin() + size, new_events_->end(), Event::lessThan()); - } - - if (stream_thread_) { - emit segmentsMerged(); - } - updateEvents([&]() { - events_.swap(new_events_); - segments_merged_ = segments_need_merge; - // Do not wake up the stream thread if the current segment has not been merged. - return isSegmentMerged(current_segment_) || (segments_.count(current_segment_) == 0); - }); - } -} - -void Replay::startStream(const Segment *cur_segment) { - const auto &events = cur_segment->log->events; - - // each segment has an INIT_DATA - route_start_ts_ = events.front()->mono_time; - cur_mono_time_ += route_start_ts_ - 1; - - // write CarParams - auto it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; }); - if (it != events.end()) { - car_fingerprint_ = (*it)->event.getCarParams().getCarFingerprint(); - capnp::MallocMessageBuilder builder; - builder.setRoot((*it)->event.getCarParams()); - auto words = capnp::messageToFlatArray(builder); - auto bytes = words.asBytes(); - Params().put("CarParams", (const char *)bytes.begin(), bytes.size()); - Params().put("CarParamsPersistent", (const char *)bytes.begin(), bytes.size()); - } else { - rWarning("failed to read CarParams from current segment"); - } - - // start camera server - if (!hasFlag(REPLAY_FLAG_NO_VIPC)) { - std::pair camera_size[MAX_CAMERAS] = {}; - for (auto type : ALL_CAMERAS) { - if (auto &fr = cur_segment->frames[type]) { - camera_size[type] = {fr->width, fr->height}; - } - } - camera_server_ = std::make_unique(camera_size); - } - - emit segmentsMerged(); - // start stream thread - stream_thread_ = new QThread(); - QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); - QObject::connect(stream_thread_, &QThread::finished, stream_thread_, &QThread::deleteLater); - stream_thread_->start(); - - timeline_future = QtConcurrent::run(this, &Replay::buildTimeline); -} - -void Replay::publishMessage(const Event *e) { - if (event_filter && event_filter(e, filter_opaque)) return; - - if (sm == nullptr) { - auto bytes = e->bytes(); - int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); - if (ret == -1) { - rWarning("stop publishing %s due to multiple publishers error", sockets_[e->which]); - sockets_[e->which] = nullptr; - } - } else { - sm->update_msgs(nanos_since_boot(), {{sockets_[e->which], e->event}}); - } -} - -void Replay::publishFrame(const Event *e) { - static const std::map cam_types{ - {cereal::Event::ROAD_ENCODE_IDX, RoadCam}, - {cereal::Event::DRIVER_ENCODE_IDX, DriverCam}, - {cereal::Event::WIDE_ROAD_ENCODE_IDX, WideRoadCam}, - }; - if ((e->which == cereal::Event::DRIVER_ENCODE_IDX && !hasFlag(REPLAY_FLAG_DCAM)) || - (e->which == cereal::Event::WIDE_ROAD_ENCODE_IDX && !hasFlag(REPLAY_FLAG_ECAM))) { - return; - } - auto eidx = capnp::AnyStruct::Reader(e->event).getPointerSection()[0].getAs(); - if (eidx.getType() == cereal::EncodeIndex::Type::FULL_H_E_V_C && isSegmentMerged(eidx.getSegmentNum())) { - CameraType cam = cam_types.at(e->which); - camera_server_->pushFrame(cam, segments_[eidx.getSegmentNum()]->frames[cam].get(), eidx); - } -} - -void Replay::stream() { - cereal::Event::Which cur_which = cereal::Event::Which::INIT_DATA; - double prev_replay_speed = speed_; - std::unique_lock lk(stream_lock_); - - while (true) { - stream_cv_.wait(lk, [=]() { return exit_ || (events_updated_ && !paused_); }); - events_updated_ = false; - if (exit_) break; - - Event cur_event(cur_which, cur_mono_time_); - auto eit = std::upper_bound(events_->begin(), events_->end(), &cur_event, Event::lessThan()); - if (eit == events_->end()) { - rInfo("waiting for events..."); - continue; - } - - uint64_t evt_start_ts = cur_mono_time_; - uint64_t loop_start_ts = nanos_since_boot(); - - for (auto end = events_->end(); !updating_events_ && eit != end; ++eit) { - const Event *evt = (*eit); - cur_which = evt->which; - cur_mono_time_ = evt->mono_time; - setCurrentSegment(toSeconds(cur_mono_time_) / 60); - - if (sockets_[cur_which] != nullptr) { - // keep time - long etime = (cur_mono_time_ - evt_start_ts) / speed_; - long rtime = nanos_since_boot() - loop_start_ts; - long behind_ns = etime - rtime; - // if behind_ns is greater than 1 second, it means that an invalid segment is skipped by seeking/replaying - if (behind_ns >= 1 * 1e9 || speed_ != prev_replay_speed) { - // reset event start times - evt_start_ts = cur_mono_time_; - loop_start_ts = nanos_since_boot(); - prev_replay_speed = speed_; - } else if (behind_ns > 0) { - precise_nano_sleep(behind_ns); - } - - if (!evt->frame) { - publishMessage(evt); - } else if (camera_server_) { - if (speed_ > 1.0) { - camera_server_->waitForSent(); - } - publishFrame(evt); - } - } - } - // wait for frame to be sent before unlock.(frameReader may be deleted after unlock) - if (camera_server_) { - camera_server_->waitForSent(); - } - - if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) { - int last_segment = segments_.empty() ? 0 : segments_.rbegin()->first; - if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) { - rInfo("reaches the end of route, restart from beginning"); - QMetaObject::invokeMethod(this, std::bind(&Replay::seekTo, this, 0, false), Qt::QueuedConnection); - } - } - } -} diff --git a/tools/replay/replay.h b/tools/replay/replay.h deleted file mode 100644 index 1a74b69c3d..0000000000 --- a/tools/replay/replay.h +++ /dev/null @@ -1,146 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "tools/replay/camera.h" -#include "tools/replay/route.h" - -const QString DEMO_ROUTE = "a2a0ccea32023010|2023-07-27--13-01-19"; - -// one segment uses about 100M of memory -constexpr int MIN_SEGMENTS_CACHE = 5; - -enum REPLAY_FLAGS { - REPLAY_FLAG_NONE = 0x0000, - REPLAY_FLAG_DCAM = 0x0002, - REPLAY_FLAG_ECAM = 0x0004, - REPLAY_FLAG_NO_LOOP = 0x0010, - REPLAY_FLAG_NO_FILE_CACHE = 0x0020, - REPLAY_FLAG_QCAMERA = 0x0040, - REPLAY_FLAG_NO_HW_DECODER = 0x0100, - REPLAY_FLAG_NO_VIPC = 0x0400, - REPLAY_FLAG_ALL_SERVICES = 0x0800, -}; - -enum class FindFlag { - nextEngagement, - nextDisEngagement, - nextUserFlag, - nextInfo, - nextWarning, - nextCritical -}; - -enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical, UserFlag }; -typedef bool (*replayEventFilter)(const Event *, void *); -Q_DECLARE_METATYPE(std::shared_ptr); - -class Replay : public QObject { - Q_OBJECT - -public: - Replay(QString route, QStringList allow, QStringList block, SubMaster *sm = nullptr, - uint32_t flags = REPLAY_FLAG_NONE, QString data_dir = "", QObject *parent = 0); - ~Replay(); - bool load(); - void start(int seconds = 0); - void stop(); - void pause(bool pause); - void seekToFlag(FindFlag flag); - void seekTo(double seconds, bool relative); - inline bool isPaused() const { return paused_; } - // the filter is called in streaming thread.try to return quickly from it to avoid blocking streaming. - // the filter function must return true if the event should be filtered. - // otherwise it must return false. - inline void installEventFilter(replayEventFilter filter, void *opaque) { - filter_opaque = opaque; - event_filter = filter; - } - inline int segmentCacheLimit() const { return segment_cache_limit; } - inline void setSegmentCacheLimit(int n) { segment_cache_limit = std::max(MIN_SEGMENTS_CACHE, n); } - inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } - inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } - inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } - inline const Route* route() const { return route_.get(); } - inline double currentSeconds() const { return double(cur_mono_time_ - route_start_ts_) / 1e9; } - inline QDateTime currentDateTime() const { return route_->datetime().addSecs(currentSeconds()); } - inline uint64_t routeStartTime() const { return route_start_ts_; } - inline double toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } - inline int totalSeconds() const { return (!segments_.empty()) ? (segments_.rbegin()->first + 1) * 60 : 0; } - inline void setSpeed(float speed) { speed_ = speed; } - inline float getSpeed() const { return speed_; } - inline const std::vector *events() const { return events_.get(); } - inline const std::map> &segments() const { return segments_; } - inline const std::string &carFingerprint() const { return car_fingerprint_; } - inline const std::vector> getTimeline() { - std::lock_guard lk(timeline_lock); - return timeline; - } - -signals: - void streamStarted(); - void segmentsMerged(); - void seekedTo(double sec); - void qLogLoaded(int segnum, std::shared_ptr qlog); - -protected slots: - void segmentLoadFinished(bool success); - -protected: - typedef std::map> SegmentMap; - std::optional find(FindFlag flag); - void startStream(const Segment *cur_segment); - void stream(); - void setCurrentSegment(int n); - void queueSegment(); - void mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end); - void updateEvents(const std::function& lambda); - void publishMessage(const Event *e); - void publishFrame(const Event *e); - void buildTimeline(); - inline bool isSegmentMerged(int n) { - return std::find(segments_merged_.begin(), segments_merged_.end(), n) != segments_merged_.end(); - } - - QThread *stream_thread_ = nullptr; - std::mutex stream_lock_; - std::condition_variable stream_cv_; - std::atomic updating_events_ = false; - std::atomic current_segment_ = 0; - SegmentMap segments_; - // the following variables must be protected with stream_lock_ - std::atomic exit_ = false; - bool paused_ = false; - bool events_updated_ = false; - uint64_t route_start_ts_ = 0; - std::atomic cur_mono_time_ = 0; - std::unique_ptr> events_; - std::unique_ptr> new_events_; - std::vector segments_merged_; - - // messaging - SubMaster *sm = nullptr; - std::unique_ptr pm; - std::vector sockets_; - std::unique_ptr route_; - std::unique_ptr camera_server_; - std::atomic flags_ = REPLAY_FLAG_NONE; - - std::mutex timeline_lock; - QFuture timeline_future; - std::vector> timeline; - std::string car_fingerprint_; - std::atomic speed_ = 1.0; - replayEventFilter event_filter = nullptr; - void *filter_opaque = nullptr; - int segment_cache_limit = MIN_SEGMENTS_CACHE; -}; diff --git a/tools/replay/route.cc b/tools/replay/route.cc deleted file mode 100644 index d0ddf7f3c8..0000000000 --- a/tools/replay/route.cc +++ /dev/null @@ -1,144 +0,0 @@ -#include "tools/replay/route.h" - -#include -#include -#include -#include -#include -#include -#include - -#include "selfdrive/ui/qt/api.h" -#include "system/hardware/hw.h" -#include "tools/replay/replay.h" -#include "tools/replay/util.h" - -Route::Route(const QString &route, const QString &data_dir) : data_dir_(data_dir) { - route_ = parseRoute(route); -} - -RouteIdentifier Route::parseRoute(const QString &str) { - QRegExp rx(R"(^(?:([a-z0-9]{16})([|_/]))?(\d{4}-\d{2}-\d{2}--\d{2}-\d{2}-\d{2})(?:(--|/)(\d*))?$)"); - if (rx.indexIn(str) == -1) return {}; - - const QStringList list = rx.capturedTexts(); - return {.dongle_id = list[1], .timestamp = list[3], .segment_id = list[5].toInt(), .str = list[1] + "|" + list[3]}; -} - -bool Route::load() { - if (route_.str.isEmpty() || (data_dir_.isEmpty() && route_.dongle_id.isEmpty())) { - rInfo("invalid route format"); - return false; - } - date_time_ = QDateTime::fromString(route_.timestamp, "yyyy-MM-dd--HH-mm-ss"); - return data_dir_.isEmpty() ? loadFromServer() : loadFromLocal(); -} - -bool Route::loadFromServer() { - QEventLoop loop; - HttpRequest http(nullptr, !Hardware::PC()); - QObject::connect(&http, &HttpRequest::requestDone, [&](const QString &json, bool success, QNetworkReply::NetworkError error) { - if (error == QNetworkReply::ContentAccessDenied || error == QNetworkReply::AuthenticationRequiredError) { - qWarning() << ">> Unauthorized. Authenticate with tools/lib/auth.py <<"; - } - - loop.exit(success ? loadFromJson(json) : 0); - }); - http.sendRequest(CommaApi::BASE_URL + "/v1/route/" + route_.str + "/files"); - return loop.exec(); -} - -bool Route::loadFromJson(const QString &json) { - QRegExp rx(R"(\/(\d+)\/)"); - for (const auto &value : QJsonDocument::fromJson(json.trimmed().toUtf8()).object()) { - for (const auto &url : value.toArray()) { - QString url_str = url.toString(); - if (rx.indexIn(url_str) != -1) { - addFileToSegment(rx.cap(1).toInt(), url_str); - } - } - } - return !segments_.empty(); -} - -bool Route::loadFromLocal() { - QDir log_dir(data_dir_); - for (const auto &folder : log_dir.entryList(QDir::Dirs | QDir::NoDot | QDir::NoDotDot, QDir::NoSort)) { - int pos = folder.lastIndexOf("--"); - if (pos != -1 && folder.left(pos) == route_.timestamp) { - const int seg_num = folder.mid(pos + 2).toInt(); - QDir segment_dir(log_dir.filePath(folder)); - for (const auto &f : segment_dir.entryList(QDir::Files)) { - addFileToSegment(seg_num, segment_dir.absoluteFilePath(f)); - } - } - } - return !segments_.empty(); -} - -void Route::addFileToSegment(int n, const QString &file) { - QString name = QUrl(file).fileName(); - - const int pos = name.lastIndexOf("--"); - name = pos != -1 ? name.mid(pos + 2) : name; - - if (name == "rlog.bz2" || name == "rlog") { - segments_[n].rlog = file; - } else if (name == "qlog.bz2" || name == "qlog") { - segments_[n].qlog = file; - } else if (name == "fcamera.hevc") { - segments_[n].road_cam = file; - } else if (name == "dcamera.hevc") { - segments_[n].driver_cam = file; - } else if (name == "ecamera.hevc") { - segments_[n].wide_road_cam = file; - } else if (name == "qcamera.ts") { - segments_[n].qcamera = file; - } -} - -// class Segment - -Segment::Segment(int n, const SegmentFile &files, uint32_t flags) : seg_num(n), flags(flags) { - // [RoadCam, DriverCam, WideRoadCam, log]. fallback to qcamera/qlog - const std::array file_list = { - (flags & REPLAY_FLAG_QCAMERA) || files.road_cam.isEmpty() ? files.qcamera : files.road_cam, - flags & REPLAY_FLAG_DCAM ? files.driver_cam : "", - flags & REPLAY_FLAG_ECAM ? files.wide_road_cam : "", - files.rlog.isEmpty() ? files.qlog : files.rlog, - }; - for (int i = 0; i < file_list.size(); ++i) { - if (!file_list[i].isEmpty() && (!(flags & REPLAY_FLAG_NO_VIPC) || i >= MAX_CAMERAS)) { - ++loading_; - synchronizer_.addFuture(QtConcurrent::run(this, &Segment::loadFile, i, file_list[i].toStdString())); - } - } -} - -Segment::~Segment() { - disconnect(); - abort_ = true; - synchronizer_.setCancelOnWait(true); - synchronizer_.waitForFinished(); -} - -void Segment::loadFile(int id, const std::string file) { - const bool local_cache = !(flags & REPLAY_FLAG_NO_FILE_CACHE); - bool success = false; - if (id < MAX_CAMERAS) { - frames[id] = std::make_unique(); - success = frames[id]->load(file, flags & REPLAY_FLAG_NO_HW_DECODER, &abort_, local_cache, 20 * 1024 * 1024, 3); - } else { - log = std::make_unique(); - success = log->load(file, &abort_, local_cache, 0, 3); - } - - if (!success) { - // abort all loading jobs. - abort_ = true; - } - - if (--loading_ == 0) { - emit loadFinished(!abort_); - } -} diff --git a/tools/replay/route.h b/tools/replay/route.h deleted file mode 100644 index 4dad7a1f37..0000000000 --- a/tools/replay/route.h +++ /dev/null @@ -1,75 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include - -#include "tools/replay/framereader.h" -#include "tools/replay/logreader.h" -#include "tools/replay/util.h" - -struct RouteIdentifier { - QString dongle_id; - QString timestamp; - int segment_id; - QString str; -}; - -struct SegmentFile { - QString rlog; - QString qlog; - QString road_cam; - QString driver_cam; - QString wide_road_cam; - QString qcamera; -}; - -class Route { -public: - Route(const QString &route, const QString &data_dir = {}); - bool load(); - inline const QString &name() const { return route_.str; } - inline const QDateTime datetime() const { return date_time_; } - inline const QString &dir() const { return data_dir_; } - inline const RouteIdentifier &identifier() const { return route_; } - inline const std::map &segments() const { return segments_; } - inline const SegmentFile &at(int n) { return segments_.at(n); } - static RouteIdentifier parseRoute(const QString &str); - -protected: - bool loadFromLocal(); - bool loadFromServer(); - bool loadFromJson(const QString &json); - void addFileToSegment(int seg_num, const QString &file); - RouteIdentifier route_ = {}; - QString data_dir_; - std::map segments_; - QDateTime date_time_; -}; - -class Segment : public QObject { - Q_OBJECT - -public: - Segment(int n, const SegmentFile &files, uint32_t flags); - ~Segment(); - inline bool isLoaded() const { return !loading_ && !abort_; } - - const int seg_num = 0; - std::unique_ptr log; - std::unique_ptr frames[MAX_CAMERAS] = {}; - -signals: - void loadFinished(bool success); - -protected: - void loadFile(int id, const std::string file); - - std::atomic abort_ = false; - std::atomic loading_ = 0; - QFutureSynchronizer synchronizer_; - uint32_t flags; -}; diff --git a/tools/replay/util.cc b/tools/replay/util.cc deleted file mode 100644 index b4f72d0530..0000000000 --- a/tools/replay/util.cc +++ /dev/null @@ -1,331 +0,0 @@ -#include "tools/replay/util.h" - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "common/timing.h" -#include "common/util.h" - -ReplayMessageHandler message_handler = nullptr; -void installMessageHandler(ReplayMessageHandler handler) { message_handler = handler; } - -void logMessage(ReplyMsgType type, const char *fmt, ...) { - static std::mutex lock; - std::lock_guard lk(lock); - - char *msg_buf = nullptr; - va_list args; - va_start(args, fmt); - int ret = vasprintf(&msg_buf, fmt, args); - va_end(args); - if (ret <= 0 || !msg_buf) return; - - if (message_handler) { - message_handler(type, msg_buf); - } else { - if (type == ReplyMsgType::Debug) { - std::cout << "\033[38;5;248m" << msg_buf << "\033[00m" << std::endl; - } else if (type == ReplyMsgType::Warning) { - std::cout << "\033[38;5;227m" << msg_buf << "\033[00m" << std::endl; - } else if (type == ReplyMsgType::Critical) { - std::cout << "\033[38;5;196m" << msg_buf << "\033[00m" << std::endl; - } else { - std::cout << msg_buf << std::endl; - } - } - - free(msg_buf); -} - -namespace { - -struct CURLGlobalInitializer { - CURLGlobalInitializer() { curl_global_init(CURL_GLOBAL_DEFAULT); } - ~CURLGlobalInitializer() { curl_global_cleanup(); } -}; - -static CURLGlobalInitializer curl_initializer; - -template -struct MultiPartWriter { - T *buf; - size_t *total_written; - size_t offset; - size_t end; - - size_t write(char *data, size_t size, size_t count) { - size_t bytes = size * count; - if ((offset + bytes) > end) return 0; - - if constexpr (std::is_same::value) { - memcpy(buf->data() + offset, data, bytes); - } else if constexpr (std::is_same::value) { - buf->seekp(offset); - buf->write(data, bytes); - } - - offset += bytes; - *total_written += bytes; - return bytes; - } -}; - -template -size_t write_cb(char *data, size_t size, size_t count, void *userp) { - auto w = (MultiPartWriter *)userp; - return w->write(data, size, count); -} - -size_t dumy_write_cb(char *data, size_t size, size_t count, void *userp) { return size * count; } - -struct DownloadStats { - void installDownloadProgressHandler(DownloadProgressHandler handler) { - std::lock_guard lk(lock); - download_progress_handler = handler; - } - - void add(const std::string &url, uint64_t total_bytes) { - std::lock_guard lk(lock); - items[url] = {0, total_bytes}; - } - - void remove(const std::string &url) { - std::lock_guard lk(lock); - items.erase(url); - } - - void update(const std::string &url, uint64_t downloaded, bool success = true) { - std::lock_guard lk(lock); - items[url].first = downloaded; - - auto stat = std::accumulate(items.begin(), items.end(), std::pair{}, [=](auto &a, auto &b){ - return std::pair{a.first + b.second.first, a.second + b.second.second}; - }); - double tm = millis_since_boot(); - if (download_progress_handler && ((tm - prev_tm) > 500 || !success || stat.first >= stat.second)) { - download_progress_handler(stat.first, stat.second, success); - prev_tm = tm; - } - } - - std::mutex lock; - std::map> items; - double prev_tm = 0; - DownloadProgressHandler download_progress_handler = nullptr; -}; - -static DownloadStats download_stats; - -} // namespace - -void installDownloadProgressHandler(DownloadProgressHandler handler) { - download_stats.installDownloadProgressHandler(handler); -} - -std::string formattedDataSize(size_t size) { - if (size < 1024) { - return std::to_string(size) + " B"; - } else if (size < 1024 * 1024) { - return util::string_format("%.2f KB", (float)size / 1024); - } else { - return util::string_format("%.2f MB", (float)size / (1024 * 1024)); - } -} - -size_t getRemoteFileSize(const std::string &url, std::atomic *abort) { - CURL *curl = curl_easy_init(); - if (!curl) return -1; - - curl_easy_setopt(curl, CURLOPT_URL, url.c_str()); - curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, dumy_write_cb); - curl_easy_setopt(curl, CURLOPT_HEADER, 1); - curl_easy_setopt(curl, CURLOPT_NOBODY, 1); - - CURLM *cm = curl_multi_init(); - curl_multi_add_handle(cm, curl); - int still_running = 1; - while (still_running > 0 && !(abort && *abort)) { - CURLMcode mc = curl_multi_perform(cm, &still_running); - if (!mc) curl_multi_wait(cm, nullptr, 0, 1000, nullptr); - } - - double content_length = -1; - curl_easy_getinfo(curl, CURLINFO_CONTENT_LENGTH_DOWNLOAD, &content_length); - curl_multi_remove_handle(cm, curl); - curl_easy_cleanup(curl); - curl_multi_cleanup(cm); - return content_length > 0 ? (size_t)content_length : 0; -} - -std::string getUrlWithoutQuery(const std::string &url) { - size_t idx = url.find("?"); - return (idx == std::string::npos ? url : url.substr(0, idx)); -} - -template -bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t content_length, std::atomic *abort) { - download_stats.add(url, content_length); - - int parts = 1; - if (chunk_size > 0 && content_length > 10 * 1024 * 1024) { - parts = std::nearbyint(content_length / (float)chunk_size); - parts = std::clamp(parts, 1, 5); - } - - CURLM *cm = curl_multi_init(); - size_t written = 0; - std::map> writers; - const int part_size = content_length / parts; - for (int i = 0; i < parts; ++i) { - CURL *eh = curl_easy_init(); - writers[eh] = { - .buf = &buf, - .total_written = &written, - .offset = (size_t)(i * part_size), - .end = i == parts - 1 ? content_length : (i + 1) * part_size, - }; - curl_easy_setopt(eh, CURLOPT_WRITEFUNCTION, write_cb); - curl_easy_setopt(eh, CURLOPT_WRITEDATA, (void *)(&writers[eh])); - curl_easy_setopt(eh, CURLOPT_URL, url.c_str()); - curl_easy_setopt(eh, CURLOPT_RANGE, util::string_format("%d-%d", writers[eh].offset, writers[eh].end - 1).c_str()); - curl_easy_setopt(eh, CURLOPT_HTTPGET, 1); - curl_easy_setopt(eh, CURLOPT_NOSIGNAL, 1); - curl_easy_setopt(eh, CURLOPT_FOLLOWLOCATION, 1); - - curl_multi_add_handle(cm, eh); - } - - int still_running = 1; - while (still_running > 0 && !(abort && *abort)) { - curl_multi_wait(cm, nullptr, 0, 1000, nullptr); - curl_multi_perform(cm, &still_running); - download_stats.update(url, written); - } - - CURLMsg *msg; - int msgs_left = -1; - int complete = 0; - while ((msg = curl_multi_info_read(cm, &msgs_left)) && !(abort && *abort)) { - if (msg->msg == CURLMSG_DONE) { - if (msg->data.result == CURLE_OK) { - long res_status = 0; - curl_easy_getinfo(msg->easy_handle, CURLINFO_RESPONSE_CODE, &res_status); - if (res_status == 206) { - complete++; - } else { - rWarning("Download failed: http error code: %d", res_status); - } - } else { - rWarning("Download failed: connection failure: %d", msg->data.result); - } - } - } - - bool success = complete == parts; - download_stats.update(url, written, success); - download_stats.remove(url); - - for (const auto &[e, w] : writers) { - curl_multi_remove_handle(cm, e); - curl_easy_cleanup(e); - } - curl_multi_cleanup(cm); - - return success; -} - -std::string httpGet(const std::string &url, size_t chunk_size, std::atomic *abort) { - size_t size = getRemoteFileSize(url, abort); - if (size == 0) return {}; - - std::string result(size, '\0'); - return httpDownload(url, result, chunk_size, size, abort) ? result : ""; -} - -bool httpDownload(const std::string &url, const std::string &file, size_t chunk_size, std::atomic *abort) { - size_t size = getRemoteFileSize(url, abort); - if (size == 0) return false; - - std::ofstream of(file, std::ios::binary | std::ios::out); - of.seekp(size - 1).write("\0", 1); - return httpDownload(url, of, chunk_size, size, abort); -} - -std::string decompressBZ2(const std::string &in, std::atomic *abort) { - return decompressBZ2((std::byte *)in.data(), in.size(), abort); -} - -std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic *abort) { - if (in_size == 0) return {}; - - bz_stream strm = {}; - int bzerror = BZ2_bzDecompressInit(&strm, 0, 0); - assert(bzerror == BZ_OK); - - strm.next_in = (char *)in; - strm.avail_in = in_size; - std::string out(in_size * 5, '\0'); - do { - strm.next_out = (char *)(&out[strm.total_out_lo32]); - strm.avail_out = out.size() - strm.total_out_lo32; - - const char *prev_write_pos = strm.next_out; - bzerror = BZ2_bzDecompress(&strm); - if (bzerror == BZ_OK && prev_write_pos == strm.next_out) { - // content is corrupt - bzerror = BZ_STREAM_END; - rWarning("decompressBZ2 error : content is corrupt"); - break; - } - - if (bzerror == BZ_OK && strm.avail_in > 0 && strm.avail_out == 0) { - out.resize(out.size() * 2); - } - } while (bzerror == BZ_OK && !(abort && *abort)); - - BZ2_bzDecompressEnd(&strm); - if (bzerror == BZ_STREAM_END && !(abort && *abort)) { - out.resize(strm.total_out_lo32); - return out; - } - return {}; -} - -void precise_nano_sleep(long sleep_ns) { - const long estimate_ns = 1 * 1e6; // 1ms - struct timespec req = {.tv_nsec = estimate_ns}; - uint64_t start_sleep = nanos_since_boot(); - while (sleep_ns > estimate_ns) { - nanosleep(&req, nullptr); - uint64_t end_sleep = nanos_since_boot(); - sleep_ns -= (end_sleep - start_sleep); - start_sleep = end_sleep; - } - // spin wait - if (sleep_ns > 0) { - while ((nanos_since_boot() - start_sleep) <= sleep_ns) { - std::this_thread::yield(); - } - } -} - -std::string sha256(const std::string &str) { - unsigned char hash[SHA256_DIGEST_LENGTH]; - SHA256_CTX sha256; - SHA256_Init(&sha256); - SHA256_Update(&sha256, str.c_str(), str.size()); - SHA256_Final(hash, &sha256); - return util::hexdump(hash, SHA256_DIGEST_LENGTH); -} diff --git a/tools/replay/util.h b/tools/replay/util.h deleted file mode 100644 index 6c808095e8..0000000000 --- a/tools/replay/util.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include -#include -#include - -enum class ReplyMsgType { - Info, - Debug, - Warning, - Critical -}; - -typedef std::function ReplayMessageHandler; -void installMessageHandler(ReplayMessageHandler); -void logMessage(ReplyMsgType type, const char* fmt, ...); - -#define rInfo(fmt, ...) ::logMessage(ReplyMsgType::Info, fmt, ## __VA_ARGS__) -#define rDebug(fmt, ...) ::logMessage(ReplyMsgType::Debug, fmt, ## __VA_ARGS__) -#define rWarning(fmt, ...) ::logMessage(ReplyMsgType::Warning, fmt, ## __VA_ARGS__) -#define rError(fmt, ...) ::logMessage(ReplyMsgType::Critical , fmt, ## __VA_ARGS__) - -std::string sha256(const std::string &str); -void precise_nano_sleep(long sleep_ns); -std::string decompressBZ2(const std::string &in, std::atomic *abort = nullptr); -std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic *abort = nullptr); -std::string getUrlWithoutQuery(const std::string &url); -size_t getRemoteFileSize(const std::string &url, std::atomic *abort = nullptr); -std::string httpGet(const std::string &url, size_t chunk_size = 0, std::atomic *abort = nullptr); - -typedef std::function DownloadProgressHandler; -void installDownloadProgressHandler(DownloadProgressHandler); -bool httpDownload(const std::string &url, const std::string &file, size_t chunk_size = 0, std::atomic *abort = nullptr); -std::string formattedDataSize(size_t size);