pull/24762/head
ZwX1616 3 years ago
commit 79623f1da0
  1. 6
      .github/workflows/selfdrive_tests.yaml
  2. 1
      .pre-commit-config.yaml
  3. 2
      Pipfile
  4. 33
      Pipfile.lock
  5. 2
      cereal
  6. 6
      common/realtime.py
  7. 7
      common/xattr.py
  8. 17
      docs/CARS.md
  9. 2
      opendbc
  10. 2
      panda
  11. 15
      release/files_common
  12. 6
      scripts/count_cars.py
  13. 2
      scripts/waste.py
  14. 2
      selfdrive/athena/athenad.py
  15. 4
      selfdrive/athena/registration.py
  16. 2
      selfdrive/boardd/boardd.cc
  17. 6
      selfdrive/boardd/pandad.py
  18. 129
      selfdrive/camerad/cameras/real_debayer.cl
  19. 3
      selfdrive/car/__init__.py
  20. 4
      selfdrive/car/body/values.py
  21. 5
      selfdrive/car/chrysler/values.py
  22. 26
      selfdrive/car/docs.py
  23. 39
      selfdrive/car/docs_definitions.py
  24. 25
      selfdrive/car/gm/values.py
  25. 8
      selfdrive/car/honda/carcontroller.py
  26. 44
      selfdrive/car/honda/values.py
  27. 4
      selfdrive/car/hyundai/interface.py
  28. 93
      selfdrive/car/hyundai/values.py
  29. 4
      selfdrive/car/mazda/values.py
  30. 6
      selfdrive/car/nissan/values.py
  31. 6
      selfdrive/car/subaru/values.py
  32. 9
      selfdrive/car/tests/test_docs.py
  33. 10
      selfdrive/car/tests/test_models.py
  34. 2
      selfdrive/car/toyota/interface.py
  35. 3
      selfdrive/car/toyota/values.py
  36. 26
      selfdrive/car/volkswagen/values.py
  37. 1
      selfdrive/common/params.cc
  38. 6
      selfdrive/controls/controlsd.py
  39. 2
      selfdrive/controls/lib/events.py
  40. 28
      selfdrive/controls/lib/vehicle_model.py
  41. 81
      selfdrive/debug/internal/measure_steering_accuracy.py
  42. 2
      selfdrive/debug/live_cpu_and_temp.py
  43. 23
      selfdrive/debug/profiling/ftrace.sh
  44. 2
      selfdrive/debug/run_process_on_route.py
  45. 18
      selfdrive/hardware/tici/agnos.py
  46. 2
      selfdrive/locationd/calibrationd.py
  47. 2
      selfdrive/locationd/test/test_locationd.py
  48. 9
      selfdrive/loggerd/SConscript
  49. 13
      selfdrive/loggerd/encoder.h
  50. 79
      selfdrive/loggerd/encoder/encoder.cc
  51. 60
      selfdrive/loggerd/encoder/encoder.h
  52. 58
      selfdrive/loggerd/encoder/ffmpeg_encoder.cc
  53. 38
      selfdrive/loggerd/encoder/ffmpeg_encoder.h
  54. 94
      selfdrive/loggerd/encoder/v4l_encoder.cc
  55. 26
      selfdrive/loggerd/encoder/v4l_encoder.h
  56. 32
      selfdrive/loggerd/encoder/video_writer.cc
  57. 7
      selfdrive/loggerd/encoder/video_writer.h
  58. 6
      selfdrive/loggerd/encoderd.cc
  59. 16
      selfdrive/loggerd/loggerd.cc
  60. 8
      selfdrive/loggerd/loggerd.h
  61. 41
      selfdrive/loggerd/raw_logger.h
  62. 30
      selfdrive/loggerd/remote_encoder.cc
  63. 3
      selfdrive/loggerd/remote_encoder.h
  64. 4
      selfdrive/loggerd/uploader.py
  65. 2
      selfdrive/loggerd/xattr_cache.py
  66. 4
      selfdrive/manager/process_config.py
  67. 4
      selfdrive/modeld/thneed/lib.py
  68. 2
      selfdrive/sensord/rawgps/compare.py
  69. 6
      selfdrive/test/openpilotci.py
  70. 28
      selfdrive/test/process_replay/README.md
  71. 2
      selfdrive/test/process_replay/debayer_replay_ref_commit
  72. 4
      selfdrive/test/process_replay/model_replay.py
  73. 1
      selfdrive/test/process_replay/process_replay.py
  74. 87
      selfdrive/test/process_replay/test_processes.py
  75. 38
      selfdrive/test/process_replay/update_refs.py
  76. 8
      selfdrive/ui/qt/widgets/cameraview.cc
  77. 8
      selfdrive/ui/replay/camera.cc
  78. 4
      selfdrive/ui/replay/camera.h
  79. 4
      selfdrive/ui/replay/replay.cc
  80. 6
      selfdrive/ui/replay/route.cc
  81. 2
      selfdrive/ui/replay/util.cc
  82. 2
      selfdrive/updated.py
  83. 6
      selfdrive/version.py
  84. 2
      tools/camerastream/compressed_vipc.py
  85. 13
      tools/joystick/web.py
  86. 4
      tools/lib/auth.py
  87. 4
      tools/lib/bootlog.py
  88. 47
      tools/plotjuggler/layouts/thermal_debug.xml
  89. 4
      tools/replay/unlog_ci_segment.py
  90. 8
      tools/sim/bridge.py
  91. 131
      tools/tuning/measure_steering_accuracy.py

@ -8,6 +8,7 @@ on:
env:
BASE_IMAGE: openpilot-base
DOCKER_REGISTRY: ghcr.io/commaai
AZURE_TOKEN: ${{ secrets.AZURE_COMMADATACI_OPENPILOTCI_TOKEN }}
DOCKER_LOGIN: docker login ghcr.io -u adeebshihadeh -p ${{ secrets.CONTAINER_TOKEN }}
BUILD: |
@ -316,6 +317,11 @@ jobs:
${{ env.RUN }} "scons -j$(nproc) && \
FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \
coverage xml"
- name: Upload reference logs
if: ${{ failure() && github.event_name == 'pull_request' && github.repository == 'commaai/openpilot' && env.AZURE_TOKEN != '' }}
run: |
${{ env.RUN }} "scons -j$(nproc) && \
CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py --upload-only"
- name: "Upload coverage to Codecov"
uses: codecov/codecov-action@v2
- name: Print diff

@ -23,6 +23,7 @@ repos:
additional_dependencies: ['lxml', 'numpy', 'types-atomicwrites', 'types-pycurl', 'types-requests', 'types-certifi']
args:
- --warn-redundant-casts
- --warn-return-any
- --warn-unreachable
- --warn-unused-ignores
#- --html-report=/home/batman/openpilot

@ -38,7 +38,7 @@ breathe = "*"
subprocess32 = "*"
tenacity = "*"
mpld3 = "*"
carla = "==0.9.12"
carla = {version = "==0.9.12", markers="platform_system != 'Darwin'"}
[packages]
atomicwrites = "*"

33
Pipfile.lock generated

@ -1,7 +1,7 @@
{
"_meta": {
"hash": {
"sha256": "19a7b58f24cd7542ccb9fd386c7716d77fff3c1f87de496f3f42753cf34a5dde"
"sha256": "72117588efb763049752818a64ea7b13448e2ec7e396e8dd4bd7f066622c5314"
},
"pipfile-spec": 6,
"requires": {
@ -366,7 +366,7 @@
"sha256:6f62d78e2f89b4500b080fe3a81690850cd254227f27f75c3a0c491a1f351ba7",
"sha256:e8443a5e7a020e9d7f97f1d7d9cd17c88bcb3bc7e218bf9cf5095fe550be2951"
],
"markers": "python_version < '4.0' and python_full_version >= '3.6.1'",
"markers": "python_version < '4' and python_full_version >= '3.6.1'",
"version": "==5.10.1"
},
"itsdangerous": {
@ -988,11 +988,11 @@
},
"sentry-sdk": {
"hashes": [
"sha256:6c01d9d0b65935fd275adc120194737d1df317dce811e642cbf0394d0d37a007",
"sha256:c17179183cac614e900cbd048dab03f49a48e2820182ec686c25e7ce46f8548f"
"sha256:259535ba66933eacf85ab46524188c84dcb4c39f40348455ce15e2c0aca68863",
"sha256:778b53f0a6c83b1ee43d3b7886318ba86d975e686cb2c7906ccc35b334360be1"
],
"index": "pypi",
"version": "==1.5.11"
"version": "==1.5.12"
},
"setproctitle": {
"hashes": [
@ -1073,11 +1073,11 @@
},
"setuptools": {
"hashes": [
"sha256:26ead7d1f93efc0f8c804d9fafafbe4a44b179580a7105754b245155f9af05a8",
"sha256:47c7b0c0f8fc10eec4cf1e71c6fdadf8decaa74ffa087e68cd1c20db7ad6a592"
"sha256:5534570b9980fc650d45c62877ff603c7aaaf24893371708736cc016bd221c3c",
"sha256:ca6ba73b7fd5f734ae70ece8c4c1f7062b07f3352f6428f6277e27c8f5c64237"
],
"markers": "python_version >= '3.7'",
"version": "==62.1.0"
"version": "==62.2.0"
},
"six": {
"hashes": [
@ -1132,7 +1132,7 @@
"sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708",
"sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376"
],
"markers": "python_version >= '3.7'",
"markers": "python_version < '3.10'",
"version": "==4.2.0"
},
"urllib3": {
@ -1327,6 +1327,7 @@
"sha256:dd392a267e14b785a8f65dafef86e05a92201253e9fb4a01e1e262834f20bed2"
],
"index": "pypi",
"markers": "platform_system != 'Darwin'",
"version": "==0.9.12"
},
"certifi": {
@ -1597,11 +1598,11 @@
},
"hypothesis": {
"hashes": [
"sha256:37ca37f61939d0a92c18b5e17ab8088dccc5fc077c913d6619bbe1d233dfca1e",
"sha256:c671b3801527f3a8189314d9385418a67d4c7ab1b3330a3ff8b267d0d4599b3d"
"sha256:022c269158a2db28c40a005fc477f18a1395025dd8b09ce394e49cd0f36c550a",
"sha256:8d65fcd14634b08f5b2237f66b9cb188df84a81a5cffd3e9748de24baf0fa535"
],
"index": "pypi",
"version": "==6.46.2"
"version": "==6.46.3"
},
"identify": {
"hashes": [
@ -2143,11 +2144,11 @@
},
"pyparsing": {
"hashes": [
"sha256:7bf433498c016c4314268d95df76c81b842a4cb2b276fa3312cfb1e1d85f6954",
"sha256:ef7b523f6356f763771559412c0d7134753f037822dad1b16945b7b846f7ad06"
"sha256:2b020ecf7d21b687f219b71ecad3631f644a47f01403fa1d1036b0c6416d70fb",
"sha256:5026bae9a10eeaefb61dab2f09052b9f4307d44aee4eda64b309723d8d206bbc"
],
"markers": "python_full_version >= '3.6.8'",
"version": "==3.0.8"
"version": "==3.0.9"
},
"pyprof2calltree": {
"hashes": [
@ -2409,7 +2410,7 @@
"sha256:6657594ee297170d19f67d55c05852a874e7eb634f4f753dbd667855e07c1708",
"sha256:f1c24655a0da0d1b67f07e17a5e6b2a105894e6824b92096378bb3668ef02376"
],
"markers": "python_version >= '3.7'",
"markers": "python_version < '3.10'",
"version": "==4.2.0"
},
"urllib3": {

@ -1 +1 @@
Subproject commit 242d6bd0b08784a325febae4362d2fe8b048daab
Subproject commit 8669b82f5fbe0b21a51e018ead0d6d20cb090e0a

@ -36,12 +36,12 @@ class Priority:
def set_realtime_priority(level: int) -> None:
if not PC:
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # type: ignore[attr-defined]
os.sched_setscheduler(0, os.SCHED_FIFO, os.sched_param(level)) # type: ignore[attr-defined] # pylint: disable=no-member
def set_core_affinity(cores: List[int]) -> None:
if not PC:
os.sched_setaffinity(0, cores)
os.sched_setaffinity(0, cores) # pylint: disable=no-member
def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None:
@ -52,7 +52,7 @@ def config_realtime_process(cores: Union[int, List[int]], priority: int) -> None
class Ratekeeper:
def __init__(self, rate: int, print_delay_threshold: Optional[float] = 0.0) -> None:
def __init__(self, rate: float, print_delay_threshold: Optional[float] = 0.0) -> None:
"""Rate in Hz for ratekeeping. print_delay_threshold must be nonnegative."""
self._interval = 1. / rate
self._next_frame_time = sec_since_boot() + self._interval

@ -1,5 +1,6 @@
import os
from cffi import FFI
from typing import Any, List
# Workaround for the EON/termux build of Python having os.*xattr removed.
ffi = FFI()
@ -11,7 +12,7 @@ int removexattr(const char *path, const char *name);
""")
libc = ffi.dlopen(None)
def setxattr(path, name, value, flags=0):
def setxattr(path, name, value, flags=0) -> None:
path = path.encode()
name = name.encode()
if libc.setxattr(path, name, value, len(value), flags) == -1:
@ -29,7 +30,7 @@ def getxattr(path, name, size=128):
raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: getxattr({path}, {name}, {size})")
return ffi.buffer(value)[:l]
def listxattr(path, size=128):
def listxattr(path, size=128) -> List[Any]:
path = path.encode()
attrs = ffi.new(f"char[{size}]")
l = libc.listxattr(path, attrs, size)
@ -38,7 +39,7 @@ def listxattr(path, size=128):
# attrs is b'\0' delimited values (so chop off trailing empty item)
return [a.decode() for a in ffi.buffer(attrs)[:l].split(b"\0")[0:-1]]
def removexattr(path, name):
def removexattr(path, name) -> None:
path = path.encode()
name = name.encode()
if libc.removexattr(path, name) == -1:

@ -44,7 +44,9 @@ How We Rate The Cars
|Hyundai|Santa Fe 2019-20|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Hyundai|Sonata 2020-22|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Hyundai|Sonata Hybrid 2020-22|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Electric 2019-22|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Electric 2019-20|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Electric 2021|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Electric 2022|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Telluride 2020|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Lexus|ES 2019-21|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Lexus|ES Hybrid 2019-22|All|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
@ -95,12 +97,15 @@ How We Rate The Cars
|Hyundai|Sonata 2018-19|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Hyundai|Tucson Diesel 2019|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Ceed 2019|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Forte 2018-21|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Forte 2018|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Forte 2019-21|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|K5 2021-22|SCC + LFA|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Hybrid 2021-22|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Hybrid 2021|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Niro Hybrid 2022|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Optima 2019|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Seltos 2021|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Sorento 2018-19|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Sorento 2018|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Sorento 2019|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Kia|Stinger 2018|SCC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Lexus|CT Hybrid 2017-18|LSS|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Lexus|ES Hybrid 2017-18|LSS|<a href="##"><img valign="top" src="assets/icon-star-half.svg" width="22" /></a>[<sup>3</sup>](#footnotes)|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
@ -159,14 +164,14 @@ How We Rate The Cars
|Acura|ILX 2016-19|AcuraWatch Plus|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Acura|RDX 2016-18|AcuraWatch Plus|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Acura|RDX 2019-21|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Cadillac|Escalade ESV 2016[<sup>1</sup>](#footnotes)|ACC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Cadillac|Escalade ESV 2016[<sup>1</sup>](#footnotes)|ACC + LKAS|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|
|Chevrolet|Volt 2017-18[<sup>1</sup>](#footnotes)|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Chrysler|Pacifica 2017-18|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Chrysler|Pacifica 2020|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Chrysler|Pacifica Hybrid 2017-18|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Chrysler|Pacifica Hybrid 2019-21|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Genesis|G90 2018|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|
|GMC|Acadia 2018[<sup>1</sup>](#footnotes)|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|GMC|Acadia 2018[<sup>1</sup>](#footnotes)|Adaptive Cruise|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|
|Honda|Accord 2018-21|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Honda|Accord Hybrid 2018-21|All|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|Honda|Civic 2016-18|Honda Sensing|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="##"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|

@ -1 +1 @@
Subproject commit 919154efe2bd07c4dd124d7e6a11a4afc8685f9d
Subproject commit 9564b74d80525c9f289b730febbb2348c529c9cc

@ -1 +1 @@
Subproject commit eb662e4e5014a3fc3c04512d708f61080c7707c1
Subproject commit d5bd81e5b517c79e164d87b96355e6bc75915da0

@ -298,11 +298,12 @@ selfdrive/proclogd/proclog.cc
selfdrive/proclogd/proclog.h
selfdrive/loggerd/SConscript
selfdrive/loggerd/encoder.h
selfdrive/loggerd/v4l_encoder.cc
selfdrive/loggerd/v4l_encoder.h
selfdrive/loggerd/video_writer.cc
selfdrive/loggerd/video_writer.h
selfdrive/loggerd/encoder/encoder.cc
selfdrive/loggerd/encoder/encoder.h
selfdrive/loggerd/encoder/v4l_encoder.cc
selfdrive/loggerd/encoder/v4l_encoder.h
selfdrive/loggerd/encoder/video_writer.cc
selfdrive/loggerd/encoder/video_writer.h
selfdrive/loggerd/remote_encoder.cc
selfdrive/loggerd/remote_encoder.h
selfdrive/loggerd/logger.cc
@ -312,8 +313,8 @@ selfdrive/loggerd/loggerd.h
selfdrive/loggerd/encoderd.cc
selfdrive/loggerd/main.cc
selfdrive/loggerd/bootlog.cc
selfdrive/loggerd/raw_logger.cc
selfdrive/loggerd/raw_logger.h
selfdrive/loggerd/encoder/ffmpeg_encoder.cc
selfdrive/loggerd/encoder/ffmpeg_encoder.h
selfdrive/loggerd/__init__.py
selfdrive/loggerd/config.py

@ -2,12 +2,10 @@
from collections import Counter
from pprint import pprint
from selfdrive.car.docs import get_tier_car_info
from selfdrive.car.docs import get_all_car_info
if __name__ == "__main__":
tiers = get_tier_car_info()
cars = [car for tier_cars in tiers.values() for car in tier_cars]
cars = get_all_car_info()
make_count = Counter(l.make for l in cars)
print("\n", "*" * 20, len(cars), "total", "*" * 20, "\n")
pprint(make_count)

@ -6,7 +6,7 @@ from multiprocessing import Process
from setproctitle import setproctitle # pylint: disable=no-name-in-module
def waste(core):
os.sched_setaffinity(0, [core,])
os.sched_setaffinity(0, [core,]) # pylint: disable=no-member
m1 = np.zeros((200, 200)) + 0.8
m2 = np.zeros((200, 200)) + 1.2

@ -439,7 +439,7 @@ def startLocalProxy(global_end_event, remote_ws_uri, local_port):
ssock, csock = socket.socketpair()
local_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
local_sock.connect(('127.0.0.1', local_port))
local_sock.setblocking(0)
local_sock.setblocking(False)
proxy_end_event = threading.Event()
threads = [

@ -23,13 +23,13 @@ def is_registered_device() -> bool:
return dongle not in (None, UNREGISTERED_DONGLE_ID)
def register(show_spinner=False) -> str:
def register(show_spinner=False) -> Optional[str]:
params = Params()
params.put("SubscriberInfo", HARDWARE.get_subscriber_info())
IMEI = params.get("IMEI", encoding='utf8')
HardwareSerial = params.get("HardwareSerial", encoding='utf8')
dongle_id = params.get("DongleId", encoding='utf8')
dongle_id: Optional[str] = params.get("DongleId", encoding='utf8')
needs_registration = None in (IMEI, HardwareSerial, dongle_id)
pubkey = Path(PERSIST+"/comma/id_rsa.pub")

@ -646,8 +646,6 @@ void boardd_main_thread(std::vector<std::string> serials) {
Panda *peripheral_panda = pandas[0];
std::vector<std::thread> threads;
Params().put("LastPeripheralPandaType", std::to_string((int) peripheral_panda->get_hw_type()));
threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
threads.emplace_back(peripheral_control_thread, peripheral_panda);
threads.emplace_back(pigeon_thread, peripheral_panda);

@ -4,7 +4,7 @@ import os
import usb1
import time
import subprocess
from typing import NoReturn
from typing import List, NoReturn
from functools import cmp_to_key
from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU
@ -102,7 +102,7 @@ def main() -> NoReturn:
cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
# Flash pandas
pandas = []
pandas: List[Panda] = []
for serial in panda_serials:
pandas.append(flash_panda(serial))
@ -119,7 +119,7 @@ def main() -> NoReturn:
# sort pandas to have deterministic order
pandas.sort(key=cmp_to_key(panda_sort_cmp))
panda_serials = list(map(lambda p: p.get_usb_serial(), pandas))
panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) # type: ignore
# log panda fw versions
params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))

@ -13,30 +13,33 @@ const __constant half3 color_correction_2 = (half3)(-0.25277411, -0.05627105, 1.
// tone mapping params
const half gamma_k = 0.75;
const half gamma_b = 0.125;
const half mp_default = 0.01; // ideally midpoint should be adaptive
const half mp = 0.01; // ideally midpoint should be adaptive
const half rk = 9 - 100*mp;
half gamma_apply(half x, half mp) {
inline half3 gamma_apply(half3 x) {
// poly approximation for s curve
half rk = 9 - 100*mp;
if (x > mp) {
return (rk * (x-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(x-mp))) + gamma_k*mp + gamma_b;
} else if (x < mp) {
return (rk * (x-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(x-mp))) + gamma_k*mp + gamma_b;
} else {
return x;
}
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);
}
half3 color_correct(half3 rgb) {
half3 ret = (half3)(0.0, 0.0, 0.0);
ret += (half)rgb.x * color_correction_0;
inline half3 color_correct(half3 rgb) {
half3 ret = (half)rgb.x * color_correction_0;
ret += (half)rgb.y * color_correction_1;
ret += (half)rgb.z * color_correction_2;
ret.x = gamma_apply(ret.x, mp_default);
ret.y = gamma_apply(ret.y, mp_default);
ret.z = gamma_apply(ret.z, mp_default);
ret = clamp(0.0, 255.0, ret*255.0);
return ret;
return gamma_apply(ret);
}
inline half get_vignetting_s(float r) {
if (r < 62500) {
return (half)(1.0f + 0.0000008f*r);
} else if (r < 490000) {
return (half)(0.9625f + 0.0000014f*r);
} else if (r < 1102500) {
return (half)(1.26434f + 0.0000000000016f*r*r);
} else {
return (half)(0.53503625f + 0.0000000000022f*r*r);
}
}
inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
@ -49,42 +52,21 @@ inline half val_from_10(const uchar * source, int gx, int gy, half black_level)
// normalize
pv = max((half)0.0, pv - black_level);
pv /= (1024.0f - black_level);
pv /= (1024.0 - black_level);
// correct vignetting
if (CAM_NUM == 1) { // fcamera
gx = (gx - RGB_WIDTH/2);
gy = (gy - RGB_HEIGHT/2);
float r = gx*gx + gy*gy;
half s;
if (r < 62500) {
s = (half)(1.0f + 0.0000008f*r);
} else if (r < 490000) {
s = (half)(0.9625f + 0.0000014f*r);
} else if (r < 1102500) {
s = (half)(1.26434f + 0.0000000000016f*r*r);
} else {
s = (half)(0.53503625f + 0.0000000000022f*r*r);
}
pv = s * pv;
pv *= get_vignetting_s(gx*gx + gy*gy);
}
pv = clamp((half)0.0, (half)1.0, pv);
pv = clamp(pv, (half)0.0, (half)1.0);
return pv;
}
half fabs_diff(half x, half y) {
return fabs(x-y);
}
half phi(half x) {
// detection funtion
return 2 - x;
// if (x > 1) {
// return 1 / x;
// } else {
// return 2 - x;
// }
inline half get_k(half a, half b, half c, half d) {
return 2.0 - (fabs(a - b) + fabs(c - d));
}
__kernel void debayer10(const __global uchar * in,
@ -96,23 +78,26 @@ __kernel void debayer10(const __global uchar * in,
const int x_global = get_global_id(0);
const int y_global = get_global_id(1);
const int x_local = get_local_id(0);
const int y_local = get_local_id(1);
const int localRowLen = 2 + get_local_size(0); // 2 padding
const int x_local = get_local_id(0); // 0-15
const int y_local = get_local_id(1); // 0-15
const int localOffset = (y_local + 1) * localRowLen + x_local + 1; // max 18x18-1
const int localColLen = 2 + get_local_size(1);
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH;
const int localOffset = (y_local + 1) * localRowLen + x_local + 1;
half pv = val_from_10(in, x_global, y_global, black_level);
cached[localOffset] = pv;
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH;
// cache padding
int localColOffset = -1;
int globalColOffset = -1;
int globalColOffset;
const int x_global_mod = (x_global == 0 || x_global == RGB_WIDTH - 1) ? -1: 1;
const int y_global_mod = (y_global == 0 || y_global == RGB_HEIGHT - 1) ? -1: 1;
half pv = val_from_10(in, x_global, y_global, black_level);
cached[localOffset] = pv;
// cache padding
if (x_local < 1) {
localColOffset = x_local;
@ -154,20 +139,20 @@ __kernel void debayer10(const __global uchar * in,
if (x_global % 2 == 0) {
if (y_global % 2 == 0) {
rgb.y = pv; // G1(R)
half k1 = phi(fabs_diff(d1, pv) + fabs_diff(d2, pv));
half k2 = phi(fabs_diff(d2, pv) + fabs_diff(d4, pv));
half k3 = phi(fabs_diff(d3, pv) + fabs_diff(d4, pv));
half k4 = phi(fabs_diff(d1, pv) + fabs_diff(d3, pv));
half k1 = get_k(d1, pv, d2, pv);
half k2 = get_k(d2, pv, d4, pv);
half k3 = get_k(d3, pv, d4, pv);
half k4 = get_k(d1, pv, d3, pv);
// R_G1
rgb.x = (k2*n2+k4*n4)/(k2+k4);
// B_G1
rgb.z = (k1*n1+k3*n3)/(k1+k3);
} else {
rgb.z = pv; // B
half k1 = phi(fabs_diff(d1, d3) + fabs_diff(d2, d4));
half k2 = phi(fabs_diff(n1, n4) + fabs_diff(n2, n3));
half k3 = phi(fabs_diff(d1, d2) + fabs_diff(d3, d4));
half k4 = phi(fabs_diff(n1, n2) + fabs_diff(n3, n4));
half k1 = get_k(d1, d3, d2, d4);
half k2 = get_k(n1, n4, n2, n3);
half k3 = get_k(d1, d2, d3, d4);
half k4 = get_k(n1, n2, n3, n4);
// G_B
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3);
// R_B
@ -176,20 +161,20 @@ __kernel void debayer10(const __global uchar * in,
} else {
if (y_global % 2 == 0) {
rgb.x = pv; // R
half k1 = phi(fabs_diff(d1, d3) + fabs_diff(d2, d4));
half k2 = phi(fabs_diff(n1, n4) + fabs_diff(n2, n3));
half k3 = phi(fabs_diff(d1, d2) + fabs_diff(d3, d4));
half k4 = phi(fabs_diff(n1, n2) + fabs_diff(n3, n4));
half k1 = get_k(d1, d3, d2, d4);
half k2 = get_k(n1, n4, n2, n3);
half k3 = get_k(d1, d2, d3, d4);
half k4 = get_k(n1, n2, n3, n4);
// G_R
rgb.y = (k1*(n1+n3)*0.5+k3*(n2+n4)*0.5)/(k1+k3);
// B_R
rgb.z = (k2*(d2+d3)*0.5+k4*(d1+d4)*0.5)/(k2+k4);
} else {
rgb.y = pv; // G2(B)
half k1 = phi(fabs_diff(d1, pv) + fabs_diff(d2, pv));
half k2 = phi(fabs_diff(d2, pv) + fabs_diff(d4, pv));
half k3 = phi(fabs_diff(d3, pv) + fabs_diff(d4, pv));
half k4 = phi(fabs_diff(d1, pv) + fabs_diff(d3, pv));
half k1 = get_k(d1, pv, d2, pv);
half k2 = get_k(d2, pv, d4, pv);
half k3 = get_k(d3, pv, d4, pv);
half k4 = get_k(d1, pv, d3, pv);
// R_G2
rgb.x = (k1*n1+k3*n3)/(k1+k3);
// B_G2
@ -197,10 +182,8 @@ __kernel void debayer10(const __global uchar * in,
}
}
rgb = clamp(0.0, 1.0, rgb);
rgb = color_correct(rgb);
out[out_idx + 0] = (uchar)(rgb.z);
out[out_idx + 1] = (uchar)(rgb.y);
out[out_idx + 2] = (uchar)(rgb.x);
uchar3 rgbc = convert_uchar3_sat(color_correct(clamp(rgb, (half)0.0, (half)1.0)) * 255.0);
out[out_idx + 0] = rgbc.z;
out[out_idx + 1] = rgbc.y;
out[out_idx + 2] = rgbc.x;
}

@ -1,6 +1,7 @@
# functions common among cars
from cereal import car
from common.numpy_fast import clip
from typing import Dict
# kg of standard extra cargo to count for drive, gas, etc...
STD_CARGO_KG = 136.
@ -41,7 +42,7 @@ 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):
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, str]:
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc}

@ -2,7 +2,7 @@ from typing import Dict
from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
Ecu = car.CarParams.Ecu
SPEED_FROM_RPM = 0.008587
@ -18,7 +18,7 @@ class CAR:
BODY = "COMMA BODY"
CAR_INFO: Dict[str, CarInfo] = {
CAR.BODY: CarInfo("comma body", package="All", good_torque=True),
CAR.BODY: CarInfo("comma body", package="All", good_torque=True, harness=Harness.none),
}
FW_VERSIONS = {

@ -1,11 +1,13 @@
from dataclasses import dataclass
from enum import Enum
from typing import Dict, List, Union
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
from cereal import car
Ecu = car.CarParams.Ecu
class CarControllerParams:
STEER_MAX = 261 # 262 faults
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
@ -26,6 +28,7 @@ class CAR:
@dataclass
class ChryslerCarInfo(CarInfo):
package: str = "Adaptive Cruise"
harness: Enum = Harness.fca
CAR_INFO: Dict[str, Union[ChryslerCarInfo, List[ChryslerCarInfo]]] = {

@ -26,9 +26,8 @@ CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md")
CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md")
def get_tier_car_info() -> Dict[Tier, List[CarInfo]]:
tier_car_info: Dict[Tier, List[CarInfo]] = {tier: [] for tier in Tier}
def get_all_car_info() -> List[CarInfo]:
all_car_info: List[CarInfo] = []
for models in get_interface_attr("CAR_INFO").values():
for model, car_info in models.items():
# Hyundai exception: those with radar have openpilot longitudinal
@ -43,8 +42,17 @@ def get_tier_car_info() -> Dict[Tier, List[CarInfo]]:
car_info = (car_info,)
for _car_info in car_info:
_car_info.init(CP, non_tested_cars, ALL_FOOTNOTES)
tier_car_info[_car_info.tier].append(_car_info)
all_car_info.append(_car_info.init(CP, non_tested_cars, ALL_FOOTNOTES))
# Sort cars by make and model + year
sorted_cars: List[CarInfo] = natsorted(all_car_info, key=lambda car: (car.make + car.model).lower())
return sorted_cars
def sort_by_tier(all_car_info: List[CarInfo]) -> Dict[Tier, List[CarInfo]]:
tier_car_info: Dict[Tier, List[CarInfo]] = {tier: [] for tier in Tier}
for car_info in all_car_info:
tier_car_info[car_info.tier].append(car_info)
# Sort cars by make and model + year
for tier, cars in tier_car_info.items():
@ -53,12 +61,14 @@ def get_tier_car_info() -> Dict[Tier, List[CarInfo]]:
return tier_car_info
def generate_cars_md(tier_car_info: Dict[Tier, List[CarInfo]], template_fn: str) -> str:
def generate_cars_md(all_car_info: List[CarInfo], template_fn: str) -> str:
with open(template_fn, "r") as f:
template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True)
footnotes = [fn.value.text for fn in ALL_FOOTNOTES]
return template.render(tiers=tier_car_info, footnotes=footnotes, Star=Star, Column=Column)
cars_md: str = template.render(tiers=sort_by_tier(all_car_info), all_car_info=all_car_info,
footnotes=footnotes, Star=Star, Column=Column)
return cars_md
if __name__ == "__main__":
@ -70,5 +80,5 @@ if __name__ == "__main__":
args = parser.parse_args()
with open(args.out, 'w') as f:
f.write(generate_cars_md(get_tier_car_info(), args.template))
f.write(generate_cars_md(get_all_car_info(), args.template))
print(f"Generated and written to {args.out}")

@ -50,13 +50,16 @@ class CarInfo:
min_steer_speed: Optional[float] = None
min_enable_speed: Optional[float] = None
good_torque: bool = False
harness: Optional[Enum] = None
def init(self, CP: car.CarParams, non_tested_cars: List[str], all_footnotes: Dict[Enum, int]):
# TODO: set all the min steer speeds in carParams and remove this
min_steer_speed = CP.minSteerSpeed
if self.min_steer_speed is not None:
min_steer_speed = self.min_steer_speed
assert CP.minSteerSpeed == 0, f"Minimum steer speed set in both CarInfo and CarParams for {CP.carFingerprint}"
assert CP.minSteerSpeed == 0, f"{CP.carFingerprint}: Minimum steer speed set in both CarInfo and CarParams"
assert self.harness is not None, f"{CP.carFingerprint}: Need to specify car harness"
# TODO: set all the min enable speeds in carParams correctly and remove this
min_enable_speed = CP.minEnableSpeed
@ -74,7 +77,7 @@ class CarInfo:
Column.FSR_LONGITUDINAL: min_enable_speed <= 0.,
Column.FSR_STEERING: min_steer_speed <= 0.,
Column.STEERING_TORQUE: self.good_torque,
Column.MAINTAINED: CP.carFingerprint not in non_tested_cars,
Column.MAINTAINED: CP.carFingerprint not in non_tested_cars and self.harness is not Harness.none,
}
if CP.notCar:
@ -91,6 +94,7 @@ class CarInfo:
self.row[column] = footnote.value.star
self.tier = {5: Tier.GOLD, 4: Tier.SILVER}.get(list(self.row.values()).count(Star.FULL), Tier.BRONZE)
return self
@no_type_check
def get_column(self, column: Column, star_icon: str, footnote_tag: str) -> str:
@ -103,3 +107,34 @@ class CarInfo:
item += footnote_tag.format(self.all_footnotes[footnote])
return item
class Harness(Enum):
nidec = "Honda Nidec"
bosch = "Honda Bosch"
toyota = "Toyota"
subaru = "Subaru"
fca = "FCA"
vw = "VW"
j533 = "J533"
hyundai_a = "Hyundai A"
hyundai_b = "Hyundai B"
hyundai_c = "Hyundai C"
hyundai_d = "Hyundai D"
hyundai_e = "Hyundai E"
hyundai_f = "Hyundai F"
hyundai_g = "Hyundai G"
hyundai_h = "Hyundai H"
hyundai_i = "Hyundai I"
hyundai_j = "Hyundai J"
hyundai_k = "Hyundai K"
hyundai_l = "Hyundai L"
hyundai_m = "Hyundai M"
hyundai_n = "Hyundai N"
hyundai_o = "Hyundai O"
custom = "Developer"
obd_ii = "OBD-II"
nissan_a = "Nissan A"
nissan_b = "Nissan B"
mazda = "Mazda"
none = "None"

@ -1,9 +1,10 @@
from dataclasses import dataclass
from enum import Enum
from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
Ecu = car.CarParams.Ecu
@ -62,14 +63,20 @@ class Footnote(Enum):
Column.MODEL)
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
CAR.HOLDEN_ASTRA: CarInfo("Holden Astra 2017", "Adaptive Cruise"),
CAR.VOLT: CarInfo("Chevrolet Volt 2017-18", "Adaptive Cruise", footnotes=[Footnote.OBD_II], min_enable_speed=0),
CAR.CADILLAC_ATS: CarInfo("Cadillac ATS Premium Performance 2018", "Adaptive Cruise"),
CAR.MALIBU: CarInfo("Chevrolet Malibu Premier 2017", "Adaptive Cruise"),
CAR.ACADIA: CarInfo("GMC Acadia 2018", "Adaptive Cruise", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo", footnotes=[Footnote.OBD_II]),
CAR.BUICK_REGAL: CarInfo("Buick Regal Essence 2018", "Adaptive Cruise"),
CAR.ESCALADE_ESV: CarInfo("Cadillac Escalade ESV 2016", "ACC + LKAS", footnotes=[Footnote.OBD_II]),
@dataclass
class GMCarInfo(CarInfo):
package: str = "Adaptive Cruise"
harness: Enum = Harness.none
CAR_INFO: Dict[str, Union[GMCarInfo, List[GMCarInfo]]] = {
CAR.HOLDEN_ASTRA: GMCarInfo("Holden Astra 2017", harness=Harness.custom),
CAR.VOLT: GMCarInfo("Chevrolet Volt 2017-18", footnotes=[Footnote.OBD_II], min_enable_speed=0, harness=Harness.custom),
CAR.CADILLAC_ATS: GMCarInfo("Cadillac ATS Premium Performance 2018"),
CAR.MALIBU: GMCarInfo("Chevrolet Malibu Premier 2017", harness=Harness.custom),
CAR.ACADIA: GMCarInfo("GMC Acadia 2018", video_link="https://www.youtube.com/watch?v=0ZN6DdsBUZo", footnotes=[Footnote.OBD_II]),
CAR.BUICK_REGAL: GMCarInfo("Buick Regal Essence 2018"),
CAR.ESCALADE_ESV: GMCarInfo("Cadillac Escalade ESV 2016", "ACC + LKAS", footnotes=[Footnote.OBD_II]),
}

@ -112,10 +112,10 @@ class CarController:
self.apply_brake_last = 0
self.last_pump_ts = 0.
self.accel = 0
self.speed = 0
self.gas = 0
self.brake = 0
self.accel = 0.0
self.speed = 0.0
self.gas = 0.0
self.brake = 0.0
def update(self, CC, CS):
actuators = CC.actuators

@ -5,7 +5,7 @@ from typing import Dict, List, Union
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -108,31 +108,31 @@ class HondaCarInfo(CarInfo):
CAR_INFO: Dict[str, Union[HondaCarInfo, List[HondaCarInfo]]] = {
CAR.ACCORD: [
HondaCarInfo("Honda Accord 2018-21", "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),
HondaCarInfo("Honda Accord 2018-21", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
],
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-21", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18"),
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-21", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", harness=Harness.nidec),
CAR.CIVIC_BOSCH: [
HondaCarInfo("Honda Civic 2019-20", "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"),
HondaCarInfo("Honda Civic 2019-20", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8", footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS, harness=Harness.bosch),
HondaCarInfo("Honda Civic Hatchback 2017-21", harness=Harness.bosch),
],
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"),
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-21"),
CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS, harness=Harness.nidec),
CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring", harness=Harness.nidec),
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-21", harness=Harness.bosch),
# CAR.CRV_EU: HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19"),
CAR.FIT: HondaCarInfo("Honda Fit 2018-19"),
CAR.FREED: HondaCarInfo("Honda Freed 2020"),
CAR.HRV: HondaCarInfo("Honda HR-V 2019-20"),
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", min_steer_speed=0.),
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus"),
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS),
CAR.PILOT: HondaCarInfo("Honda Pilot 2016-21"),
CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All"),
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22"),
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-21", "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),
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", harness=Harness.bosch),
CAR.FIT: HondaCarInfo("Honda Fit 2018-19", harness=Harness.nidec),
CAR.FREED: HondaCarInfo("Honda Freed 2020", harness=Harness.nidec),
CAR.HRV: HondaCarInfo("Honda HR-V 2019-20", harness=Harness.nidec),
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20", min_steer_speed=0., harness=Harness.nidec),
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", harness=Harness.nidec),
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
CAR.PILOT: HondaCarInfo("Honda Pilot 2016-21", harness=Harness.nidec),
CAR.PASSPORT: HondaCarInfo("Honda Passport 2019-21", "All", harness=Harness.nidec),
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-22", harness=Harness.nidec),
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS, harness=Harness.bosch),
}

@ -125,8 +125,8 @@ class CarInterface(CarInterfaceBase):
elif candidate in (CAR.KONA, CAR.KONA_EV, CAR.KONA_HEV):
ret.lateralTuning.pid.kf = 0.00005
ret.mass = {CAR.KONA_EV: 1685., CAR.KONA_HEV: 1425.}.get(candidate, 1275.) + STD_CARGO_KG
ret.wheelbase = 2.7
ret.steerRatio = 13.73 * 1.15 # Spec
ret.wheelbase = 2.6
ret.steerRatio = 13.42 # Spec
tire_stiffness_factor = 0.385
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]

@ -4,7 +4,7 @@ from typing import Dict, List, Union
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
Ecu = car.CarParams.Ecu
# Steer torque limits
@ -82,53 +82,66 @@ class HyundaiCarInfo(CarInfo):
CAR_INFO: Dict[str, Union[HyundaiCarInfo, List[HyundaiCarInfo]]] = {
CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS),
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c"),
CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", video_link="https://youtu.be/_EdYQtV52-c"),
CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS),
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19"),
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "SCC + LFA"),
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019"),
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020"),
CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", "SCC + LKAS"),
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-21"),
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020"),
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21"),
CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020"),
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All"),
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All"),
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All"),
CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All"),
CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw"),
CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19"),
CAR.TUCSON_DIESEL_2019: HyundaiCarInfo("Hyundai Tucson Diesel 2019", "SCC + LKAS"),
CAR.ELANTRA: HyundaiCarInfo("Hyundai Elantra 2017-19", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_b),
CAR.ELANTRA_2021: HyundaiCarInfo("Hyundai Elantra 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
CAR.ELANTRA_HEV_2021: HyundaiCarInfo("Hyundai Elantra Hybrid 2021-22", video_link="https://youtu.be/_EdYQtV52-c", harness=Harness.hyundai_k),
CAR.HYUNDAI_GENESIS: HyundaiCarInfo("Hyundai Genesis 2015-16", min_enable_speed=19 * CV.MPH_TO_MS, harness=Harness.hyundai_j),
CAR.IONIQ: HyundaiCarInfo("Hyundai Ioniq Hybrid 2017-19", harness=Harness.hyundai_c),
CAR.IONIQ_HEV_2022: HyundaiCarInfo("Hyundai Ioniq Hybrid 2020-22", "SCC + LFA", harness=Harness.hyundai_h),
CAR.IONIQ_EV_LTD: HyundaiCarInfo("Hyundai Ioniq Electric 2019", harness=Harness.hyundai_c),
CAR.IONIQ_EV_2020: HyundaiCarInfo("Hyundai Ioniq Electric 2020", harness=Harness.hyundai_h),
CAR.IONIQ_PHEV_2019: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2019", harness=Harness.hyundai_c),
CAR.IONIQ_PHEV: HyundaiCarInfo("Hyundai Ioniq Plug-in Hybrid 2020-21", harness=Harness.hyundai_h),
CAR.KONA: HyundaiCarInfo("Hyundai Kona 2020", harness=Harness.hyundai_b),
CAR.KONA_EV: HyundaiCarInfo("Hyundai Kona Electric 2018-21", harness=Harness.hyundai_g),
CAR.KONA_HEV: HyundaiCarInfo("Hyundai Kona Hybrid 2020", harness=Harness.hyundai_i),
CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", harness=Harness.hyundai_d),
CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-22", "All", harness=Harness.hyundai_l),
CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022", "All", harness=Harness.hyundai_l),
CAR.SANTA_FE_PHEV_2022: HyundaiCarInfo("Hyundai Santa Fe Plug-in Hybrid 2022", "All", harness=Harness.hyundai_l),
CAR.SONATA: HyundaiCarInfo("Hyundai Sonata 2020-22", "All", video_link="https://www.youtube.com/watch?v=ix63r9kE3Fw", harness=Harness.hyundai_a),
CAR.SONATA_LF: HyundaiCarInfo("Hyundai Sonata 2018-19", harness=Harness.hyundai_e),
CAR.TUCSON_DIESEL_2019: HyundaiCarInfo("Hyundai Tucson Diesel 2019", harness=Harness.hyundai_l),
CAR.PALISADE: [
HyundaiCarInfo("Hyundai Palisade 2020-21", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456"),
HyundaiCarInfo("Kia Telluride 2020"),
HyundaiCarInfo("Hyundai Palisade 2020-21", "All", video_link="https://youtu.be/TAnDqjF4fDY?t=456", harness=Harness.hyundai_h),
HyundaiCarInfo("Kia Telluride 2020", harness=Harness.hyundai_h),
],
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All"),
CAR.VELOSTER: HyundaiCarInfo("Hyundai Veloster 2019-20", min_enable_speed=5. * CV.MPH_TO_MS, harness=Harness.hyundai_e),
CAR.SONATA_HYBRID: HyundaiCarInfo("Hyundai Sonata Hybrid 2020-22", "All", harness=Harness.hyundai_a),
# Kia
CAR.KIA_FORTE: HyundaiCarInfo("Kia Forte 2018-21"),
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "SCC + LFA"),
CAR.KIA_NIRO_EV: HyundaiCarInfo("Kia Niro Electric 2019-22", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo"),
CAR.KIA_NIRO_HEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2019", min_enable_speed=10. * CV.MPH_TO_MS),
CAR.KIA_NIRO_HEV_2021: HyundaiCarInfo("Kia Niro Hybrid 2021-22"),
CAR.KIA_FORTE: [
HyundaiCarInfo("Kia Forte 2018", harness=Harness.hyundai_b),
HyundaiCarInfo("Kia Forte 2019-21", harness=Harness.hyundai_g),
],
CAR.KIA_K5_2021: HyundaiCarInfo("Kia K5 2021-22", "SCC + LFA", harness=Harness.hyundai_a),
CAR.KIA_NIRO_EV: [
HyundaiCarInfo("Kia Niro Electric 2019-20", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_f),
HyundaiCarInfo("Kia Niro Electric 2021", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_c),
HyundaiCarInfo("Kia Niro Electric 2022", "All", video_link="https://www.youtube.com/watch?v=lT7zcG6ZpGo", harness=Harness.hyundai_h),
],
CAR.KIA_NIRO_HEV: HyundaiCarInfo("Kia Niro Plug-in Hybrid 2019", min_enable_speed=10. * CV.MPH_TO_MS, harness=Harness.hyundai_c),
CAR.KIA_NIRO_HEV_2021: [
HyundaiCarInfo("Kia Niro Hybrid 2021", harness=Harness.hyundai_f), # TODO: could be hyundai_d, verify
HyundaiCarInfo("Kia Niro Hybrid 2022", harness=Harness.hyundai_h),
],
CAR.KIA_OPTIMA: [
HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS),
HyundaiCarInfo("Kia Optima 2019"),
HyundaiCarInfo("Kia Optima 2017", min_steer_speed=32. * CV.MPH_TO_MS, harness=Harness.hyundai_b),
HyundaiCarInfo("Kia Optima 2019", harness=Harness.hyundai_g),
],
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021", harness=Harness.hyundai_a),
CAR.KIA_SORENTO: [
HyundaiCarInfo("Kia Sorento 2018", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_c),
HyundaiCarInfo("Kia Sorento 2019", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8", harness=Harness.hyundai_e),
],
CAR.KIA_SELTOS: HyundaiCarInfo("Kia Seltos 2021"),
CAR.KIA_SORENTO: HyundaiCarInfo("Kia Sorento 2018-19", video_link="https://www.youtube.com/watch?v=Fkh3s6WHJz8"),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0"),
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019"),
CAR.KIA_STINGER: HyundaiCarInfo("Kia Stinger 2018", video_link="https://www.youtube.com/watch?v=MJ94qoofYw0", harness=Harness.hyundai_c),
CAR.KIA_CEED: HyundaiCarInfo("Kia Ceed 2019", harness=Harness.hyundai_e),
# Genesis
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018", "All"),
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All"),
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018", "All"),
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2018", "All"),
CAR.GENESIS_G70: HyundaiCarInfo("Genesis G70 2018", "All", harness=Harness.hyundai_f),
CAR.GENESIS_G70_2020: HyundaiCarInfo("Genesis G70 2020", "All", harness=Harness.hyundai_f),
CAR.GENESIS_G80: HyundaiCarInfo("Genesis G80 2018", "All", harness=Harness.hyundai_h),
CAR.GENESIS_G90: HyundaiCarInfo("Genesis G90 2018", "All", harness=Harness.hyundai_c),
}
class Buttons:

@ -1,8 +1,9 @@
from dataclasses import dataclass
from enum import Enum
from typing import Dict, List, Union
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
from cereal import car
Ecu = car.CarParams.Ecu
@ -31,6 +32,7 @@ class CAR:
@dataclass
class MazdaCarInfo(CarInfo):
package: str = "All"
harness: Enum = Harness.mazda
CAR_INFO: Dict[str, Union[MazdaCarInfo, List[MazdaCarInfo]]] = {

@ -1,8 +1,9 @@
from dataclasses import dataclass
from typing import Dict, List, Union
from enum import Enum
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
from cereal import car
Ecu = car.CarParams.Ecu
@ -28,13 +29,14 @@ class CAR:
@dataclass
class NissanCarInfo(CarInfo):
package: str = "ProPILOT"
harness: Enum = Harness.nissan_a
CAR_INFO: Dict[str, Union[NissanCarInfo, List[NissanCarInfo]]] = {
CAR.XTRAIL: NissanCarInfo("Nissan X-Trail 2017"),
CAR.LEAF: NissanCarInfo("Nissan Leaf 2018-22"),
CAR.ROGUE: NissanCarInfo("Nissan Rogue 2018-20"),
CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20"),
CAR.ALTIMA: NissanCarInfo("Nissan Altima 2019-20", harness=Harness.nissan_b),
}
FINGERPRINTS = {

@ -1,11 +1,14 @@
from dataclasses import dataclass
from enum import Enum
from typing import Dict, List, Union
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarInfo
from selfdrive.car.docs_definitions import CarInfo, Harness
from cereal import car
Ecu = car.CarParams.Ecu
class CarControllerParams:
def __init__(self, CP):
if CP.carFingerprint == CAR.IMPREZA_2020:
@ -34,6 +37,7 @@ class CAR:
@dataclass
class SubaruCarInfo(CarInfo):
package: str = "EyeSight"
harness: Enum = Harness.subaru
CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = {

@ -1,15 +1,15 @@
#!/usr/bin/env python3
import unittest
from selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_tier_car_info
from selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_info
class TestCarDocs(unittest.TestCase):
def setUp(self):
self.tier_cars = get_tier_car_info()
self.all_cars = get_all_car_info()
def test_generator(self):
generated_cars_md = generate_cars_md(self.tier_cars, CARS_MD_TEMPLATE)
generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE)
with open(CARS_MD_OUT, "r") as f:
current_cars_md = f.read()
@ -18,8 +18,7 @@ class TestCarDocs(unittest.TestCase):
def test_naming_conventions(self):
# Asserts market-standard car naming conventions by make
for cars in self.tier_cars.values():
for car in cars:
for car in self.all_cars:
if car.car_name == "hyundai":
tokens = car.model.lower().split(" ")
self.assertNotIn("phev", tokens, "Use `Plug-in Hybrid`")

@ -143,11 +143,11 @@ class TestCarModel(unittest.TestCase):
assert RI
error_cnt = 0
for msg in self.can_msgs:
radar_data = RI.update((msg.as_builder().to_bytes(),))
if radar_data is not None:
error_cnt += car.RadarData.Error.canError in radar_data.errors
self.assertLess(error_cnt, 20)
for i, msg in enumerate(self.can_msgs):
rr = RI.update((msg.as_builder().to_bytes(),))
if rr is not None and i > 50:
error_cnt += car.RadarData.Error.canError in rr.errors
self.assertEqual(error_cnt, 0)
def test_panda_safety_rx_valid(self):
if self.CP.dashcamOnly:

@ -46,7 +46,7 @@ class CarInterface(CarInterfaceBase):
ret.wheelbase = 2.78
ret.steerRatio = 17.4
tire_stiffness_factor = 0.5533
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06)
elif candidate in (CAR.RAV4, CAR.RAV4H):

@ -6,7 +6,7 @@ from typing import Dict, List, Union
from cereal import car
from common.conversions import Conversions as CV
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Star
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness, Star
Ecu = car.CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
@ -100,6 +100,7 @@ class Footnote(Enum):
class ToyotaCarInfo(CarInfo):
package: str = "All"
good_torque: bool = True
harness: Enum = Harness.toyota
CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {

@ -5,13 +5,14 @@ from typing import Dict, List, Union
from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column
from selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, Harness
Ecu = car.CarParams.Ecu
NetworkLocation = car.CarParams.NetworkLocation
TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter
class CarControllerParams:
HCA_STEP = 2 # HCA_01 message frequency 50Hz
LDW_STEP = 10 # LDW_02 message frequency 10Hz
@ -30,14 +31,17 @@ class CarControllerParams:
STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily
STEER_DRIVER_FACTOR = 1 # from dbc
class CANBUS:
pt = 0
cam = 2
class DBC_FILES:
mqb = "vw_mqb_2010" # Used for all cars with MQB-style CAN messaging
DBC = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None)) # type: Dict[str, Dict[str, str]]
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None))
BUTTON_STATES = {
"accelCruise": False,
@ -60,6 +64,7 @@ MQB_LDW_MESSAGES = {
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward
}
# 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.
@ -110,11 +115,12 @@ class Footnote(Enum):
class VWCarInfo(CarInfo):
package: str = "Driver Assistance"
good_torque: bool = True
harness: Enum = Harness.vw
CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
CAR.ARTEON_MK1: VWCarInfo("Volkswagen Arteon 2018, 2021", footnotes=[Footnote.VW_HARNESS]),
CAR.ATLAS_MK1: VWCarInfo("Volkswagen Atlas 2018-19, 2022", footnotes=[Footnote.VW_HARNESS]),
CAR.ARTEON_MK1: VWCarInfo("Volkswagen Arteon 2018, 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.ATLAS_MK1: VWCarInfo("Volkswagen Atlas 2018-19, 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.GOLF_MK7: [
VWCarInfo("Volkswagen e-Golf 2014, 2018-20"),
VWCarInfo("Volkswagen Golf 2015-20"),
@ -131,15 +137,15 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
],
CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2016-18", footnotes=[Footnote.PASSAT]),
CAR.POLO_MK6: VWCarInfo("Volkswagen Polo 2020"),
CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS]),
CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS]),
CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS]),
CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.TIGUAN_MK2: VWCarInfo("Volkswagen Tiguan 2019-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.TOURAN_MK2: VWCarInfo("Volkswagen Touran 2017"),
CAR.TRANSPORTER_T61: [
VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS]),
VWCarInfo("Volkswagen California 2021", footnotes=[Footnote.VW_HARNESS]),
VWCarInfo("Volkswagen Caravelle 2020", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
VWCarInfo("Volkswagen California 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
],
CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_HARNESS]),
CAR.TROC_MK1: VWCarInfo("Volkswagen T-Roc 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
CAR.AUDI_A3_MK3: [
VWCarInfo("Audi A3 2014-19", "ACC + Lane Assist"),
VWCarInfo("Audi A3 Sportback e-tron 2017-18", "ACC + Lane Assist"),

@ -132,7 +132,6 @@ std::unordered_map<std::string, uint32_t> keys = {
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
{"LastManagerExitReason", CLEAR_ON_MANAGER_START},
{"LastPeripheralPandaType", PERSISTENT},
{"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastSystemShutdown", CLEAR_ON_MANAGER_START},
{"LastUpdateException", CLEAR_ON_MANAGER_START},

@ -1,7 +1,7 @@
#!/usr/bin/env python3
import os
import math
from numbers import Number
from typing import SupportsFloat
from cereal import car, log
from common.numpy_fast import clip
@ -17,6 +17,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise
from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
from selfdrive.controls.lib.latcontrol import LatControl
from selfdrive.controls.lib.longcontrol import LongControl
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
@ -137,6 +138,7 @@ class Controls:
self.LoC = LongControl(self.CP)
self.VM = VehicleModel(self.CP)
self.LaC: LatControl
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
@ -607,7 +609,7 @@ class Controls:
# Ensure no NaNs/Infs
for p in ACTUATOR_FIELDS:
attr = getattr(actuators, p)
if not isinstance(attr, Number):
if not isinstance(attr, SupportsFloat):
continue
if not math.isfinite(attr):

@ -135,6 +135,8 @@ class Alert:
return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
def __gt__(self, alert2) -> bool:
if not isinstance(alert2, Alert):
return False
return self.priority > alert2.priority

@ -29,22 +29,22 @@ class VehicleModel:
CP: Car Parameters
"""
# for math readability, convert long names car params into short names
self.m = CP.mass
self.j = CP.rotationalInertia
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
self.chi = CP.steerRatioRear
self.cF_orig = CP.tireStiffnessFront
self.cR_orig = CP.tireStiffnessRear
self.m: float = CP.mass
self.j: float = CP.rotationalInertia
self.l: float = CP.wheelbase
self.aF: float = CP.centerToFront
self.aR: float = CP.wheelbase - CP.centerToFront
self.chi: float = CP.steerRatioRear
self.cF_orig: float = CP.tireStiffnessFront
self.cR_orig: float = CP.tireStiffnessRear
self.update_params(1.0, CP.steerRatio)
def update_params(self, stiffness_factor: float, steer_ratio: float) -> None:
"""Update the vehicle model with a new stiffness factor and steer ratio"""
self.cF = stiffness_factor * self.cF_orig
self.cR = stiffness_factor * self.cR_orig
self.sR = steer_ratio
self.cF: float = stiffness_factor * self.cF_orig
self.cR: float = stiffness_factor * self.cR_orig
self.sR: float = steer_ratio
def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray:
"""Returns the steady state solution.
@ -221,10 +221,10 @@ def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray
"""
A, B = create_dyn_state_matrices(u, VM)
inp = np.array([[sa], [roll]])
return -solve(A, B) @ inp
return -solve(A, B) @ inp # type: ignore
def calc_slip_factor(VM):
def calc_slip_factor(VM: VehicleModel) -> float:
"""The slip factor is a measure of how the curvature changes with speed
it's positive for Oversteering vehicle, negative (usual case) otherwise.
"""

@ -1,81 +0,0 @@
#!/usr/bin/env python3
# type: ignore
import os
import argparse
import signal
from collections import defaultdict
import cereal.messaging as messaging
def sigint_handler(signal, frame):
print("handler!")
exit(0)
signal.signal(signal.SIGINT, sigint_handler)
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()
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState'], addr=args.addr)
msg_cnt = 0
stats = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0})
cnt = 0
total_error = 0
while messaging.recv_one(carControl):
sm.update()
msg_cnt += 1
actual_speed = sm['carState'].vEgo
enabled = sm['controlsState'].enabled
steer_override = sm['controlsState'].steerOverride
# must be above 10 m/s, engaged and not overriding steering
if actual_speed > 10.0 and enabled and not steer_override:
cnt += 1
# wait 5 seconds after engage/override
if cnt >= 500:
# calculate error before rounding
actual_angle = sm['controlsState'].angleSteers
desired_angle = sm['carControl'].actuators.steeringAngleDeg
angle_error = abs(desired_angle - actual_angle)
# round numbers
actual_angle = round(actual_angle, 1)
desired_angle = round(desired_angle, 1)
angle_error = round(angle_error, 2)
angle_abs = int(abs(round(desired_angle, 0)))
# collect stats
stats[angle_abs]["err"] += angle_error
stats[angle_abs]["cnt"] += 1
if actual_angle == desired_angle:
stats[angle_abs]["="] += 1
else:
if desired_angle == 0.:
overshoot = True
else:
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle
stats[angle_abs]["+" if overshoot else "-"] += 1
else:
cnt = 0
if msg_cnt % 100 == 0:
print(chr(27) + "[2J")
if cnt != 0:
print("COLLECTING ...")
else:
print("DISABLED (speed too low, not engaged, or steer override)")
for k in sorted(stats.keys()):
v = stats[k]
print(f'angle: {k:#2} | error: {round(v["err"] / v["cnt"], 2):2.2f} | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#4}% | -:{int(v["-"] / v["cnt"] * 100):#3}% | count: {v["cnt"]:#4}')

@ -88,7 +88,7 @@ if __name__ == "__main__":
pass
print("Top CPU usage:")
for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]:
for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]: # type: ignore
print(f"{k.rjust(70)} {v:.2f} %")
print()

@ -0,0 +1,23 @@
#!/usr/bin/bash
set -e
cd /sys/kernel/tracing
echo 1 > tracing_on
echo boot > trace_clock
echo 1000 > buffer_size_kb
# /sys/kernel/tracing/available_events
echo 1 > events/irq/enable
echo 1 > events/sched/enable
echo 1 > events/kgsl/enable
echo 1 > events/camera/enable
echo 1 > events/workqueue/enable
echo > trace
sleep 5
echo 0 > tracing_on
cp trace /tmp/trace
chown comma: /tmp/trace
echo /tmp/trace

@ -25,7 +25,7 @@ if __name__ == "__main__":
# 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)
outputs = sorted(inputs + outputs, key=lambda x: x.logMonoTime) # type: ignore
fn = f"{args.route}_{args.process}.bz2"
save_log(fn, outputs)

@ -7,7 +7,7 @@ import struct
import subprocess
import time
import os
from typing import Generator
from typing import Dict, Generator, Union
SPARSE_CHUNK_FMT = struct.Struct('H2xI4x')
@ -99,26 +99,30 @@ def get_partition_path(target_slot_number: int, partition: dict) -> str:
return path
def verify_partition(target_slot_number: int, partition: dict) -> bool:
def verify_partition(target_slot_number: int, partition: Dict[str, Union[str, int]]) -> bool:
full_check = partition['full_check']
path = get_partition_path(target_slot_number, partition)
partition_size = partition['size']
if not isinstance(partition['size'], int):
return False
partition_size: int = partition['size']
if not isinstance(partition['hash_raw'], str):
return False
partition_hash: str = partition['hash_raw']
with open(path, 'rb+') as out:
if full_check:
raw_hash = hashlib.sha256()
pos = 0
chunk_size = 1024 * 1024
pos, chunk_size = 0, 1024 * 1024
while pos < partition_size:
n = min(chunk_size, partition_size - pos)
raw_hash.update(out.read(n))
pos += n
return raw_hash.hexdigest().lower() == partition['hash_raw'].lower()
return raw_hash.hexdigest().lower() == partition_hash.lower()
else:
out.seek(partition_size)
return out.read(64) == partition['hash_raw'].lower().encode()
return out.read(64) == partition_hash.lower().encode()
def clear_partition_hash(target_slot_number: int, partition: dict) -> None:

@ -48,7 +48,7 @@ class Calibration:
def is_calibration_valid(rpy: np.ndarray) -> bool:
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1])
return (PITCH_LIMITS[0] < rpy[1] < PITCH_LIMITS[1]) and (YAW_LIMITS[0] < rpy[2] < YAW_LIMITS[1]) # type: ignore
def sanity_clip(rpy: np.ndarray) -> np.ndarray:

@ -108,7 +108,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
class TestLocationdProc(unittest.TestCase):
MAX_WAITS = 1000
LLD_MSGS = {'gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'}
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration']
def setUp(self):
random.seed(123489234)

@ -5,11 +5,11 @@ libs = [common, cereal, messaging, visionipc,
'avformat', 'avcodec', 'swscale', 'avutil',
'yuv', 'OpenCL', 'pthread']
src = ['logger.cc', 'loggerd.cc', 'video_writer.cc', 'remote_encoder.cc']
src = ['logger.cc', 'loggerd.cc', 'encoder/video_writer.cc', 'remote_encoder.cc', 'encoder/encoder.cc']
if arch == "larch64":
src += ['v4l_encoder.cc']
src += ['encoder/v4l_encoder.cc']
else:
src += ['raw_logger.cc']
src += ['encoder/ffmpeg_encoder.cc']
if arch == "Darwin":
# fix OpenCL
@ -20,8 +20,7 @@ logger_lib = env.Library('logger', src)
libs.insert(0, logger_lib)
env.Program('loggerd', ['main.cc'], LIBS=libs)
if arch == "larch64":
env.Program('encoderd', ['encoderd.cc'], LIBS=libs)
env.Program('encoderd', ['encoderd.cc'], LIBS=libs)
env.Program('bootlog.cc', LIBS=libs)
if GetOption('test'):

@ -1,13 +0,0 @@
#pragma once
#include <cstdint>
#include "cereal/visionipc/visionipc.h"
class VideoEncoder {
public:
virtual ~VideoEncoder() {}
virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra) = 0;
virtual void encoder_open(const char* path) = 0;
virtual void encoder_close() = 0;
};

@ -0,0 +1,79 @@
#include <cassert>
#include "selfdrive/loggerd/encoder/encoder.h"
VideoEncoder::~VideoEncoder() {}
void VideoEncoder::publisher_init() {
// publish
service_name = this->type == DriverCam ? "driverEncodeData" :
(this->type == WideRoadCam ? "wideRoadEncodeData" :
(this->in_width == this->out_width ? "roadEncodeData" : "qRoadEncodeData"));
pm.reset(new PubMaster({service_name}));
}
void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) {
// broadcast packet
MessageBuilder msg;
auto event = msg.initEvent(true);
auto edat = (e->type == DriverCam) ? event.initDriverEncodeData() :
((e->type == WideRoadCam) ? event.initWideRoadEncodeData() :
(e->in_width == e->out_width ? event.initRoadEncodeData() : event.initQRoadEncodeData()));
auto edata = edat.initIdx();
edata.setFrameId(extra.frame_id);
edata.setTimestampSof(extra.timestamp_sof);
edata.setTimestampEof(extra.timestamp_eof);
edata.setType(e->codec);
edata.setEncodeId(idx);
edata.setSegmentNum(segment_num);
edata.setSegmentId(idx);
edata.setFlags(flags);
edata.setLen(dat.size());
edat.setData(dat);
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg));
auto bytes = words->asBytes();
e->pm->send(e->service_name, bytes.begin(), bytes.size());
if (e->write) {
e->to_write.push(words);
} else {
delete words;
}
}
// TODO: writing should be moved to loggerd
void VideoEncoder::write_handler(VideoEncoder *e, const char *path) {
VideoWriter writer(path, e->filename, e->codec != cereal::EncodeIndex::Type::FULL_H_E_V_C, e->out_width, e->out_height, e->fps, e->codec);
bool first = true;
kj::Array<capnp::word>* out_buf;
while ((out_buf = e->to_write.pop())) {
capnp::FlatArrayMessageReader cmsg(*out_buf);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto edata = (e->type == DriverCam) ? event.getDriverEncodeData() :
((e->type == WideRoadCam) ? event.getWideRoadEncodeData() :
(e->in_width == e->out_width ? event.getRoadEncodeData() : event.getQRoadEncodeData()));
auto idx = edata.getIdx();
auto flags = idx.getFlags();
if (first) {
assert(flags & V4L2_BUF_FLAG_KEYFRAME);
auto header = edata.getHeader();
writer.write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
first = false;
}
// dangerous cast from const, but should be fine
auto data = edata.getData();
if (data.size() > 0) {
writer.write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
}
// free the data
delete out_buf;
}
// VideoWriter is freed on out of scope
}

@ -0,0 +1,60 @@
#pragma once
#include <cassert>
#include <cstdint>
#include <thread>
#include "cereal/messaging/messaging.h"
#include "cereal/visionipc/visionipc.h"
#include "selfdrive/common/queue.h"
#include "selfdrive/loggerd/encoder/video_writer.h"
#include "selfdrive/camerad/cameras/camera_common.h"
#define V4L2_BUF_FLAG_KEYFRAME 8
class VideoEncoder {
public:
VideoEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write)
: filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps),
bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { }
virtual ~VideoEncoder();
virtual int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra) = 0;
virtual void encoder_open(const char* path) = 0;
virtual void encoder_close() = 0;
void publisher_init();
static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
void writer_open(const char* path) {
if (this->write) write_handler_thread = std::thread(VideoEncoder::write_handler, this, path);
}
void writer_close() {
if (this->write) {
to_write.push(NULL);
write_handler_thread.join();
}
assert(to_write.empty());
}
protected:
bool write;
const char* filename;
int in_width, in_height;
int out_width, out_height, fps;
int bitrate;
cereal::EncodeIndex::Type codec;
CameraType type;
private:
// publishing
std::unique_ptr<PubMaster> pm;
const char *service_name;
// writing support
static void write_handler(VideoEncoder *e, const char *path);
std::thread write_handler_thread;
SafeQueue<kj::Array<capnp::word>* > to_write;
};

@ -1,6 +1,6 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include "selfdrive/loggerd/raw_logger.h"
#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h"
#include <fcntl.h>
#include <unistd.h>
@ -22,10 +22,9 @@ extern "C" {
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
RawLogger::RawLogger(const char* filename, CameraType type, int in_width, int in_height, int fps,
int bitrate, bool h265, int out_width, int out_height, bool write)
: in_width_(in_width), in_height_(in_height), filename(filename), fps(fps) {
// TODO: respect write arg
const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
void FfmpegEncoder::encoder_init() {
frame = av_frame_alloc();
assert(frame);
frame->format = AV_PIX_FMT_YUV420P;
@ -38,30 +37,43 @@ RawLogger::RawLogger(const char* filename, CameraType type, int in_width, int in
if (in_width != out_width || in_height != out_height) {
downscale_buf.resize(out_width * out_height * 3 / 2);
}
publisher_init();
}
RawLogger::~RawLogger() {
FfmpegEncoder::~FfmpegEncoder() {
encoder_close();
av_frame_free(&frame);
}
void RawLogger::encoder_open(const char* path) {
writer = new VideoWriter(path, this->filename, true, frame->width, frame->height, this->fps, false, true);
// write the header
writer->write(NULL, 0, 0, true, false);
void FfmpegEncoder::encoder_open(const char* path) {
const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
this->codec_ctx = avcodec_alloc_context3(codec);
assert(this->codec_ctx);
this->codec_ctx->width = frame->width;
this->codec_ctx->height = frame->height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, fps };
int err = avcodec_open2(this->codec_ctx, codec, NULL);
assert(err >= 0);
writer_open(path);
is_open = true;
segment_num++;
counter = 0;
}
void RawLogger::encoder_close() {
void FfmpegEncoder::encoder_close() {
if (!is_open) return;
delete writer;
writer_close();
is_open = false;
}
int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra) {
assert(in_width == this->in_width_);
assert(in_height == this->in_height_);
int FfmpegEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width_, int in_height_, VisionIpcBufExtra *extra) {
assert(in_width_ == this->in_width);
assert(in_height_ == this->in_height);
if (downscale_buf.size() > 0) {
uint8_t *out_y = downscale_buf.data();
@ -88,7 +100,7 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
int ret = counter;
int err = avcodec_send_frame(writer->codec_ctx, frame);
int err = avcodec_send_frame(this->codec_ctx, frame);
if (err < 0) {
LOGE("avcodec_send_frame error %d", err);
ret = -1;
@ -99,7 +111,7 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
pkt.data = NULL;
pkt.size = 0;
while (ret >= 0) {
err = avcodec_receive_packet(writer->codec_ctx, &pkt);
err = avcodec_receive_packet(this->codec_ctx, &pkt);
if (err == AVERROR_EOF) {
break;
} else if (err == AVERROR(EAGAIN)) {
@ -112,7 +124,15 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui
break;
}
writer->write(pkt.data, pkt.size, pkt.pts, false, pkt.flags & AV_PKT_FLAG_KEY);
if (env_debug_encoder) {
printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", this->filename, pkt.size, pkt.flags, counter, extra->frame_id);
}
publisher_publish(this, segment_num, counter, *extra,
(pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));
counter++;
}
av_packet_unref(&pkt);

@ -0,0 +1,38 @@
#pragma once
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
}
#include "selfdrive/loggerd/encoder/encoder.h"
#include "selfdrive/loggerd/loggerd.h"
#include "selfdrive/loggerd/encoder/video_writer.h"
class FfmpegEncoder : public VideoEncoder {
public:
FfmpegEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) :
VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); }
~FfmpegEncoder();
void encoder_init();
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width_, int in_height_, VisionIpcBufExtra *extra);
void encoder_open(const char* path);
void encoder_close();
private:
int segment_num = -1;
int counter = 0;
bool is_open = false;
AVCodecContext *codec_ctx;
AVFrame *frame = NULL;
std::vector<uint8_t> downscale_buf;
};

@ -2,7 +2,7 @@
#include <sys/ioctl.h>
#include <poll.h>
#include "selfdrive/loggerd/v4l_encoder.h"
#include "selfdrive/loggerd/encoder/v4l_encoder.h"
#include "selfdrive/common/util.h"
#include "selfdrive/common/timing.h"
@ -67,42 +67,6 @@ static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count)
checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
}
// TODO: writing should be moved to loggerd
void V4LEncoder::write_handler(V4LEncoder *e, const char *path) {
VideoWriter writer(path, e->filename, !e->h265, e->width, e->height, e->fps, e->h265, false);
bool first = true;
kj::Array<capnp::word>* out_buf;
while ((out_buf = e->to_write.pop())) {
capnp::FlatArrayMessageReader cmsg(*out_buf);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto edata = (e->type == DriverCam) ? event.getDriverEncodeData() :
((e->type == WideRoadCam) ? event.getWideRoadEncodeData() :
(e->h265 ? event.getRoadEncodeData() : event.getQRoadEncodeData()));
auto idx = edata.getIdx();
auto flags = idx.getFlags();
if (first) {
assert(flags & V4L2_BUF_FLAG_KEYFRAME);
auto header = edata.getHeader();
writer.write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
first = false;
}
// dangerous cast from const, but should be fine
auto data = edata.getData();
if (data.size() > 0) {
writer.write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
}
// free the data
delete out_buf;
}
// VideoWriter is freed on out of scope
}
void V4LEncoder::dequeue_handler(V4LEncoder *e) {
std::string dequeue_thread_name = "dq-"+std::string(e->filename);
util::set_thread_name(dequeue_thread_name.c_str());
@ -138,7 +102,6 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
// eof packet, we exit
if (flags & V4L2_QCOM_BUF_FLAG_EOS) {
if (e->write) e->to_write.push(NULL);
exit = true;
} else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) {
// save header
@ -146,37 +109,9 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
} else {
VisionIpcBufExtra extra = e->extras.pop();
assert(extra.timestamp_eof/1000 == ts); // stay in sync
frame_id = extra.frame_id;
++idx;
// broadcast packet
MessageBuilder msg;
auto event = msg.initEvent(true);
auto edat = (e->type == DriverCam) ? event.initDriverEncodeData() :
((e->type == WideRoadCam) ? event.initWideRoadEncodeData() :
(e->h265 ? event.initRoadEncodeData() : event.initQRoadEncodeData()));
auto edata = edat.initIdx();
edata.setFrameId(extra.frame_id);
edata.setTimestampSof(extra.timestamp_sof);
edata.setTimestampEof(extra.timestamp_eof);
edata.setType(e->h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264);
edata.setEncodeId(idx);
edata.setSegmentNum(e->segment_num);
edata.setSegmentId(idx);
edata.setFlags(flags);
edata.setLen(bytesused);
edat.setData(kj::arrayPtr<capnp::byte>(buf, bytesused));
if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg));
auto bytes = words->asBytes();
e->pm->send(e->service_name, bytes.begin(), bytes.size());
if (e->write) {
e->to_write.push(words);
} else {
delete words;
}
e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
}
if (env_debug_encoder) {
@ -196,12 +131,7 @@ void V4LEncoder::dequeue_handler(V4LEncoder *e) {
}
}
V4LEncoder::V4LEncoder(
const char* filename, CameraType type, int in_width, int in_height,
int fps, int bitrate, bool h265, int out_width, int out_height, bool write)
: type(type), in_width_(in_width), in_height_(in_height),
filename(filename), h265(h265),
width(out_width), height(out_height), fps(fps), write(write) {
void V4LEncoder::encoder_init() {
fd = open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK);
assert(fd >= 0);
@ -218,7 +148,7 @@ V4LEncoder::V4LEncoder(
// downscales are free with v4l
.width = (unsigned int)out_width,
.height = (unsigned int)out_height,
.pixelformat = h265 ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264,
.pixelformat = (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264,
.field = V4L2_FIELD_ANY,
.colorspace = V4L2_COLORSPACE_DEFAULT,
}
@ -272,7 +202,7 @@ V4LEncoder::V4LEncoder(
}
}
if (h265) {
if (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) {
struct v4l2_control ctrls[] = {
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN},
{ .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5},
@ -321,23 +251,18 @@ V4LEncoder::V4LEncoder(
free_buf_in.push(i);
}
// publish
service_name = this->type == DriverCam ? "driverEncodeData" :
(this->type == WideRoadCam ? "wideRoadEncodeData" :
(this->h265 ? "roadEncodeData" : "qRoadEncodeData"));
pm.reset(new PubMaster({service_name}));
publisher_init();
}
void V4LEncoder::encoder_open(const char* path) {
dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this);
if (this->write) write_handler_thread = std::thread(V4LEncoder::write_handler, this, path);
writer_open(path);
this->is_open = true;
this->counter = 0;
}
int V4LEncoder::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra) {
int in_width_, int in_height_, VisionIpcBufExtra *extra) {
assert(in_width == in_width_);
assert(in_height == in_height_);
assert(is_open);
@ -383,8 +308,7 @@ void V4LEncoder::encoder_close() {
// join waits for V4L2_QCOM_BUF_FLAG_EOS
dequeue_handler_thread.join();
assert(extras.empty());
if (this->write) write_handler_thread.join();
assert(to_write.empty());
writer_close();
}
this->is_open = false;
}

@ -1,17 +1,19 @@
#pragma once
#include "selfdrive/common/queue.h"
#include "selfdrive/loggerd/encoder.h"
#include "selfdrive/loggerd/loggerd.h"
#include "selfdrive/loggerd/video_writer.h"
#include "selfdrive/loggerd/encoder/encoder.h"
#include "selfdrive/loggerd/encoder/video_writer.h"
#define BUF_IN_COUNT 7
#define BUF_OUT_COUNT 6
class V4LEncoder : public VideoEncoder {
public:
V4LEncoder(const char* filename, CameraType type, int width, int height, int fps, int bitrate, bool h265, int out_width, int out_height, bool write = true);
V4LEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) :
VideoEncoder(filename, type, in_width, in_height, fps, bitrate, codec, out_width, out_height, write) { encoder_init(); }
~V4LEncoder();
void encoder_init();
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra);
void encoder_open(const char* path);
@ -19,16 +21,11 @@ public:
private:
int fd;
const char* filename;
CameraType type;
unsigned int in_width_, in_height_;
bool h265;
bool is_open = false;
int segment_num = -1;
int counter = 0;
std::unique_ptr<PubMaster> pm;
const char *service_name;
SafeQueue<VisionIpcBufExtra> extras;
static void dequeue_handler(V4LEncoder *e);
std::thread dequeue_handler_thread;
@ -36,13 +33,4 @@ private:
VisionBuf buf_in[BUF_IN_COUNT];
VisionBuf buf_out[BUF_OUT_COUNT];
SafeQueue<unsigned int> free_buf_in;
SafeQueue<VisionIpcBufExtra> extras;
// writing support
int width, height, fps;
bool write;
static void write_handler(V4LEncoder *e, const char *path);
std::thread write_handler_thread;
SafeQueue<kj::Array<capnp::word>* > to_write;
};

@ -1,14 +1,13 @@
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#include <cassert>
#include <cstdlib>
#include "selfdrive/loggerd/video_writer.h"
#include "selfdrive/loggerd/encoder/video_writer.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, bool h265, bool raw)
: remuxing(remuxing), raw(raw) {
VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec)
: remuxing(remuxing) {
raw = codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS;
vid_path = util::string_format("%s/%s", path, filename);
lock_path = util::string_format("%s/%s.lock", path, filename);
@ -22,27 +21,24 @@ VideoWriter::VideoWriter(const char *path, const char *filename, bool remuxing,
assert(this->ofmt_ctx);
// set codec correctly. needed?
av_register_all();
AVCodec *codec = NULL;
assert(!h265);
codec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264);
assert(codec);
assert(codec != cereal::EncodeIndex::Type::FULL_H_E_V_C);
const AVCodec *avcodec = avcodec_find_encoder(raw ? AV_CODEC_ID_FFVHUFF : AV_CODEC_ID_H264);
assert(avcodec);
this->codec_ctx = avcodec_alloc_context3(codec);
this->codec_ctx = avcodec_alloc_context3(avcodec);
assert(this->codec_ctx);
this->codec_ctx->width = width;
this->codec_ctx->height = height;
this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
this->codec_ctx->time_base = (AVRational){ 1, fps };
if (raw) {
// since the codec is actually used, we open it
int err = avcodec_open2(this->codec_ctx, codec, NULL);
if (codec == cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS) {
// without this, there's just noise
int err = avcodec_open2(this->codec_ctx, avcodec, NULL);
assert(err >= 0);
}
this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? codec : NULL);
this->out_stream = avformat_new_stream(this->ofmt_ctx, raw ? avcodec : NULL);
assert(this->out_stream);
int err = avio_open(&this->ofmt_ctx->pb, this->vid_path.c_str(), AVIO_FLAG_WRITE);
@ -64,7 +60,7 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
if (remuxing) {
if (codecconfig) {
if (data) {
if (len > 0) {
codec_ctx->extradata = (uint8_t*)av_mallocz(len + AV_INPUT_BUFFER_PADDING_SIZE);
codec_ctx->extradata_size = len;
memcpy(codec_ctx->extradata, data, len);
@ -94,7 +90,7 @@ void VideoWriter::write(uint8_t *data, int len, long long timestamp, bool codecc
int err = av_interleaved_write_frame(ofmt_ctx, &pkt);
if (err < 0) { LOGW("ts encoder write issue"); }
av_free_packet(&pkt);
av_packet_unref(&pkt);
}
}
}

@ -4,19 +4,22 @@
extern "C" {
#include <libavformat/avformat.h>
#include <libavcodec/avcodec.h>
}
#include "cereal/messaging/messaging.h"
class VideoWriter {
public:
VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, bool h265, bool raw);
VideoWriter(const char *path, const char *filename, bool remuxing, int width, int height, int fps, cereal::EncodeIndex::Type codec);
void write(uint8_t *data, int len, long long timestamp, bool codecconfig, bool keyframe);
~VideoWriter();
AVCodecContext *codec_ctx;
private:
std::string vid_path, lock_path;
FILE *of = nullptr;
AVCodecContext *codec_ctx;
AVFormatContext *ofmt_ctx;
AVStream *out_stream;
bool remuxing, raw;

@ -54,12 +54,14 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) {
// main encoder
encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height,
cam_info.fps, cam_info.bitrate, cam_info.is_h265,
cam_info.fps, cam_info.bitrate,
cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264,
buf_info.width, buf_info.height, false));
// qcamera encoder
if (cam_info.has_qcamera) {
encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height,
qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265,
qcam_info.fps, qcam_info.bitrate,
qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264,
qcam_info.frame_width, qcam_info.frame_height, false));
}
}

@ -1,6 +1,6 @@
#include "selfdrive/loggerd/loggerd.h"
#include "selfdrive/loggerd/remote_encoder.h"
bool USE_REMOTE_ENCODER = false;
bool env_remote_encoder = getenv("REMOTE_ENCODER") != NULL;
ExitHandler do_exit;
@ -65,13 +65,15 @@ void encoder_thread(LoggerdState *s, const LogCameraInfo &cam_info) {
// main encoder
encoders.push_back(new Encoder(cam_info.filename, cam_info.type, buf_info.width, buf_info.height,
cam_info.fps, cam_info.bitrate, cam_info.is_h265,
cam_info.fps, cam_info.bitrate,
cam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264,
buf_info.width, buf_info.height, cam_info.record));
// qcamera encoder
if (cam_info.has_qcamera) {
encoders.push_back(new Encoder(qcam_info.filename, cam_info.type, buf_info.width, buf_info.height,
qcam_info.fps, qcam_info.bitrate, qcam_info.is_h265,
qcam_info.frame_width, qcam_info.frame_height));
qcam_info.fps, qcam_info.bitrate,
qcam_info.is_h265 ? cereal::EncodeIndex::Type::FULL_H_E_V_C : cereal::EncodeIndex::Type::QCAMERA_H264,
qcam_info.frame_width, qcam_info.frame_height, true));
}
}
@ -200,7 +202,7 @@ void loggerd_thread() {
// subscribe to all socks
for (const auto& it : services) {
const bool encoder = USE_REMOTE_ENCODER & (strcmp(it.name+strlen(it.name)-strlen("EncodeData"), "EncodeData") == 0);
const bool encoder = env_remote_encoder && (strcmp(it.name+strlen(it.name)-strlen("EncodeData"), "EncodeData") == 0);
if (!it.should_log && !encoder) continue;
LOGD("logging %s (on port %d)", it.name, it.port);
@ -226,7 +228,11 @@ void loggerd_thread() {
std::vector<std::thread> encoder_threads;
for (const auto &cam : cameras_logged) {
if (cam.enable) {
if (env_remote_encoder) {
if (cam.has_qcamera) { s.max_waiting++; }
} else {
encoder_threads.push_back(std::thread(encoder_thread, &s, cam));
}
if (cam.trigger_rotate) s.max_waiting++;
}
}

@ -22,14 +22,14 @@
#include "selfdrive/common/util.h"
#include "selfdrive/hardware/hw.h"
#include "selfdrive/loggerd/encoder.h"
#include "selfdrive/loggerd/encoder/encoder.h"
#include "selfdrive/loggerd/logger.h"
#ifdef QCOM2
#include "selfdrive/loggerd/v4l_encoder.h"
#include "selfdrive/loggerd/encoder/v4l_encoder.h"
#define Encoder V4LEncoder
#else
#include "selfdrive/loggerd/raw_logger.h"
#define Encoder RawLogger
#include "selfdrive/loggerd/encoder/ffmpeg_encoder.h"
#define Encoder FfmpegEncoder
#endif
constexpr int MAIN_FPS = 20;

@ -1,41 +0,0 @@
#pragma once
#include <cstdio>
#include <cstdlib>
#include <string>
#include <vector>
extern "C" {
#include <libavcodec/avcodec.h>
#include <libavformat/avformat.h>
#include <libavutil/imgutils.h>
}
#include "selfdrive/loggerd/encoder.h"
#include "selfdrive/loggerd/loggerd.h"
#include "selfdrive/loggerd/video_writer.h"
class RawLogger : public VideoEncoder {
public:
RawLogger(const char* filename, CameraType type, int in_width, int in_height, int fps,
int bitrate, bool h265, int out_width, int out_height, bool write = true);
~RawLogger();
int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
int in_width, int in_height, VisionIpcBufExtra *extra);
void encoder_open(const char* path);
void encoder_close();
private:
const char* filename;
//bool write;
int fps;
int counter = 0;
bool is_open = false;
int in_width_, in_height_;
AVFrame *frame = NULL;
std::vector<uint8_t> downscale_buf;
VideoWriter *writer = NULL;
};

@ -7,11 +7,17 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
((name == "qRoadEncodeData") ? qcam_info : cameras_logged[0]));
if (!cam_info.record) return 0; // TODO: handle this by not subscribing
// rotation happened, process the queue (happens before the current message)
int bytes_count = 0;
if (re.logger_segment != s->rotate_segment) {
re.logger_segment = s->rotate_segment;
for (auto &qmsg: re.q) {
bytes_count += handle_encoder_msg(s, qmsg, name, re);
}
re.q.clear();
}
// TODO: AlignedBuffer is making a copy and allocing
//AlignedBuffer aligned_buf;
//capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg->getData(), msg->getSize()));
// extract the message
capnp::FlatArrayMessageReader cmsg(kj::ArrayPtr<capnp::word>((capnp::word *)msg->getData(), msg->getSize()));
auto event = cmsg.getRoot<cereal::Event>();
auto edata = (name == "driverEncodeData") ? event.getDriverEncodeData() :
@ -20,15 +26,6 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
auto idx = edata.getIdx();
auto flags = idx.getFlags();
// rotation happened, process the queue (happens before the current message)
if (re.logger_segment != s->rotate_segment) {
re.logger_segment = s->rotate_segment;
for (auto &qmsg: re.q) {
bytes_count += handle_encoder_msg(s, qmsg, name, re);
}
re.q.clear();
}
if (!re.writer) {
// only create on iframe
if (flags & V4L2_BUF_FLAG_KEYFRAME) {
@ -38,9 +35,8 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
re.dropped_frames = 0;
}
re.writer.reset(new VideoWriter(s->segment_path,
cam_info.filename, !cam_info.is_h265,
cam_info.frame_width, cam_info.frame_height,
cam_info.fps, cam_info.is_h265, false));
cam_info.filename, idx.getType() != cereal::EncodeIndex::Type::FULL_H_E_V_C,
cam_info.frame_width, cam_info.frame_height, cam_info.fps, idx.getType()));
// write the header
auto header = edata.getHeader();
re.writer->write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
@ -74,8 +70,10 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct
if (name == "roadEncodeData") { evt.setRoadEncodeIdx(idx); }
auto new_msg = bmsg.toBytes();
logger_log(&s->logger, (uint8_t *)new_msg.begin(), new_msg.size(), true); // always in qlog?
delete msg;
bytes_count += new_msg.size();
// this frees the message
delete msg;
}
return bytes_count;

@ -1,5 +1,4 @@
#include "selfdrive/loggerd/video_writer.h"
#define V4L2_BUF_FLAG_KEYFRAME 0x00000008
#include "selfdrive/loggerd/encoder/video_writer.h"
struct RemoteEncoder {
std::unique_ptr<VideoWriter> writer;

@ -69,8 +69,8 @@ class Uploader():
self.immediate_count = 0
# stats for last successfully uploaded file
self.last_time = 0
self.last_speed = 0
self.last_time = 0.0
self.last_speed = 0.0
self.last_filename = ""
self.immediate_folders = ["crash/", "boot/"]

@ -4,7 +4,7 @@ from common.xattr import getxattr as getattr1
from common.xattr import setxattr as setattr1
cached_attributes: Dict[Tuple, bytes] = {}
def getxattr(path: str, attr_name: bytes) -> bytes:
def getxattr(path: str, attr_name: str) -> bytes:
if (path, attr_name) not in cached_attributes:
response = getattr1(path, attr_name)
cached_attributes[(path, attr_name)] = response

@ -8,10 +8,10 @@ from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProces
WEBCAM = os.getenv("USE_WEBCAM") is not None
def driverview(started: bool, params: Params, CP: car.CarParams) -> bool:
return params.get_bool("IsDriverViewEnabled")
return params.get_bool("IsDriverViewEnabled") # type: ignore
def notcar(started: bool, params: Params, CP: car.CarParams) -> bool:
return CP.notCar
return CP.notCar # type: ignore
def logging(started, params, CP: car.CarParams) -> bool:
run = (not CP.notCar) or not params.get_bool("DisableLogging")

@ -23,9 +23,9 @@ def save_thneed(jdat, fn):
if 'data' in o:
new_weights.append(o['data'])
del o['data']
new_weights = b''.join(new_weights)
new_weights_bytes = b''.join(new_weights)
with open(fn, "wb") as f:
j = json.dumps(jdat, ensure_ascii=False).encode('latin_1')
f.write(struct.pack("I", len(j)))
f.write(j)
f.write(new_weights)
f.write(new_weights_bytes)

@ -56,7 +56,7 @@ if __name__ == "__main__":
pr_err /= len(car)
speed_err /= len(car)
print("avg psuedorange err %f avg speed err %f" % (pr_err, speed_err))
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)):
for c in sorted(car, key=lambda x: abs(x[1] - x[3] - pr_err)): # type: ignore
svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed, cno = c
print("svid: %3d pseudorange: %10.2f m speed: %8.2f m/s meas: %12.2f speed: %10.2f meas_err: %10.3f speed_err: %8.3f cno: %d" %
(svid, ublox_psuedorange, ublox_speed, qcom_psuedorange, qcom_speed,

@ -4,17 +4,18 @@ import sys
import subprocess
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
TOKEN_PATH = "/data/azure_token"
def get_url(route_name, segment_num, log_type="rlog"):
ext = "hevc" if log_type.endswith('camera') else "bz2"
return BASE_URL + f"{route_name.replace('|', '/')}/{segment_num}/{log_type}.{ext}"
def upload_file(path, name):
from azure.storage.blob import BlockBlobService # pylint: disable=import-error
sas_token = None
sas_token = os.environ.get("AZURE_TOKEN", None)
if os.path.isfile(TOKEN_PATH):
sas_token = open(TOKEN_PATH).read().strip()
@ -25,6 +26,7 @@ def upload_file(path, name):
service.create_blob_from_path("openpilotci", name, path)
return "https://commadataci.blob.core.windows.net/openpilotci/" + name
if __name__ == "__main__":
for f in sys.argv[1:]:
name = os.path.basename(f)

@ -1,4 +1,4 @@
# process replay
# 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.
@ -12,14 +12,34 @@ Currently the following processes are tested:
* radard
* plannerd
* calibrationd
* dmonitoringd
* locationd
* paramsd
* ubloxd
### 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. carEvents)
--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
openpilot forks can use this test with their own reference logs, by default `test_proccess.py` saves logs locally.
To generate new logs:
`./update_refs.py --no-upload`
`./test_processes.py`
Then, check in the new logs using git-lfs. Make sure to also include the updated `ref_commit` file.
Then, check in the new logs using git-lfs. Make sure to also update the `ref_commit` file to the current commit.

@ -1 +1 @@
14f411de8085d1cc9e467592c90bcaf95447a467
01ab99d416ffa6fb5f96ebd2091be94a3dc6c6bf

@ -61,8 +61,8 @@ def model_replay(lr, frs):
log_msgs = []
last_desire = None
recv_cnt = defaultdict(lambda: 0)
frame_idxs = defaultdict(lambda: 0)
recv_cnt = defaultdict(int)
frame_idxs = defaultdict(int)
# init modeld with valid calibration
cal_msgs = [msg for msg in lr if msg.which() == "liveCalibration"]

@ -24,6 +24,7 @@ from selfdrive.manager.process_config import managed_processes
NUMPY_TOLERANCE = 1e-7
CI = "CI" in os.environ
TIMEOUT = 15
PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__))
ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config'], defaults=({},))

@ -2,12 +2,13 @@
import argparse
import os
import sys
from typing import Any
from typing import Any, Dict
from selfdrive.car.car_helpers import interface_names
from selfdrive.test.openpilotci import get_url
from selfdrive.test.process_replay.compare_logs import compare_logs
from selfdrive.test.process_replay.process_replay import CONFIGS, replay_process, check_enabled
from selfdrive.test.openpilotci import get_url, upload_file
from selfdrive.test.process_replay.compare_logs import compare_logs, save_log
from selfdrive.test.process_replay.process_replay import CONFIGS, PROC_REPLAY_DIR, check_enabled, replay_process
from selfdrive.version import get_commit
from tools.lib.logreader import LogReader
@ -50,32 +51,31 @@ segments = [
excluded_interfaces = ["mock", "ford", "mazda", "tesla"]
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
REF_COMMIT_FN = os.path.join(PROC_REPLAY_DIR, "ref_commit")
# run the full test (including checks) when no args given
FULL_TEST = len(sys.argv) <= 1
def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None):
def test_process(cfg, lr, ref_log_fn, ignore_fields=None, ignore_msgs=None):
if ignore_fields is None:
ignore_fields = []
if ignore_msgs is None:
ignore_msgs = []
cmp_log_path = cmp_log_fn if os.path.exists(cmp_log_fn) else BASE_URL + os.path.basename(cmp_log_fn)
cmp_log_msgs = list(LogReader(cmp_log_path))
ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn)
ref_log_msgs = list(LogReader(ref_log_path))
log_msgs = replay_process(cfg, lr)
# check to make sure openpilot is engaged in the route
if cfg.proc_name == "controlsd":
if not check_enabled(log_msgs):
segment = cmp_log_fn.split("/")[-1].split("_")[0]
segment = ref_log_fn.split("/")[-1].split("_")[0]
raise Exception(f"Route never enabled: {segment}")
try:
return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs, cfg.tolerance)
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)
return str(e), log_msgs
def format_diff(results, ref_commit):
diff1, diff2 = "", ""
@ -94,7 +94,7 @@ def format_diff(results, ref_commit):
diff1 += f"\t\t{diff}\n"
failed = True
elif len(diff):
cnt = {}
cnt: Dict[str, int] = {}
for d in diff:
diff2 += f"\t{str(d)}\n"
@ -106,8 +106,8 @@ def format_diff(results, ref_commit):
failed = True
return diff1, diff2, failed
if __name__ == "__main__":
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output")
# whitelist has precedence over blacklist in case both are defined
@ -123,30 +123,40 @@ if __name__ == "__main__":
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")
args = parser.parse_args()
cars_whitelisted = len(args.whitelist_cars) > 0
procs_whitelisted = len(args.whitelist_procs) > 0
full_test = all(len(x) == 0 for x in (args.whitelist_procs, args.whitelist_cars, args.blacklist_procs, args.blacklist_cars, args.ignore_fields, args.ignore_msgs))
upload = args.update_refs or args.upload_only
if upload:
assert full_test, "Need to run full test when updating refs"
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
try:
ref_commit = open(os.path.join(process_replay_dir, "ref_commit")).read().strip()
ref_commit = open(REF_COMMIT_FN).read().strip()
except FileNotFoundError:
print("couldn't find reference commit")
print("Couldn't find reference commit")
sys.exit(1)
cur_commit = get_commit()
if cur_commit is None:
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:
if full_test:
tested_cars = {c.lower() for c, _ in segments}
untested = (set(interface_names) - set(excluded_interfaces)) - tested_cars
assert len(untested) == 0, f"Cars missing routes: {str(untested)}"
results: Any = {}
for car_brand, segment in segments:
if (cars_whitelisted and car_brand.upper() not in args.whitelist_cars) or \
(not cars_whitelisted and car_brand.upper() in args.blacklist_cars):
if (len(args.whitelist_cars) and car_brand.upper() not in args.whitelist_cars) or \
(not len(args.whitelist_cars) and car_brand.upper() in args.blacklist_cars):
continue
print(f"***** testing route segment {segment} *****\n")
@ -157,23 +167,40 @@ if __name__ == "__main__":
lr = LogReader(get_url(r, n))
for cfg in CONFIGS:
if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \
(not procs_whitelisted and cfg.proc_name in args.blacklist_procs):
if (len(args.whitelist_procs) and cfg.proc_name not in args.whitelist_procs) or \
(not len(args.whitelist_procs) and cfg.proc_name in args.blacklist_procs):
continue
cmp_log_fn = os.path.join(process_replay_dir, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs)
cur_log_fn = os.path.join(PROC_REPLAY_DIR, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2")
if not args.upload_only:
ref_log_fn = os.path.join(PROC_REPLAY_DIR, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
results[segment][cfg.proc_name], log_msgs = test_process(cfg, lr, ref_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 upload:
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)
diff1, diff2, failed = format_diff(results, ref_commit)
with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f:
with open(os.path.join(PROC_REPLAY_DIR, "diff.txt"), "w") as f:
f.write(diff2)
print(diff1)
if failed:
print("TEST FAILED")
print("\n\nTo update the reference logs for this test run:")
print("./update_refs.py")
if not upload:
print("\n\nTo push the new reference logs for this commit run:")
print("./test_processes.py --upload-only")
else:
print("TEST SUCCEEDED")
if upload:
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))

@ -1,38 +0,0 @@
#!/usr/bin/env python3
import os
import sys
from selfdrive.test.openpilotci import upload_file, get_url
from selfdrive.test.process_replay.compare_logs import save_log
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS
from selfdrive.test.process_replay.test_processes import segments
from selfdrive.version import get_commit
from tools.lib.logreader import LogReader
if __name__ == "__main__":
no_upload = "--no-upload" in sys.argv
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
ref_commit = get_commit()
if ref_commit is None:
raise Exception("couldn't get ref commit")
with open(ref_commit_fn, "w") as f:
f.write(ref_commit)
for car_brand, segment in segments:
r, n = segment.rsplit("--", 1)
lr = LogReader(get_url(r, n))
for cfg in CONFIGS:
log_msgs = replay_process(cfg, lr)
log_fn = os.path.join(process_replay_dir, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2")
save_log(log_fn, log_msgs)
if not no_upload:
upload_file(log_fn, os.path.basename(log_fn))
os.remove(log_fn)
print("done")

@ -13,7 +13,7 @@ namespace {
const char frame_vertex_shader[] =
#ifdef __APPLE__
"#version 150 core\n"
"#version 330 core\n"
#else
"#version 300 es\n"
#endif
@ -28,7 +28,7 @@ const char frame_vertex_shader[] =
const char frame_fragment_shader[] =
#ifdef __APPLE__
"#version 150 core\n"
"#version 330 core\n"
#else
"#version 300 es\n"
"precision mediump float;\n"
@ -217,7 +217,7 @@ void CameraViewWidget::paintGL() {
glBindTexture(GL_TEXTURE_2D, textures[i]);
int width = i == 0 ? stream_width : stream_width / 2;
int height = i == 0 ? stream_height : stream_height / 2;
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_LUMINANCE, GL_UNSIGNED_BYTE, address[i]);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GL_RED, GL_UNSIGNED_BYTE, address[i]);
assert(glGetError() == GL_NO_ERROR);
}
@ -246,7 +246,7 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
int width = i == 0 ? stream_width : stream_width / 2;
int height = i == 0 ? stream_height : stream_height / 2;
glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, width, height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, nullptr);
glTexImage2D(GL_TEXTURE_2D, 0, GL_R8, width, height, 0, GL_RED, GL_UNSIGNED_BYTE, nullptr);
assert(glGetError() == GL_NO_ERROR);
}

@ -77,10 +77,16 @@ void CameraServer::pushFrame(CameraType type, FrameReader *fr, const cereal::Enc
if (cam.width != fr->width || cam.height != fr->height) {
cam.width = fr->width;
cam.height = fr->height;
waitFinish();
waitForSent();
startVipcServer();
}
++publishing_;
cam.queue.push({fr, eidx});
}
void CameraServer::waitForSent() {
while (publishing_ > 0) {
std::this_thread::yield();
}
}

@ -11,9 +11,7 @@ public:
CameraServer(std::pair<int, int> camera_size[MAX_CAMERAS] = nullptr, bool send_yuv = false);
~CameraServer();
void pushFrame(CameraType type, FrameReader* fr, const cereal::EncodeIndex::Reader& eidx);
inline void waitFinish() {
while (publishing_ > 0) usleep(0);
}
void waitForSent();
protected:
struct Camera {

@ -383,7 +383,7 @@ void Replay::stream() {
publishMessage(evt);
} else if (camera_server_) {
if (hasFlag(REPLAY_FLAG_FULL_SPEED)) {
camera_server_->waitFinish();
camera_server_->waitForSent();
}
publishFrame(evt);
}
@ -391,7 +391,7 @@ void Replay::stream() {
}
// wait for frame to be sent before unlock.(frameReader may be deleted after unlock)
if (camera_server_) {
camera_server_->waitFinish();
camera_server_->waitForSent();
}
if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) {

@ -77,7 +77,11 @@ bool Route::loadFromLocal() {
}
void Route::addFileToSegment(int n, const QString &file) {
const QString name = QUrl(file).fileName();
QString name = QUrl(file).fileName();
const int pos = name.lastIndexOf("--");
name = pos != -1 ? name.mid(pos + 2) : name;
if (name == "rlog.bz2") {
segments_[n].rlog = file;
} else if (name == "qlog.bz2") {

@ -305,7 +305,7 @@ void precise_nano_sleep(long sleep_ns) {
// spin wait
if (sleep_ns > 0) {
while ((nanos_since_boot() - start_sleep) <= sleep_ns) {
usleep(0);
std::this_thread::yield();
}
}
}

@ -311,7 +311,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool:
cur_hash = run(["git", "rev-parse", "HEAD"], OVERLAY_MERGED).rstrip()
upstream_hash = run(["git", "rev-parse", "@{u}"], OVERLAY_MERGED).rstrip()
new_version = cur_hash != upstream_hash
new_version: bool = cur_hash != upstream_hash
git_fetch_result = check_git_fetch_result(git_fetch_output)
cloudlog.info(f"comparing {cur_hash} to {upstream_hash}")

@ -55,7 +55,7 @@ def get_origin(default: Optional[str] = None) -> Optional[str]:
@cache
def get_normalized_origin(default: Optional[str] = None) -> Optional[str]:
origin = get_origin()
origin: Optional[str] = get_origin()
if origin is None:
return default
@ -74,7 +74,7 @@ def get_version() -> str:
@cache
def get_short_version() -> str:
return get_version().split('-')[0]
return get_version().split('-')[0] # type: ignore
@cache
def is_prebuilt() -> bool:
@ -85,7 +85,7 @@ def is_prebuilt() -> bool:
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
origin = get_origin()
origin: Optional[str] = get_origin()
if origin is None:
return False

@ -71,7 +71,7 @@ def decoder(addr, sock_name, vipc_server, vst, nvidia):
assert len(frames) == 1
img_yuv = frames[0].to_ndarray(format=av.video.format.VideoFormat('yuv420p'))
vipc_server.send(vst, img_yuv.flatten().data, cnt, 0, 0)
vipc_server.send(vst, img_yuv.flatten().data, cnt, cnt*1e9/20, cnt*1e9/20)
cnt += 1
pc_latency = (time.monotonic()-time_q[0])*1000

@ -16,11 +16,24 @@ index = """
<body>
<div id="joyDiv" style="width:100%;height:100%"></div>
<script type="text/javascript">
// Set up gamepad handlers
let gamepad = null;
window.addEventListener("gamepadconnected", function(e) {
gamepad = e.gamepad;
});
window.addEventListener("gamepaddisconnected", function(e) {
gamepad = null;
});
// Create JoyStick object into the DIV 'joyDiv'
var joy = new JoyStick('joyDiv');
setInterval(function(){
var x = -joy.GetX()/100;
var y = joy.GetY()/100;
if (x === 0 && y === 0 && gamepad !== null) {
let gamepadstate = navigator.getGamepads()[gamepad.index];
x = -gamepadstate.axes[0];
y = -gamepadstate.axes[1];
}
let xhr = new XMLHttpRequest();
xhr.open("GET", "/control/"+x+"/"+y);
xhr.send();

@ -46,8 +46,8 @@ class ClientRedirectHandler(BaseHTTPRequestHandler):
return
query = self.path.split('?', 1)[-1]
query = parse_qs(query, keep_blank_values=True)
self.server.query_params = query
query_parsed = parse_qs(query, keep_blank_values=True)
self.server.query_params = query_parsed
self.send_response(200)
self.send_header('Content-type', 'text/plain')

@ -36,9 +36,13 @@ class Bootlog:
return timestamp_to_datetime(self._timestamp)
def __eq__(self, b) -> bool:
if not isinstance(b, Bootlog):
return False
return self.datetime == b.datetime
def __lt__(self, b) -> bool:
if not isinstance(b, Bootlog):
return False
return self.datetime < b.datetime

@ -1,12 +1,12 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<Tab tab_name="tab1" containers="1">
<tabbed_widget name="Main Window" parent="main_window">
<Tab containers="1" tab_name="tab1">
<Container>
<DockSplitter sizes="0.200099;0.200099;0.199606;0.200099;0.200099" orientation="-" count="5">
<DockSplitter count="5" sizes="0.200198;0.199703;0.200198;0.199703;0.200198" orientation="-">
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="62.240001" right="1671.937015" top="81.559998"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="63.929997" top="96.270000" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/deviceState/cpuTempC/0"/>
<curve color="#d62728" name="/deviceState/cpuTempC/1"/>
@ -19,8 +19,8 @@
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="56.440473" right="1671.937015" top="82.418528"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="55.402226" top="98.526775" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/deviceState/pmicTempC/0"/>
<curve color="#d62728" name="/deviceState/gpuTempC/0"/>
@ -29,32 +29,39 @@
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="-0.025000" right="1671.937015" top="1.025000"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="-0.050000" top="2.050000" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/deviceState/thermalStatus"/>
</plot>
</DockArea>
<DockSplitter sizes="0.500152;0.499848" orientation="|" count="2">
<DockSplitter count="3" sizes="0.333333;0.333333;0.333333" orientation="|">
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="4.185483" right="1671.937015" top="14.634767"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="5.045103" top="12.507322" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#ff7f0e" name="/deviceState/powerDrawW"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="29.475000" right="1671.937015" top="51.525000"/>
<limitY/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="0.000000" top="100.000000" right="2052.279275" left="0.000000"/>
<limitY min="0" max="100"/>
<curve color="#9467bd" name="/deviceState/fanSpeedPercentDesired"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="2409.750000" top="5780.250000" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/peripheralState/fanSpeedRpm"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter sizes="0.500152;0.499848" orientation="|" count="2">
<DockSplitter count="2" sizes="0.502632;0.497368" orientation="|">
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="1.600000" right="1671.937015" top="102.400000"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="-0.350000" top="98.350000" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#1f77b4" name="/deviceState/cpuUsagePercent/0"/>
<curve color="#d62728" name="/deviceState/cpuUsagePercent/1"/>
@ -63,8 +70,8 @@
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" flip_x="false" mode="TimeSeries" flip_y="false">
<range left="0.000000" bottom="-2.500000" right="1671.937015" top="102.500000"/>
<plot style="Lines" mode="TimeSeries" flip_x="false" flip_y="false">
<range bottom="-2.500000" top="102.500000" right="2052.279275" left="0.000000"/>
<limitY/>
<curve color="#f14cc1" name="/deviceState/cpuUsagePercent/4"/>
<curve color="#9467bd" name="/deviceState/cpuUsagePercent/5"/>

@ -35,7 +35,7 @@ def replay(route, segment, loop):
frame_idx = {m.roadEncodeIdx.frameId: m.roadEncodeIdx.segmentId for m in msgs if m.which() == 'roadEncodeIdx' and m.roadEncodeIdx.type == 'fullHEVC'}
socks = {}
lag = 0
lag = 0.0
i = 0
max_i = len(msgs) - 2
@ -66,7 +66,7 @@ def replay(route, segment, loop):
lag += (next_msg.logMonoTime - msg.logMonoTime) / 1e9
lag -= time.time() - start_time
dt = max(lag, 0)
dt = max(lag, 0.0)
lag -= dt
time.sleep(dt)

@ -44,8 +44,8 @@ def parse_args(add_args=None):
class VehicleState:
def __init__(self):
self.speed = 0
self.angle = 0
self.speed = 0.0
self.angle = 0.0
self.bearing_deg = 0.0
self.vel = carla.Vector3D()
self.cruise_button = 0
@ -111,7 +111,7 @@ class Camerad:
rgb_cl = cl_array.to_device(self.queue, rgb)
yuv_cl = cl_array.empty_like(rgb_cl)
self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait()
yuv = np.resize(yuv_cl.get(), np.int32(rgb.size / 2))
yuv = np.resize(yuv_cl.get(), rgb.size // 2)
eof = int(frame_id * 0.05 * 1e9)
# TODO: remove RGB send once the last RGB vipc subscriber is removed
@ -397,7 +397,7 @@ class CarlaBridge:
cruise_button = 0
throttle_out = steer_out = brake_out = 0.0
throttle_op = steer_op = brake_op = 0
throttle_op = steer_op = brake_op = 0.0
throttle_manual = steer_manual = brake_manual = 0.0
# --------------Step 1-------------------------------

@ -0,0 +1,131 @@
#!/usr/bin/env python3
# type: ignore
import os
import time
import argparse
import signal
from collections import defaultdict
import cereal.messaging as messaging
def sigint_handler(signal, frame):
print("handler!")
exit(0)
signal.signal(signal.SIGINT, sigint_handler)
if __name__ == "__main__":
parser = argparse.ArgumentParser(description='Sniff a communication socket')
parser.add_argument('control_type', help="[pid|indi|lqr|angle]")
parser.add_argument('--addr', default='127.0.0.1', help="IP address for optional ZMQ listener, default to msgq")
parser.add_argument('--group', default='all', help="speed group to display, [crawl|slow|medium|fast|veryfast|germany|all], default to all")
args = parser.parse_args()
if args.addr != "127.0.0.1":
os.environ["ZMQ"] = "1"
messaging.context = messaging.Context()
all_groups = {"germany": (45, "45 - up m/s // 162 - up km/h // 101 - up mph"),
"veryfast": (35, "35 - 45 m/s // 126 - 162 km/h // 78 - 101 mph"),
"fast": (25, "25 - 35 m/s // 90 - 126 km/h // 56 - 78 mph"),
"medium": (15, "15 - 25 m/s // 54 - 90 km/h // 34 - 56 mph"),
"slow": (5, " 5 - 15 m/s // 18 - 54 km/h // 11 - 34 mph"),
"crawl": (0, " 0 - 5 m/s // 0 - 18 km/h // 0 - 11 mph")}
if args.group == "all":
display_groups = all_groups.keys()
elif args.group in all_groups.keys():
display_groups = [args.group]
else:
raise ValueError("invalid speed group, see help")
speed_group_stats = {}
for group in all_groups:
speed_group_stats[group] = defaultdict(lambda: {'err': 0, "cnt": 0, "=": 0, "+": 0, "-": 0, "steer": 0, "limited": 0, "saturated": 0, "dpp": 0})
carControl = messaging.sub_sock('carControl', addr=args.addr, conflate=True)
sm = messaging.SubMaster(['carState', 'carControl', 'controlsState', 'lateralPlan'], addr=args.addr)
time.sleep(1) # Make sure all submaster data is available before going further
msg_cnt = 0
cnt = 0
total_error = 0
while messaging.recv_one(carControl):
sm.update()
msg_cnt += 1
if args.control_type == "pid":
control_state = sm['controlsState'].lateralControlState.pidState
elif args.control_type == "indi":
control_state = sm['controlsState'].lateralControlState.indiState
elif args.control_type == "lqr":
control_state = sm['controlsState'].lateralControlState.lqrState
elif args.control_type == "angle":
control_state = sm['controlsState'].lateralControlState.angleState
else:
raise ValueError("invalid lateral control type, see help")
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
steer = sm['carControl'].actuatorsOutput.steer
standstill = sm['carState'].standstill
steer_limited = sm['carState'].steeringRateLimited
overriding = sm['carState'].steeringPressed
changing_lanes = sm['lateralPlan'].laneChangeState != 0
d_path_points = sm['lateralPlan'].dPathPoints
# must be engaged, not at standstill, not overriding steering, and not changing lanes
if active and not standstill and not overriding and not changing_lanes:
cnt += 1
# wait 5 seconds after engage / standstill / override / lane change
if cnt >= 500:
actual_angle = control_state.steeringAngleDeg
desired_angle = control_state.steeringAngleDesiredDeg
# calculate error before rounding, then round for stats grouping
angle_error = abs(desired_angle - actual_angle)
actual_angle = round(actual_angle, 1)
desired_angle = round(desired_angle, 1)
angle_error = round(angle_error, 2)
angle_abs = int(abs(round(desired_angle, 0)))
for group, group_props in all_groups.items():
if v_ego > group_props[0]:
# collect stats
speed_group_stats[group][angle_abs]["cnt"] += 1
speed_group_stats[group][angle_abs]["err"] += angle_error
speed_group_stats[group][angle_abs]["steer"] += abs(steer)
if len(d_path_points):
speed_group_stats[group][angle_abs]["dpp"] += abs(d_path_points[0])
if steer_limited:
speed_group_stats[group][angle_abs]["limited"] += 1
if control_state.saturated:
speed_group_stats[group][angle_abs]["saturated"] += 1
if actual_angle == desired_angle:
speed_group_stats[group][angle_abs]["="] += 1
else:
if desired_angle == 0.:
overshoot = True
else:
overshoot = desired_angle < actual_angle if desired_angle > 0. else desired_angle > actual_angle
speed_group_stats[group][angle_abs]["+" if overshoot else "-"] += 1
break
else:
cnt = 0
if msg_cnt % 100 == 0:
print(chr(27) + "[2J")
if cnt != 0:
print("COLLECTING ...\n")
else:
print("DISABLED (not active, standstill, steering override, or lane change)\n")
for group in display_groups:
if len(speed_group_stats[group]) > 0:
print(f"speed group: {group:10s} {all_groups[group][1]:>96s}")
print(f" {'-'*118}")
for k in sorted(speed_group_stats[group].keys()):
v = speed_group_stats[group][k]
print(f' {k:#2}° | actuator:{int(v["steer"] / v["cnt"] * 100):#3}% | error: {round(v["err"] / v["cnt"], 2):2.2f}° | -:{int(v["-"] / v["cnt"] * 100):#3}% | =:{int(v["="] / v["cnt"] * 100):#3}% | +:{int(v["+"] / v["cnt"] * 100):#3}% | lim:{v["limited"]:#5} | sat:{v["saturated"]:#5} | path dev: {round(v["dpp"] / v["cnt"], 2):2.2f}m | total: {v["cnt"]:#5}')
print("")
Loading…
Cancel
Save