Merge remote-tracking branch 'upstream/master' into toyota-lta

pull/27494/head
sshane 2 years ago
commit 1cbd292d07
  1. 2
      cereal
  2. 6
      poetry.lock
  3. 14
      pyproject.toml
  4. 1
      selfdrive/car/body/carcontroller.py
  5. 1
      selfdrive/car/chrysler/carcontroller.py
  6. 1
      selfdrive/car/gm/carcontroller.py
  7. 1
      selfdrive/car/honda/carcontroller.py
  8. 10
      selfdrive/car/hyundai/carcontroller.py
  9. 20
      selfdrive/car/hyundai/hyundaicanfd.py
  10. 4
      selfdrive/car/hyundai/interface.py
  11. 6
      selfdrive/car/hyundai/values.py
  12. 1
      selfdrive/car/mazda/carcontroller.py
  13. 1
      selfdrive/car/subaru/carcontroller.py
  14. 4
      selfdrive/car/torque_data/params.yaml
  15. 3
      selfdrive/car/toyota/carcontroller.py
  16. 6
      selfdrive/car/toyota/carstate.py
  17. 2
      selfdrive/car/toyota/interface.py
  18. 1
      selfdrive/car/toyota/values.py
  19. 7
      selfdrive/car/volkswagen/carcontroller.py
  20. 6
      selfdrive/car/volkswagen/carstate.py
  21. 4
      selfdrive/car/volkswagen/mqbcan.py
  22. 4
      selfdrive/car/volkswagen/pqcan.py
  23. 8
      selfdrive/controls/controlsd.py
  24. 4
      selfdrive/controls/lib/events.py
  25. 3
      selfdrive/controls/lib/latcontrol.py
  26. 6
      selfdrive/controls/lib/latcontrol_angle.py
  27. 5
      selfdrive/modeld/modeld.cc
  28. 19
      selfdrive/modeld/models/driving.cc
  29. 10
      selfdrive/modeld/models/driving.h
  30. 12
      selfdrive/modeld/models/nav.cc
  31. 23
      selfdrive/modeld/models/nav.h
  32. 4
      selfdrive/modeld/models/navmodel.onnx
  33. 4
      selfdrive/modeld/models/navmodel_q.dlc
  34. 18
      selfdrive/modeld/runners/onnxmodel.cc
  35. 6
      selfdrive/modeld/runners/onnxmodel.h
  36. 2
      selfdrive/modeld/runners/runmodel.h
  37. 10
      selfdrive/modeld/runners/snpemodel.cc
  38. 6
      selfdrive/modeld/runners/snpemodel.h
  39. 8
      selfdrive/modeld/runners/thneedmodel.cc
  40. 4
      selfdrive/modeld/runners/thneedmodel.h
  41. 80
      selfdrive/navd/map_renderer.cc
  42. 3
      selfdrive/navd/map_renderer.h
  43. 2
      selfdrive/test/process_replay/compare_logs.py
  44. 50
      selfdrive/test/process_replay/model_replay.py
  45. 2
      selfdrive/test/process_replay/model_replay_ref_commit
  46. 2
      selfdrive/test/process_replay/ref_commit
  47. 5
      selfdrive/ui/qt/onroad.cc
  48. 34
      tools/cabana/binaryview.cc
  49. 8
      tools/cabana/binaryview.h
  50. 130
      tools/cabana/chartswidget.cc
  51. 13
      tools/cabana/chartswidget.h
  52. 14
      tools/cabana/historylog.cc
  53. 4
      tools/replay/ui.py
  54. 2
      update_requirements.sh

@ -1 +1 @@
Subproject commit 7765176413c0bb14143fe2469d5390ea0ea61a1e
Subproject commit 439429cad4d1e2ab874520cb5d4db8b8d978cbde

6
poetry.lock generated

@ -4414,7 +4414,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"]
[metadata]
lock-version = "1.1"
python-versions = "~3.8"
content-hash = "331a7b700c17c618a4f0fa7d0ec3e5b585c1dc467f6f03261c3778f729058f55"
content-hash = "82e450801a9a1de9fd98615d7deb3d8f0aa5bd3ccf0cf8b258cd8f6e6e4c5309"
[metadata.files]
adal = [
@ -5420,7 +5420,6 @@ gevent = [
{file = "gevent-22.10.1-cp310-cp310-win_amd64.whl", hash = "sha256:d2ea4ce36c09355379bc038be2bd50118f97d2eb6381b7096de4d05aa4c3e241"},
{file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3e73c9f71aa2a6795ecbec9b57282b002375e863e283558feb87b62840c8c1ac"},
{file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_ppc64le.manylinux2014_ppc64le.whl", hash = "sha256:5bc3758f0dc95007c1780d28a9fd2150416a79c50f308f62a674d78a845ea1b9"},
{file = "gevent-22.10.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:03c10ca0beeab0c6be516030471ea630447ddd1f649d3335e5b162097cd4130a"},
{file = "gevent-22.10.1-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:fe2c0ff095171c49f78f1d4e6dc89fa58253783c7b6dccab9f1d76e2ee391f10"},
{file = "gevent-22.10.1-cp36-cp36m-manylinux_2_12_x86_64.manylinux2010_x86_64.whl", hash = "sha256:d18fcc324f39a3b21795022eb47c7752d6e4f4ed89d8cca41f1cc604553265b3"},
{file = "gevent-22.10.1-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:06ea39c70ce166c4a1d4386c7fae96cb8d84ad799527b3378406051104d15443"},
@ -6795,7 +6794,6 @@ pycryptodome = [
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2010_i686.whl", hash = "sha256:7c9ed8aa31c146bef65d89a1b655f5f4eab5e1120f55fc297713c89c9e56ff0b"},
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2010_x86_64.whl", hash = "sha256:5099c9ca345b2f252f0c28e96904643153bae9258647585e5e6f649bb7a1844a"},
{file = "pycryptodome-3.15.0-cp27-cp27m-manylinux2014_aarch64.whl", hash = "sha256:2ec709b0a58b539a4f9d33fb8508264c3678d7edb33a68b8906ba914f71e8c13"},
{file = "pycryptodome-3.15.0-cp27-cp27m-musllinux_1_1_aarch64.whl", hash = "sha256:2ae53125de5b0d2c95194d957db9bb2681da8c24d0fb0fe3b056de2bcaf5d837"},
{file = "pycryptodome-3.15.0-cp27-cp27m-win32.whl", hash = "sha256:fd2184aae6ee2a944aaa49113e6f5787cdc5e4db1eb8edb1aea914bd75f33a0c"},
{file = "pycryptodome-3.15.0-cp27-cp27m-win_amd64.whl", hash = "sha256:7e3a8f6ee405b3bd1c4da371b93c31f7027944b2bcce0697022801db93120d83"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux1_i686.whl", hash = "sha256:b9c5b1a1977491533dfd31e01550ee36ae0249d78aae7f632590db833a5012b8"},
@ -6803,14 +6801,12 @@ pycryptodome = [
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2010_i686.whl", hash = "sha256:2aa55aae81f935a08d5a3c2042eb81741a43e044bd8a81ea7239448ad751f763"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2010_x86_64.whl", hash = "sha256:c3640deff4197fa064295aaac10ab49a0d55ef3d6a54ae1499c40d646655c89f"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-manylinux2014_aarch64.whl", hash = "sha256:045d75527241d17e6ef13636d845a12e54660aa82e823b3b3341bcf5af03fa79"},
{file = "pycryptodome-3.15.0-cp27-cp27mu-musllinux_1_1_aarch64.whl", hash = "sha256:eb6fce570869e70cc8ebe68eaa1c26bed56d40ad0f93431ee61d400525433c54"},
{file = "pycryptodome-3.15.0-cp35-abi3-macosx_10_9_x86_64.whl", hash = "sha256:9ee40e2168f1348ae476676a2e938ca80a2f57b14a249d8fe0d3cdf803e5a676"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux1_i686.whl", hash = "sha256:4c3ccad74eeb7b001f3538643c4225eac398c77d617ebb3e57571a897943c667"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux1_x86_64.whl", hash = "sha256:1b22bcd9ec55e9c74927f6b1f69843cb256fb5a465088ce62837f793d9ffea88"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2010_i686.whl", hash = "sha256:57f565acd2f0cf6fb3e1ba553d0cb1f33405ec1f9c5ded9b9a0a5320f2c0bd3d"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2010_x86_64.whl", hash = "sha256:4b52cb18b0ad46087caeb37a15e08040f3b4c2d444d58371b6f5d786d95534c2"},
{file = "pycryptodome-3.15.0-cp35-abi3-manylinux2014_aarch64.whl", hash = "sha256:092a26e78b73f2530b8bd6b3898e7453ab2f36e42fd85097d705d6aba2ec3e5e"},
{file = "pycryptodome-3.15.0-cp35-abi3-musllinux_1_1_aarch64.whl", hash = "sha256:50ca7e587b8e541eb6c192acf92449d95377d1f88908c0a32ac5ac2703ebe28b"},
{file = "pycryptodome-3.15.0-cp35-abi3-win32.whl", hash = "sha256:e244ab85c422260de91cda6379e8e986405b4f13dc97d2876497178707f87fc1"},
{file = "pycryptodome-3.15.0-cp35-abi3-win_amd64.whl", hash = "sha256:c77126899c4b9c9827ddf50565e93955cb3996813c18900c16b2ea0474e130e9"},
{file = "pycryptodome-3.15.0-pp27-pypy_73-macosx_10_9_x86_64.whl", hash = "sha256:9eaadc058106344a566dc51d3d3a758ab07f8edde013712bc8d22032a86b264f"},

@ -12,7 +12,7 @@ documentation = "https://docs.comma.ai"
[tool.poetry.dependencies]
python = "~3.8"
atomicwrites = "^1.4.0"
casadi = { version = "==3.5.5", markers = "platform_system != 'Darwin'" }
casadi = { version = "==3.5.5", platform = "linux" }
cffi = "^1.15.1"
crcmod = "^1.7"
cryptography = "^37.0.4"
@ -29,7 +29,7 @@ libusb1 = "^3.0.0"
nose = "^1.3.7"
numpy = "^1.23.0"
onnx = "^1.12.0"
onnxruntime-gpu = { version = "^1.11.1", markers = "platform_system != 'Darwin'" }
onnxruntime-gpu = { version = "^1.11.1", platform = "linux" }
pillow = "^9.2.0"
poetry = "==1.2.2"
protobuf = "==3.20.1"
@ -50,20 +50,20 @@ setproctitle = "^1.2.3"
six = "^1.16.0"
smbus2 = "^0.4.2"
sounddevice = "^0.4.5"
spidev = { version = "^3.6", platform = "linux" }
sympy = "^1.10.1"
timezonefinder = "^6.0.1"
tqdm = "^4.64.0"
urllib3 = "^1.26.10"
utm = "^0.7.0"
websocket_client = "^1.3.3"
spidev = "^3.6"
[tool.poetry.group.dev.dependencies]
av = "^9.2.0"
azure-storage-blob = "~2.1"
breathe = "^4.34.0"
carla = "==0.9.13"
carla = { version = "==0.9.13", platform = "linux" }
control = "^0.9.2"
coverage = "^6.4.1"
dictdiffer = "^0.9.0"
@ -81,7 +81,7 @@ mypy = "^0.961"
myst-parser = "^0.18.0"
natsort = "^8.1.0"
numpy = "^1.23.0"
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu113/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl" }
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu113/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl", platform = "linux" }
pandas = "^1.4.3"
parameterized = "^0.8.1"
paramiko = "^2.11.0"
@ -119,7 +119,6 @@ azure-cli-core = "^2.38.0"
azure-common = "^1.1.28"
azure-core = "^1.24.2"
azure-nspkg = "~3.0"
azure-storage-blob = "~2.1"
azure-storage-common = "~2.1"
azure-storage-nspkg = "~3.1"
blosc = "==1.9.2"
@ -143,15 +142,12 @@ jupyter = "^1.0.0"
jupyterlab = "^3.4.4"
jupyterlab-vim = "^0.15.1"
Markdown = "^3.4.1"
mpld3 = "^0.5.8"
msgpack-python = "^0.5.6"
networkx = "~2.3"
nvidia-ml-py3 = "^7.352.0"
onnx2torch = "^1.5.4"
onnxoptimizer = "^0.3.1"
opencv-python-headless = { url = "https://github.com/commaai/opencv-python-builder/releases/download/4.5.5.64%2Bcu113/opencv_python_headless-4.5.5.64-cp38-cp38-manylinux_2_31_x86_64.whl" }
osmium = "^3.3.0"
pandas = "^1.4.3"
pillow-avif-plugin = "^1.2.2"
pipenv = "==2022.10.12"
plotly = "^5.9.0"

@ -85,6 +85,7 @@ class CarController:
new_actuators = CC.actuators.copy()
new_actuators.accel = torque_l
new_actuators.steer = torque_r
new_actuators.steerOutputCan = torque_r
self.frame += 1
return new_actuators, can_sends

@ -78,5 +78,6 @@ class CarController:
new_actuators = CC.actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
return new_actuators, can_sends

@ -158,6 +158,7 @@ class CarController:
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.params.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
new_actuators.gas = self.apply_gas
new_actuators.brake = self.apply_brake

@ -262,6 +262,7 @@ class CarController:
new_actuators.gas = self.gas
new_actuators.brake = self.brake
new_actuators.steer = self.last_steer
new_actuators.steerOutputCan = apply_steer
self.frame += 1
return new_actuators, can_sends

@ -83,11 +83,16 @@ class CarController:
# tester present - w/ no response (keeps relevant ECU disabled)
if self.frame % 100 == 0 and not (self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value) and self.CP.openpilotLongitudinalControl:
# for longitudinal control, either radar or ADAS driving ECU
addr, bus = 0x7d0, 0
if self.CP.flags & HyundaiFlags.CANFD_HDA2.value:
addr, bus = 0x730, 5
can_sends.append([addr, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", bus])
# for blinkers
if self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.append([0x7b1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 5])
# >90 degree steering fault prevention
# Count up to MAX_ANGLE_FRAMES, at which point we need to cut torque to avoid a steering fault
if CC.latActive and abs(CS.out.steeringAngleDeg) >= MAX_ANGLE:
@ -118,6 +123,10 @@ class CarController:
if self.frame % 5 == 0 and (not hda2 or hda2_long):
can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CP, CC.enabled))
# blinkers
if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS:
can_sends.extend(hyundaicanfd.create_spas_messages(self.packer, self.frame, False, False))
if self.CP.openpilotLongitudinalControl:
if hda2:
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.frame))
@ -186,6 +195,7 @@ class CarController:
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = accel
self.frame += 1

@ -1,7 +1,6 @@
from common.numpy_fast import clip
from selfdrive.car.hyundai.values import HyundaiFlags
def get_e_can_bus(CP):
# On the CAN-FD platforms, the LKAS camera is on both A-CAN and E-CAN. HDA2 cars
# have a different harness than the HDA1 and non-HDA variants in order to split
@ -98,6 +97,25 @@ def create_acc_control(packer, CP, enabled, accel_last, accel, stopping, gas_ove
return packer.make_can_msg("SCC_CONTROL", get_e_can_bus(CP), values)
def create_spas_messages(packer, frame, left_blink, right_blink):
ret = []
values = {
}
ret.append(packer.make_can_msg("SPAS1", 5, values))
blink = 0
if left_blink:
blink = 3
elif right_blink:
blink = 4
values = {
"BLINKER_CONTROL": blink,
}
ret.append(packer.make_can_msg("SPAS2", 5, values))
return ret
def create_adrv_messages(packer, frame):
# messages needed to car happy after disabling

@ -287,6 +287,10 @@ class CarInterface(CarInterfaceBase):
addr, bus = 0x730, 5
disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01')
# for blinkers
if CP.flags & HyundaiFlags.ENABLE_BLINKERS:
disable_ecu(logcan, sendcan, bus=5, addr=0x7B1, com_cont_req=b'\x28\x83\x01')
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)

@ -58,6 +58,8 @@ class HyundaiFlags(IntFlag):
ALT_LIMITS = 16
ENABLE_BLINKERS = 32
class CAR:
# Hyundai
@ -845,6 +847,7 @@ FW_VERSIONS = {
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.04 99110-S8100 ',
b'\xf1\x00LX2_ SCC FHCUP 1.00 1.05 99110-S8100 ',
b'\xf1\x00ON__ FCA FHCUP 1.00 1.02 99110-S9100 ',
b'\xf1\x00ON__ FCA FHCUP 1.00 1.01 99110-S9110 ',
],
(Ecu.abs, 0x7d1, None): [
b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360',
@ -858,6 +861,7 @@ FW_VERSIONS = {
b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360',
b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360',
b'\xf1\x00ON ESC \x0b 101\x19\t\x05 58910-S9320',
b'\xf1\x00ON ESC \x01 101\x19\t\x08 58910-S9360',
],
(Ecu.engine, 0x7e0, None): [
b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',
@ -880,11 +884,13 @@ FW_VERSIONS = {
b'\xf1\x00ON MFC AT USA LHD 1.00 1.01 99211-S9100 181105',
b'\xf1\x00ON MFC AT USA LHD 1.00 1.03 99211-S9100 200720',
b'\xf1\x00LX2 MFC AT USA LHD 1.00 1.00 99211-S8110 210226',
b'\xf1\x00ON MFC AT USA LHD 1.00 1.04 99211-S9100 211227',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x00bcsh8p54 U872\x00\x00\x00\x00\x00\x00TON4G38NB1\x96z28',
b'\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX4G38NB3X\xa8\xc08',
b'\xf1\x00bcsh8p54 U903\x00\x00\x00\x00\x00\x00TON4G38NB2[v\\\xb6',
b'\xf1\x00bcsh8p54 U922\x00\x00\x00\x00\x00\x00TON2G38NB5j\x94.\xde',
b'\xf1\x87LBLUFN591307KF25vgvw\x97wwwy\x99\xa7\x99\x99\xaa\xa9\x9af\x88\x96h\x95o\xf7\xff\x99f/\xff\xe4c\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB2\xd7\xc1/\xd1',
b'\xf1\x87LBLUFN650868KF36\xa9\x98\x89\x88\xa8\x88\x88\x88h\x99\xa6\x89fw\x86gw\x88\x97x\xaa\x7f\xf6\xff\xbb\xbb\x8f\xff+\x82\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',
b'\xf1\x87LBLUFN655162KF36\x98\x88\x88\x88\x98\x88\x88\x88x\x99\xa7\x89x\x99\xa7\x89x\x99\x97\x89g\x7f\xf7\xffwU_\xff\xe9!\xf1\x81U891\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54 U891\x00\x00\x00\x00\x00\x00SLX2G38NB3\xd1\xc3\xf8\xa8',

@ -59,6 +59,7 @@ class CarController:
new_actuators = CC.actuators.copy()
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.steerOutputCan = apply_steer
self.frame += 1
return new_actuators, can_sends

@ -87,6 +87,7 @@ class CarController:
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
self.frame += 1
return new_actuators, can_sends

@ -27,7 +27,7 @@ HONDA PILOT 2017: [1.7262026201812795, 0.9470005614967523, 0.21351430733218763]
HONDA RIDGELINE 2017: [1.4146525028237624, 0.7356572861629564, 0.23307177552211328]
HYUNDAI ELANTRA 2021: [3.169, 2.1259108157250735, 0.0819]
HYUNDAI GENESIS 2015-2016: [1.8466226943929824, 1.5552063647830634, 0.0984484465421171]
HYUNDAI IONIQ 5 2022: [3.172929, 3.0, 0.096019]
HYUNDAI IONIQ 5 2022: [3.172929, 2.713050, 0.096019]
HYUNDAI IONIQ ELECTRIC LIMITED 2019: [1.7662975472852054, 1.613755614526594, 0.17087579756306276]
HYUNDAI IONIQ PHEV 2020: [3.2928700076638537, 2.1193482926455656, 0.12463700961468778]
HYUNDAI IONIQ PLUG-IN HYBRID 2019: [2.970807902012267, 1.6312321830002083, 0.1088964990357482]
@ -42,7 +42,7 @@ HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488
HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272]
JEEP GRAND CHEROKEE 2019: [1.7321233388827006, 1.289689569171081, 0.15046331002097185]
JEEP GRAND CHEROKEE V6 2018: [1.8776598027756923, 1.4057367824262523, 0.11725947414922003]
KIA EV6 2022: [3.2, 3.0, 0.05]
KIA EV6 2022: [3.2, 2.093457, 0.05]
KIA K5 2021: [2.405339728085138, 1.460032270828705, 0.11650989850813716]
KIA NIRO EV 2020: [2.9215954981365337, 2.1500583840260044, 0.09236802474810267]
KIA SORENTO GT LINE 2018: [2.464854685101844, 1.5335274218367956, 0.12056170567599558]

@ -192,8 +192,9 @@ class CarController:
can_sends.append(make_can_msg(addr, vl, bus))
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
new_actuators.steeringAngleDeg = apply_angle
new_actuators.steerOutputCan = apply_steer
new_actuators.accel = self.accel
new_actuators.gas = self.gas

@ -6,7 +6,7 @@ from common.realtime import DT_CTRL
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
from selfdrive.car.toyota.values import ToyotaFlags, DBC, STEER_THRESHOLD, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
from selfdrive.car.toyota.values import ToyotaFlags, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
class CarState(CarStateBase):
@ -130,7 +130,9 @@ class CarState(CarStateBase):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
ret.cruiseState.standstill = self.pcm_acc_status == 7
if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
# ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6)

@ -249,6 +249,8 @@ class CarInterface(CarInterfaceBase):
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
if candidate in TSS2_CAR:
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
else:
tune.kpBP = [0., 5., 35.]

@ -1672,6 +1672,7 @@ FW_VERSIONS = {
b'\x018966378B2100\x00\x00\x00\x00',
b'\x018966378B3000\x00\x00\x00\x00',
b'\x018966378G3000\x00\x00\x00\x00',
b'\x018966378B2000\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F152678221\x00\x00\x00\x00\x00\x00',

@ -2,6 +2,7 @@ from cereal import car
from opendbc.can.packer import CANPacker
from common.numpy_fast import clip
from common.conversions import Conversions as CV
from common.realtime import DT_CTRL
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.volkswagen import mqbcan, pqcan
from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
@ -88,10 +89,13 @@ class CarController:
CS.out.steeringPressed, hud_alert, hud_control))
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
lead_distance = 0
if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor
lead_distance = 512 if CS.upscale_lead_car_signal else 8
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
hud_control.leadVisible))
lead_distance))
# **** Stock ACC Button Controls **************************************** #
@ -103,6 +107,7 @@ class CarController:
new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
new_actuators.steerOutputCan = self.apply_steer_last
self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
self.frame += 1

@ -13,6 +13,7 @@ class CarState(CarStateBase):
self.CCP = CarControllerParams(CP)
self.button_states = {button.event_type: False for button in self.CCP.BUTTONS}
self.esp_hold_confirmation = False
self.upscale_lead_car_signal = False
def create_button_events(self, pt_cp, buttons):
button_events = []
@ -141,6 +142,9 @@ class CarState(CarStateBase):
# Additional safety checks performed in CarInterface.
ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
# Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently
self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"])
return ret
def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
@ -281,6 +285,7 @@ class CarState(CarStateBase):
("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled
("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation
("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied
("KBI_Variante", "Kombi_03"), # Digital/full-screen instrument cluster installed
("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator
("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off
("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel
@ -312,6 +317,7 @@ class CarState(CarStateBase):
("Airbag_02", 5), # From J234 Airbag control module
("Kombi_01", 2), # From J285 Instrument cluster
("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
("Kombi_03", 0), # From J285 instrument cluster (not present on older cars, 1Hz when present)
]
if CP.transmissionType == TransmissionType.automatic:

@ -96,13 +96,13 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control,
return commands
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible):
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
values = {
"ACC_Status_Anzeige": acc_hud_status,
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
"ACC_Gesetzte_Zeitluecke": 3,
"ACC_Display_Prio": 3,
# TODO: ACC_Abstandsindex for lead car distance, must determine analog vs digital cluster for scaling
"ACC_Abstandsindex": lead_distance,
}
return packer.make_can_msg("ACC_02", bus, values)

@ -77,12 +77,12 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control,
return commands
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible):
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance):
values = {
"ACA_StaACC": acc_hud_status,
"ACA_Zeitluecke": 2,
"ACA_V_Wunsch": set_speed,
"ACA_gemZeitl": 8 if lead_visible else 0,
"ACA_gemZeitl": lead_distance,
# TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke
# display/display-prio handling probably needed to stop confusing the instrument cluster
# kmh_mph handling probably needed to resolve rounding errors in displayed setpoint

@ -21,7 +21,7 @@ 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
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
@ -723,7 +723,11 @@ class Controls:
self.last_actuators, can_sends = self.CI.apply(CC)
self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
CC.actuatorsOutput = self.last_actuators
self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \
STEER_ANGLE_SATURATION_THRESHOLD
else:
self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)

@ -500,9 +500,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = {
EventName.resumeRequired: {
ET.WARNING: Alert(
"STOPPED",
"Press Resume to Exit Standstill",
AlertStatus.userPrompt, AlertSize.mid,
"",
AlertStatus.userPrompt, AlertSize.small,
Priority.LOW, VisualAlert.none, AudibleAlert.none, .2),
},

@ -12,6 +12,7 @@ class LatControl(ABC):
self.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = self.CP.steerLimitTimer
self.sat_count = 0.
self.sat_check_min_speed = 10.
# we define the steer torque scale as [-1.0...1.0]
self.steer_max = 1.0
@ -24,7 +25,7 @@ class LatControl(ABC):
self.sat_count = 0.
def _check_saturation(self, saturated, CS, steer_limited):
if saturated and CS.vEgo > 10. and not steer_limited and not CS.steeringPressed:
if saturated and CS.vEgo > self.sat_check_min_speed and not steer_limited and not CS.steeringPressed:
self.sat_count += self.sat_count_rate
else:
self.sat_count -= self.sat_count_rate

@ -7,6 +7,10 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.sat_check_min_speed = 5.
def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
angle_log = log.ControlsState.LateralAngleState.new_message()
@ -19,7 +23,7 @@ class LatControlAngle(LatControl):
angle_steers_des += params.angleOffsetDeg
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited)
angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False)
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log

@ -15,6 +15,7 @@
#include "common/util.h"
#include "system/hardware/hw.h"
#include "selfdrive/modeld/models/driving.h"
#include "selfdrive/modeld/models/nav.h"
ExitHandler do_exit;
@ -72,6 +73,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
mat3 model_transform_main = {};
mat3 model_transform_extra = {};
bool live_calib_seen = false;
float driving_style[DRIVING_STYLE_LEN] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0};
float nav_features[NAV_FEATURE_LEN] = {0};
VisionBuf *buf_main = nullptr;
VisionBuf *buf_extra = nullptr;
@ -151,7 +154,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
}
double mt1 = millis_since_boot();
ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, prepare_only);
ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, is_rhd, driving_style, nav_features, prepare_only);
double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;

@ -46,10 +46,19 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
#ifdef TRAFFIC_CONVENTION
s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
#endif
#ifdef DRIVING_STYLE
s->m->addDrivingStyle(s->driving_style, DRIVING_STYLE_LEN);
#endif
#ifdef NAV
s->m->addNavFeatures(s->nav_features, NAV_FEATURE_LEN);
#endif
}
ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only) {
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only) {
#ifdef DESIRE
std::memmove(&s->pulse_desire[0], &s->pulse_desire[DESIRE_LEN], sizeof(float) * DESIRE_LEN*HISTORY_BUFFER_LEN);
if (desire_in != NULL) {
@ -67,6 +76,14 @@ ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
LOGT("Desire enqueued");
#endif
#ifdef NAV
std::memcpy(s->nav_features, nav_features, sizeof(float)*NAV_FEATURE_LEN);
#endif
#ifdef DRIVING_STYLE
std::memcpy(s->driving_style, driving_style, sizeof(float)*DRIVING_STYLE_LEN);
#endif
int rhd_idx = is_rhd;
s->traffic_convention[rhd_idx] = 1.0;
s->traffic_convention[1-rhd_idx] = 0.0;

@ -14,6 +14,7 @@
#include "common/modeldata.h"
#include "common/util.h"
#include "selfdrive/modeld/models/commonmodel.h"
#include "selfdrive/modeld/models/nav.h"
#include "selfdrive/modeld/runners/run.h"
constexpr int FEATURE_LEN = 128;
@ -21,6 +22,7 @@ constexpr int HISTORY_BUFFER_LEN = 99;
constexpr int DESIRE_LEN = 8;
constexpr int DESIRE_PRED_LEN = 4;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int DRIVING_STYLE_LEN = 12;
constexpr int MODEL_FREQ = 20;
constexpr int DISENGAGE_LEN = 5;
@ -259,11 +261,17 @@ struct ModelState {
#ifdef TRAFFIC_CONVENTION
float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
#endif
#ifdef DRIVING_STYLE
float driving_style[DRIVING_STYLE_LEN] = {};
#endif
#ifdef NAV
float nav_features[NAV_FEATURE_LEN] = {};
#endif
};
void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, bool prepare_only);
const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only);
void model_free(ModelState* s);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,

@ -9,11 +9,11 @@
void navmodel_init(NavModelState* s) {
#ifdef USE_ONNX_MODEL
s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#else
s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#endif
#ifdef USE_ONNX_MODEL
s->m = new ONNXModel("models/navmodel.onnx", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#else
s->m = new SNPEModel("models/navmodel_q.dlc", &s->output[0], NAV_NET_OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
#endif
}
NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf) {
@ -56,7 +56,7 @@ void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &mo
framed.setDspExecutionTime(model_res.dsp_execution_time);
framed.setFeatures(to_kj_array_ptr(model_res.features.values));
framed.setDesirePrediction(to_kj_array_ptr(model_res.desire_pred.values));
fill_plan(framed, model_res.plans.get_best_prediction());
fill_plan(framed, model_res.plan);
pm.send("navModel", msg);
}

@ -10,7 +10,6 @@
constexpr int NAV_INPUT_SIZE = 256*256;
constexpr int NAV_FEATURE_LEN = 64;
constexpr int NAV_DESIRE_LEN = 32;
constexpr int NAV_PLAN_MHP_N = 5;
struct NavModelOutputXY {
float x;
@ -21,24 +20,8 @@ static_assert(sizeof(NavModelOutputXY) == sizeof(float)*2);
struct NavModelOutputPlan {
std::array<NavModelOutputXY, TRAJECTORY_SIZE> mean;
std::array<NavModelOutputXY, TRAJECTORY_SIZE> std;
float prob;
};
static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2 + sizeof(float));
struct NavModelOutputPlans {
std::array<NavModelOutputPlan, NAV_PLAN_MHP_N> predictions;
constexpr const NavModelOutputPlan &get_best_prediction() const {
int max_idx = 0;
for (int i = 1; i < predictions.size(); i++) {
if (predictions[i].prob > predictions[max_idx].prob) {
max_idx = i;
}
}
return predictions[max_idx];
}
};
static_assert(sizeof(NavModelOutputPlans) == sizeof(NavModelOutputPlan)*NAV_PLAN_MHP_N);
static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2);
struct NavModelOutputDesirePrediction {
std::array<float, NAV_DESIRE_LEN> values;
@ -51,12 +34,12 @@ struct NavModelOutputFeatures {
static_assert(sizeof(NavModelOutputFeatures) == sizeof(float)*NAV_FEATURE_LEN);
struct NavModelResult {
const NavModelOutputPlans plans;
const NavModelOutputPlan plan;
const NavModelOutputDesirePrediction desire_pred;
const NavModelOutputFeatures features;
float dsp_execution_time;
};
static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlans) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
static_assert(sizeof(NavModelResult) == sizeof(NavModelOutputPlan) + sizeof(NavModelOutputDesirePrediction) + sizeof(NavModelOutputFeatures) + sizeof(float));
constexpr int NAV_OUTPUT_SIZE = sizeof(NavModelResult) / sizeof(float);
constexpr int NAV_NET_OUTPUT_SIZE = NAV_OUTPUT_SIZE - 1;

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:eab4b986e14d7d842d6d5487011c329d356fb56995b2ae7dc7188aefe6df9d97
size 12285002
oid sha256:9028b36a591763724e95205b48f37f09260b4434fb1f3c6f228db1710a401aa8
size 12258591

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:83d53efc40053b02fe7d3da4ef6213a4a5a1ae4d1bd49c121b9beb6a54ea1148
size 3154868
oid sha256:bc7ade56bb4f9525c84a46df22252ea1e321a0518cbcef8bdfc76ccd8ad10b41
size 3154304

@ -97,6 +97,16 @@ void ONNXModel::addDesire(float *state, int state_size) {
desire_state_size = state_size;
}
void ONNXModel::addNavFeatures(float *state, int state_size) {
nav_features_input_buf = state;
nav_features_size = state_size;
}
void ONNXModel::addDrivingStyle(float *state, int state_size) {
driving_style_input_buf = state;
driving_style_size = state_size;
}
void ONNXModel::addTrafficConvention(float *state, int state_size) {
traffic_convention_input_buf = state;
traffic_convention_size = state_size;
@ -128,7 +138,13 @@ void ONNXModel::execute() {
if (desire_input_buf != NULL) {
pwrite(desire_input_buf, desire_state_size);
}
if (traffic_convention_input_buf != NULL) {
if (nav_features_input_buf != NULL) {
pwrite(nav_features_input_buf, nav_features_size);
}
if (driving_style_input_buf != NULL) {
pwrite(driving_style_input_buf, driving_style_size);
}
if (traffic_convention_input_buf != NULL) {
pwrite(traffic_convention_input_buf, traffic_convention_size);
}
if (calib_input_buf != NULL) {

@ -10,6 +10,8 @@ public:
~ONNXModel();
void addRecurrent(float *state, int state_size);
void addDesire(float *state, int state_size);
void addNavFeatures(float *state, int state_size);
void addDrivingStyle(float *state, int state_size);
void addTrafficConvention(float *state, int state_size);
void addCalib(float *state, int state_size);
void addImage(float *image_buf, int buf_size);
@ -25,6 +27,10 @@ private:
int rnn_state_size;
float *desire_input_buf = NULL;
int desire_state_size;
float *nav_features_input_buf = NULL;
int nav_features_size;
float *driving_style_input_buf = NULL;
int driving_style_size;
float *traffic_convention_input_buf = NULL;
int traffic_convention_size;
float *calib_input_buf = NULL;

@ -5,6 +5,8 @@ public:
virtual ~RunModel() {}
virtual void addRecurrent(float *state, int state_size) {}
virtual void addDesire(float *state, int state_size) {}
virtual void addNavFeatures(float *state, int state_size) {}
virtual void addDrivingStyle(float *state, int state_size) {}
virtual void addTrafficConvention(float *state, int state_size) {}
virtual void addCalib(float *state, int state_size) {}
virtual void addImage(float *image_buf, int buf_size) {}

@ -153,6 +153,16 @@ void SNPEModel::addDesire(float *state, int state_size) {
desireBuffer = this->addExtra(state, state_size, 1);
}
void SNPEModel::addNavFeatures(float *state, int state_size) {
navFeatures = state;
navFeaturesBuffer = this->addExtra(state, state_size, 1);
}
void SNPEModel::addDrivingStyle(float *state, int state_size) {
drivingStyle = state;
drivingStyleBuffer = this->addExtra(state, state_size, 2);
}
void SNPEModel::addCalib(float *state, int state_size) {
calib = state;
calibBuffer = this->addExtra(state, state_size, 1);

@ -28,6 +28,8 @@ public:
void addTrafficConvention(float *state, int state_size);
void addCalib(float *state, int state_size);
void addDesire(float *state, int state_size);
void addDrivingStyle(float *state, int state_size);
void addNavFeatures(float *state, int state_size);
void addImage(float *image_buf, int buf_size);
void addExtra(float *image_buf, int buf_size);
void execute();
@ -75,6 +77,10 @@ private:
std::unique_ptr<zdl::DlSystem::IUserBuffer> trafficConventionBuffer;
float *desire;
std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
float *navFeatures;
std::unique_ptr<zdl::DlSystem::IUserBuffer> navFeaturesBuffer;
float *drivingStyle;
std::unique_ptr<zdl::DlSystem::IUserBuffer> drivingStyleBuffer;
float *calib;
std::unique_ptr<zdl::DlSystem::IUserBuffer> calibBuffer;
};

@ -24,6 +24,14 @@ void ThneedModel::addDesire(float *state, int state_size) {
desire = state;
}
void ThneedModel::addDrivingStyle(float *state, int state_size) {
drivingStyle = state;
}
void ThneedModel::addNavFeatures(float *state, int state_size) {
navFeatures = state;
}
void ThneedModel::addImage(float *image_input_buf, int buf_size) {
input = image_input_buf;
}

@ -9,6 +9,8 @@ public:
void addRecurrent(float *state, int state_size);
void addTrafficConvention(float *state, int state_size);
void addDesire(float *state, int state_size);
void addNavFeatures(float *state, int state_size);
void addDrivingStyle(float *state, int state_size);
void addImage(float *image_buf, int buf_size);
void addExtra(float *image_buf, int buf_size);
void execute();
@ -26,6 +28,8 @@ private:
// recurrent and desire
float *recurrent;
float *trafficConvention;
float *drivingStyle;
float *desire;
float *navFeatures;
};

@ -17,6 +17,9 @@ const int NUM_VIPC_BUFFERS = 4;
const int EARTH_CIRCUMFERENCE_METERS = 40075000;
const int PIXELS_PER_TILE = 256;
const bool TEST_MODE = getenv("MAP_RENDER_TEST_MODE");
const int LLK_DECIMATION = TEST_MODE ? 1 : 10;
float get_zoom_level_for_scale(float lat, float meters_per_pixel) {
float meters_per_tile = meters_per_pixel * PIXELS_PER_TILE;
float num_tiles = cos(DEG2RAD(lat)) * EARTH_CIRCUMFERENCE_METERS / meters_per_tile;
@ -56,16 +59,21 @@ MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool online) : m_set
m_map->setFramebufferObject(fbo->handle(), fbo->size());
gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
QObject::connect(m_map.data(), &QMapboxGL::mapChanged, [=](QMapboxGL::MapChange change) {
// https://github.com/mapbox/mapbox-gl-native/blob/cf734a2fec960025350d8de0d01ad38aeae155a0/platform/qt/include/qmapboxgl.hpp#L116
//LOGD("new state %d", change);
});
QObject::connect(m_map.data(), &QMapboxGL::mapLoadingFailed, [=](QMapboxGL::MapLoadingFailure err_code, const QString &reason) {
LOGE("Map loading failed with %d: '%s'\n", err_code, reason.toStdString().c_str());
});
if (online) {
vipc_server.reset(new VisionIpcServer("navd"));
vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH, HEIGHT);
vipc_server->create_buffers(VisionStreamType::VISION_STREAM_MAP, NUM_VIPC_BUFFERS, false, WIDTH/2, HEIGHT/2);
vipc_server->start_listener();
pm.reset(new PubMaster({"navThumbnail"}));
pm.reset(new PubMaster({"navThumbnail", "mapRenderState"}));
sm.reset(new SubMaster({"liveLocationKalman", "navRoute"}, {"liveLocationKalman"}));
timer = new QTimer(this);
@ -84,8 +92,21 @@ void MapRenderer::msgUpdate() {
auto orientation = location.getCalibratedOrientationNED();
bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid();
if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % 10) == 0) {
if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
updatePosition(QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]), RAD2DEG(orientation.getValue()[2]));
// TODO: use the static rendering mode
if (!loaded() && frame_id > 0) {
for (int i = 0; i < 5 && !loaded(); i++) {
LOGW("map render retry #%d, %d", i+1, m_map.isNull());
QApplication::processEvents(QEventLoop::AllEvents, 100);
update();
}
if (!loaded()) {
LOGE("failed to render map after retry");
}
}
}
}
@ -122,24 +143,33 @@ bool MapRenderer::loaded() {
}
void MapRenderer::update() {
double start_t = millis_since_boot();
gl_functions->glClear(GL_COLOR_BUFFER_BIT);
m_map->render();
gl_functions->glFlush();
double end_t = millis_since_boot();
sendVipc();
if ((vipc_server != nullptr) && loaded()) {
publish((end_t - start_t) / 1000.0);
}
}
void MapRenderer::sendVipc() {
if (!vipc_server || !loaded()) {
return;
}
void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf) {
MessageBuilder msg;
auto thumbnaild = msg.initEvent().initNavThumbnail();
thumbnaild.setFrameId(frame_id);
thumbnaild.setTimestampEof(ts);
thumbnaild.setThumbnail(buf);
pm->send("navThumbnail", msg);
}
void MapRenderer::publish(const double render_time) {
QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
uint64_t ts = nanos_since_boot();
VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
VisionIpcBufExtra extra = {
.frame_id = frame_id,
.timestamp_sof = ts,
.timestamp_sof = sm->rcv_time("liveLocationKalman"),
.timestamp_eof = ts,
};
@ -147,15 +177,22 @@ void MapRenderer::sendVipc() {
uint8_t* dst = (uint8_t*)buf->addr;
uint8_t* src = cap.bits();
// RGB to greyscale
// RGB to greyscale and crop
memset(dst, 128, buf->len);
for (int i = 0; i < WIDTH * HEIGHT; i++) {
dst[i] = src[i * 3];
for (int r = 0; r < HEIGHT/2; r++) {
for (int c = 0; c < WIDTH/2; c++) {
dst[r*WIDTH/2 + c] = src[((HEIGHT/4 + r)*WIDTH + (c+WIDTH/4)) * 3];
}
}
vipc_server->send(buf, &extra);
if (frame_id % 100 == 0) {
// Send thumbnail
if (TEST_MODE) {
// Full image in thumbnails in test mode
kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)cap.bits(), cap.sizeInBytes());
sendThumbnail(ts, buffer_kj);
} else if (frame_id % 100 == 0) {
// Write jpeg into buffer
QByteArray buffer_bytes;
QBuffer buffer(&buffer_bytes);
@ -163,16 +200,17 @@ void MapRenderer::sendVipc() {
cap.save(&buffer, "JPG", 50);
kj::Array<capnp::byte> buffer_kj = kj::heapArray<capnp::byte>((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size());
// Send thumbnail
MessageBuilder msg;
auto thumbnaild = msg.initEvent().initNavThumbnail();
thumbnaild.setFrameId(frame_id);
thumbnaild.setTimestampEof(ts);
thumbnaild.setThumbnail(buffer_kj);
pm->send("navThumbnail", msg);
sendThumbnail(ts, buffer_kj);
}
// Send state msg
MessageBuilder msg;
auto state = msg.initEvent().initMapRenderState();
state.setLocationMonoTime(sm->rcv_time("liveLocationKalman"));
state.setRenderTime(render_time);
state.setFrameId(frame_id);
pm->send("mapRenderState", msg);
frame_id++;
}

@ -34,7 +34,8 @@ private:
std::unique_ptr<VisionIpcServer> vipc_server;
std::unique_ptr<PubMaster> pm;
std::unique_ptr<SubMaster> sm;
void sendVipc();
void publish(const double render_time);
void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
QMapboxGLSettings m_settings;
QScopedPointer<QMapboxGL> m_map;

@ -80,7 +80,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non
diff = []
for msg1, msg2 in zip(log1, log2):
if msg1.which() != msg2.which():
print(msg1, msg2)
print(msg1.which(), msg2.which())
raise Exception("msgs not aligned between logs")
msg1 = remove_ignored_fields(msg1, ignore_fields)

@ -23,7 +23,7 @@ from tools.lib.logreader import LogReader
TEST_ROUTE = "2f4452b03ccb98f0|2022-12-03--13-45-30"
SEGMENT = 6
MAX_FRAMES = 100 if PC else 600
NAV_FRAMES = 20
NAV_FRAMES = 50
NO_NAV = "NO_NAV" in os.environ # TODO: make map renderer work in CI
SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0"))
@ -43,39 +43,46 @@ def replace_calib(msg, calib):
def nav_model_replay(lr):
sm = messaging.SubMaster(['navModel', 'navThumbnail'])
pm = messaging.PubMaster(['liveLocationKalman', 'navRoute'])
sock = messaging.sub_sock('navModel', conflate=False, timeout=1000)
nav = [m for m in lr if m.which() == 'navRoute']
llk = [m for m in lr if m.which() == 'liveLocationKalman']
assert len(nav) > 0 and len(llk) >= NAV_FRAMES
log_msgs = []
try:
assert "MAPBOX_TOKEN" in os.environ
os.environ['MAP_RENDER_TEST_MODE'] = '1'
managed_processes['mapsd'].start()
managed_processes['navmodeld'].start()
# setup position and route
nav = [m for m in lr if m.which() == 'navRoute']
llk = [m for m in lr if m.which() == 'liveLocationKalman']
assert len(nav) > 0 and len(llk) > 0
for _ in range(30):
for _ in range(10):
for s in (llk, nav):
msg = s[0]
pm.send(msg.which(), msg.as_builder().to_bytes())
if messaging.recv_one(sock) is not None:
pm.send(s[0].which(), s[0].as_builder().to_bytes())
sm.update(1000)
if sm.updated['navModel']:
break
else:
raise Exception("no navmodeld outputs")
time.sleep(1)
if not sm.updated['navModel']:
raise Exception("no navmodeld outputs, failed to initialize")
# drain
time.sleep(2)
messaging.drain_sock_raw(sock)
sm.update(0)
# run replay
for _ in range(NAV_FRAMES):
# 2Hz decimation
for _ in range(10):
pm.send(llk[0].which(), llk[0].as_builder().to_bytes())
time.sleep(0.1)
with Timeout(5, "timed out waiting for nav model outputs"):
log_msgs.append(messaging.recv_one_retry(sock))
for n in range(NAV_FRAMES):
pm.send(llk[n].which(), llk[n].as_builder().to_bytes())
m = messaging.recv_one(sm.sock['navThumbnail'])
assert m is not None, f"no navThumbnail, frame={n}"
log_msgs.append(m)
m = messaging.recv_one(sm.sock['navModel'])
assert m is not None, f"no navModel response, frame={n}"
log_msgs.append(m)
finally:
managed_processes['mapsd'].stop()
managed_processes['navmodeld'].stop()
@ -212,7 +219,7 @@ if __name__ == "__main__":
try:
expected_msgs = 2*MAX_FRAMES
if not NO_NAV:
expected_msgs += NAV_FRAMES
expected_msgs += NAV_FRAMES*2
cmp_log = list(LogReader(BASE_URL + log_fn))[:expected_msgs]
ignore = [
@ -223,6 +230,7 @@ if __name__ == "__main__":
'driverStateV2.dspExecutionTime',
'navModel.dspExecutionTime',
'navModel.modelExecutionTime',
'navThumbnail.timestampEof',
]
if PC:
ignore += [

@ -1 +1 @@
4b2c6516cd460ee443b9006f01233168edf3d170
4ff972367fdb9546be68ee0ba0d45cf4f839dae7

@ -1 +1 @@
f17412941a0e8229eea308c33189a5bdb1a17ae8
6681ca22053b019a65930e76a396535d0cddf39c

@ -31,6 +31,11 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
split->insertWidget(0, arCam);
}
if (getenv("MAP_RENDER_VIEW")) {
CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this);
split->insertWidget(0, map_render);
}
stacked_layout->addWidget(split_wrapper);
alerts = new OnroadAlerts(this);

@ -10,7 +10,7 @@
// BinaryView
const int CELL_HEIGHT = 26;
const int CELL_HEIGHT = 36;
inline int get_bit_index(const QModelIndex &index, bool little_endian) {
return index.row() * 8 + (little_endian ? 7 - index.column() : index.column());
@ -24,14 +24,13 @@ BinaryView::BinaryView(QWidget *parent) : QTableView(parent) {
horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
verticalHeader()->setSectionsClickable(false);
verticalHeader()->setSectionResizeMode(QHeaderView::Fixed);
verticalHeader()->setDefaultSectionSize(CELL_HEIGHT);
horizontalHeader()->hide();
setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents);
setFrameShape(QFrame::NoFrame);
setShowGrid(false);
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
setMouseTracking(true);
setSizeAdjustPolicy(QAbstractScrollArea::AdjustToContents);
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Fixed);
}
void BinaryView::highlight(const Signal *sig) {
@ -57,7 +56,7 @@ void BinaryView::setSelection(const QRect &rect, QItemSelectionModel::SelectionF
}
void BinaryView::mousePressEvent(QMouseEvent *event) {
delegate->setSelectionColor(style()->standardPalette().color(QPalette::Active, QPalette::Highlight));
delegate->selection_color = (palette().color(QPalette::Active, QPalette::Highlight));
if (auto index = indexAt(event->pos()); index.isValid() && index.column() != 8) {
anchor_index = index;
auto item = (const BinaryViewModel::Item *)anchor_index.internalPointer();
@ -66,7 +65,7 @@ void BinaryView::mousePressEvent(QMouseEvent *event) {
if (bit_idx == s->lsb || bit_idx == s->msb) {
anchor_index = model->bitIndex(bit_idx == s->lsb ? s->msb : s->lsb, true);
resize_sig = s;
delegate->setSelectionColor(item->bg_color);
delegate->selection_color = item->bg_color;
break;
}
}
@ -79,8 +78,7 @@ void BinaryView::highlightPosition(const QPoint &pos) {
auto item = (BinaryViewModel::Item *)index.internalPointer();
const Signal *sig = item->sigs.isEmpty() ? nullptr : item->sigs.back();
highlight(sig);
sig ? QToolTip::showText(pos, sig->name.c_str(), this, rect())
: QToolTip::hideText();
QToolTip::showText(pos, sig ? sig->name.c_str() : "", this, rect());
}
}
@ -214,7 +212,7 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i
if (orientation == Qt::Vertical) {
switch (role) {
case Qt::DisplayRole: return section;
case Qt::SizeHintRole: return QSize(30, CELL_HEIGHT);
case Qt::SizeHintRole: return QSize(30, 0);
case Qt::TextAlignmentRole: return Qt::AlignCenter;
}
}
@ -224,16 +222,9 @@ QVariant BinaryViewModel::headerData(int section, Qt::Orientation orientation, i
// BinaryItemDelegate
BinaryItemDelegate::BinaryItemDelegate(QObject *parent) : QStyledItemDelegate(parent) {
// cache fonts and color
small_font.setPointSize(6);
small_font.setPixelSize(8);
hex_font = QFontDatabase::systemFont(QFontDatabase::FixedFont);
hex_font.setBold(true);
selection_color = QApplication::style()->standardPalette().color(QPalette::Active, QPalette::Highlight);
}
QSize BinaryItemDelegate::sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const {
QSize sz = QStyledItemDelegate::sizeHint(option, index);
return {sz.width(), CELL_HEIGHT};
}
void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const {
@ -245,12 +236,10 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op
painter->setFont(hex_font);
} else if (option.state & QStyle::State_Selected) {
painter->fillRect(option.rect, selection_color);
painter->setPen(QApplication::style()->standardPalette().color(QPalette::BrightText));
painter->setPen(option.palette.color(QPalette::BrightText));
} else if (!item->sigs.isEmpty() && (!bin_view->selectionModel()->hasSelection() || !item->sigs.contains(bin_view->resize_sig))) {
painter->fillRect(option.rect, item->bg_color);
painter->setPen(item->sigs.contains(bin_view->hovered_sig)
? QApplication::style()->standardPalette().color(QPalette::BrightText)
: Qt::black);
painter->setPen(item->sigs.contains(bin_view->hovered_sig) ? option.palette.color(QPalette::BrightText) : Qt::black);
}
painter->drawText(option.rect, Qt::AlignCenter, item->val);
@ -258,6 +247,5 @@ void BinaryItemDelegate::paint(QPainter *painter, const QStyleOptionViewItem &op
painter->setFont(small_font);
painter->drawText(option.rect, Qt::AlignHCenter | Qt::AlignBottom, item->is_msb ? "MSB" : "LSB");
}
painter->restore();
}

@ -9,22 +9,16 @@
#include "tools/cabana/dbcmanager.h"
class BinaryItemDelegate : public QStyledItemDelegate {
Q_OBJECT
public:
BinaryItemDelegate(QObject *parent);
void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override;
QSize sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const override;
void setSelectionColor(const QColor &color) { selection_color = color; }
private:
QFont small_font, hex_font;
QColor selection_color;
};
class BinaryViewModel : public QAbstractTableModel {
Q_OBJECT
public:
BinaryViewModel(QObject *parent) : QAbstractTableModel(parent) {}
void setMessage(const QString &message_id);
@ -52,7 +46,7 @@ public:
private:
QString msg_id;
const DBCMsg *dbc_msg;
const DBCMsg *dbc_msg = nullptr;
int row_count = 0;
const int column_count = 9;
};

@ -3,9 +3,9 @@
#include <QFutureSynchronizer>
#include <QGraphicsLayout>
#include <QRubberBand>
#include <QTimer>
#include <QToolBar>
#include <QToolButton>
#include <QToolTip>
#include <QtConcurrent>
// ChartsWidget
@ -19,6 +19,7 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) {
title_label = new QLabel();
title_label->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred);
toolbar->addWidget(title_label);
show_all_values_btn = toolbar->addAction("");
toolbar->addWidget(range_label = new QLabel());
reset_zoom_btn = toolbar->addAction("");
reset_zoom_btn->setToolTip(tr("Reset zoom (drag on chart to zoom X-Axis)"));
@ -26,7 +27,6 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) {
remove_all_btn->setToolTip(tr("Remove all charts"));
dock_btn = toolbar->addAction("");
main_layout->addWidget(toolbar);
updateToolBar();
// charts
QWidget *charts_container = new QWidget(this);
@ -37,14 +37,16 @@ ChartsWidget::ChartsWidget(QWidget *parent) : QWidget(parent) {
charts_scroll->setWidgetResizable(true);
charts_scroll->setWidget(charts_container);
charts_scroll->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
main_layout->addWidget(charts_scroll);
max_chart_range = settings.max_chart_x_range;
use_dark_theme = palette().color(QPalette::WindowText).value() > palette().color(QPalette::Background).value();
updateToolBar();
QObject::connect(dbc(), &DBCManager::DBCFileChanged, this, &ChartsWidget::removeAll);
QObject::connect(can, &CANMessages::eventsMerged, this, &ChartsWidget::eventsMerged);
QObject::connect(can, &CANMessages::updated, this, &ChartsWidget::updateState);
QObject::connect(show_all_values_btn, &QAction::triggered, this, &ChartsWidget::showAllData);
QObject::connect(remove_all_btn, &QAction::triggered, this, &ChartsWidget::removeAll);
QObject::connect(reset_zoom_btn, &QAction::triggered, this, &ChartsWidget::zoomReset);
QObject::connect(dock_btn, &QAction::triggered, [this]() {
@ -58,7 +60,7 @@ void ChartsWidget::eventsMerged() {
if (auto events = can->events(); events && !events->empty()) {
event_range.first = (events->front()->mono_time / (double)1e9) - can->routeStartTime();
event_range.second = (events->back()->mono_time / (double)1e9) - can->routeStartTime();
updateDisplayRange();
updateState();
}
}
@ -69,8 +71,8 @@ void ChartsWidget::updateDisplayRange() {
// reached the end, or seeked to a timestamp out of range.
display_range.first = current_sec - 5;
}
display_range.first = std::max(display_range.first, event_range.first);
display_range.second = std::min(display_range.first + settings.max_chart_x_range, event_range.second);
display_range.first = std::floor(std::max(display_range.first, event_range.first) * 10.0) / 10.0;
display_range.second = std::floor(std::min(display_range.first + max_chart_range, event_range.second) * 10.0) / 10.0;
if (prev_range != display_range) {
QFutureSynchronizer<void> future_synchronizer;
for (auto c : charts)
@ -100,13 +102,29 @@ void ChartsWidget::updateState() {
}
const auto &range = is_zoomed ? zoomed_range : display_range;
setUpdatesEnabled(false);
for (auto c : charts) {
c->setDisplayRange(range.first, range.second);
c->scene()->invalidate({}, QGraphicsScene::ForegroundLayer);
}
setUpdatesEnabled(true);
}
void ChartsWidget::showAllData() {
bool switch_to_show_all = max_chart_range == settings.max_chart_x_range;
max_chart_range = switch_to_show_all ? settings.cached_segment_limit * 60
: settings.max_chart_x_range;
max_chart_range = std::min(max_chart_range, (uint32_t)can->totalSeconds());
updateToolBar();
updateState();
}
void ChartsWidget::updateToolBar() {
int min_range = std::min(settings.max_chart_x_range, (int)can->totalSeconds());
bool displaying_all = max_chart_range != min_range;
show_all_values_btn->setText(tr("%1 minutes").arg(max_chart_range / 60));
show_all_values_btn->setToolTip(tr("Click to display %1 data").arg(displaying_all ? tr("%1 minutes").arg(min_range / 60) : tr("ALL cached")));
show_all_values_btn->setVisible(!is_zoomed);
remove_all_btn->setEnabled(!charts.isEmpty());
reset_zoom_btn->setEnabled(is_zoomed);
range_label->setText(is_zoomed ? tr("%1 - %2").arg(zoomed_range.first, 0, 'f', 2).arg(zoomed_range.second, 0, 'f', 2) : "");
@ -179,15 +197,6 @@ ChartView::ChartView(QWidget *parent) : QChartView(nullptr, parent) {
chart->layout()->setContentsMargins(0, 0, 0, 0);
chart->setMargins(QMargins(20, 11, 11, 11));
track_line = new QGraphicsLineItem(chart);
track_line->setPen(QPen(Qt::darkGray, 1, Qt::DashLine));
track_ellipse = new QGraphicsEllipseItem(chart);
track_ellipse->setBrush(Qt::darkGray);
value_text = new QGraphicsTextItem(chart);
item_group = scene()->createItemGroup({track_line, track_ellipse, value_text});
item_group->setZValue(chart->zValue() + 10);
// title
QToolButton *remove_btn = new QToolButton();
remove_btn->setText("X");
remove_btn->setAutoRaise(true);
@ -225,6 +234,7 @@ ChartView::~ChartView() {
void ChartView::addSeries(const QString &msg_id, const Signal *sig) {
QLineSeries *series = new QLineSeries(this);
series->setUseOpenGL(true);
chart()->addSeries(series);
series->attachAxis(axis_x);
series->attachAxis(axis_y);
@ -232,6 +242,7 @@ void ChartView::addSeries(const QString &msg_id, const Signal *sig) {
sigs.push_back({.msg_id = msg_id, .address = address, .source = source, .sig = sig, .series = series});
updateTitle();
updateSeries(sig);
updateAxisY();
}
void ChartView::removeSeries(const QString &msg_id, const Signal *sig) {
@ -242,8 +253,7 @@ void ChartView::removeSeries(const QString &msg_id, const Signal *sig) {
}
bool ChartView::hasSeries(const QString &msg_id, const Signal *sig) const {
auto it = std::find_if(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; });
return it != sigs.end();
return std::any_of(sigs.begin(), sigs.end(), [&](auto &s) { return s.msg_id == msg_id && s.sig == sig; });
}
QList<ChartView::SigItem>::iterator ChartView::removeSeries(const QList<ChartView::SigItem>::iterator &it) {
@ -261,11 +271,11 @@ QList<ChartView::SigItem>::iterator ChartView::removeSeries(const QList<ChartVie
}
void ChartView::signalUpdated(const Signal *sig) {
auto it = std::find_if(sigs.begin(), sigs.end(), [=](auto &s) { return s.sig == sig; });
if (it != sigs.end()) {
if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.sig == sig; })) {
updateTitle();
// TODO: don't update series if only name changed.
updateSeries(sig);
updateAxisY();
}
}
@ -276,8 +286,7 @@ void ChartView::signalRemoved(const Signal *sig) {
}
void ChartView::msgUpdated(uint32_t address) {
auto it = std::find_if(sigs.begin(), sigs.end(), [=](auto &s) { return s.address == address; });
if (it != sigs.end())
if (std::any_of(sigs.begin(), sigs.end(), [=](auto &s) { return s.address == address; }))
updateTitle();
}
@ -392,7 +401,6 @@ void ChartView::updateSeries(const Signal *sig) {
s.series->replace(s.vals);
}
}
updateAxisY();
}
// auto zoom on yaxis
@ -422,15 +430,44 @@ void ChartView::updateAxisY() {
axis_y->setRange(min_y - 1, max_y + 1);
} else {
double range = max_y - min_y;
axis_y->setRange(min_y - range * 0.05, max_y + range * 0.05);
axis_y->applyNiceNumbers();
applyNiceNumbers(min_y - range * 0.05, max_y + range * 0.05);
}
QTimer::singleShot(0, this, &ChartView::adjustChartMargins);
adjustChartMargins();
}
void ChartView::applyNiceNumbers(qreal min, qreal max) {
int tick_count = axis_y->tickCount();
qreal range = niceNumber((max - min), true); // range with ceiling
qreal step = niceNumber(range / (tick_count - 1), false);
min = qFloor(min / step);
max = qCeil(max / step);
tick_count = int(max - min) + 1;
axis_y->setRange(min * step, max * step);
axis_y->setTickCount(tick_count);
}
// nice numbers can be expressed as form of 1*10^n, 2* 10^n or 5*10^n
qreal ChartView::niceNumber(qreal x, bool ceiling) {
qreal z = qPow(10, qFloor(std::log10(x))); //find corresponding number of the form of 10^n than is smaller than x
qreal q = x / z; //q<10 && q>=1;
if (ceiling) {
if (q <= 1.0) q = 1;
else if (q <= 2.0) q = 2;
else if (q <= 5.0) q = 5;
else q = 10;
} else {
if (q < 1.5) q = 1;
else if (q < 3.0) q = 2;
else if (q < 7.0) q = 5;
else q = 10;
}
return q * z;
}
void ChartView::leaveEvent(QEvent *event) {
item_group->setVisible(false);
track_pt = {0, 0};
scene()->update();
QChartView::leaveEvent(event);
}
@ -461,10 +498,9 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) {
auto rubber = findChild<QRubberBand *>();
bool is_zooming = rubber && rubber->isVisible();
const auto plot_area = chart()->plotArea();
track_pt = {0, 0};
if (!is_zooming && plot_area.contains(ev->pos())) {
QStringList text_list;
QPointF pos = {};
const double sec = chart()->mapToValue(ev->pos()).x();
for (auto &s : sigs) {
QString value = "--";
@ -473,35 +509,35 @@ void ChartView::mouseMoveEvent(QMouseEvent *ev) {
if (it != s.vals.rend() && it->x() >= axis_x->min()) {
value = QString::number(it->y());
auto value_pos = chart()->mapToPosition(*it);
if (value_pos.x() > pos.x()) pos = value_pos;
if (value_pos.x() > track_pt.x()) track_pt = value_pos;
}
text_list.push_back(QString("&nbsp;%1 : %2&nbsp;").arg(sigs.size() > 1 ? s.sig->name.c_str() : "Value").arg(value));
}
if (pos.x() == 0) pos = ev->pos();
QString time = QString::number(chart()->mapToValue(pos).x(), 'f', 3);
value_text->setHtml(QString("<div style=\"background-color: darkGray;color: white;\">&nbsp;Time: %1 &nbsp;<br />%2</div>")
.arg(time).arg(text_list.join("<br />")));
QRectF text_rect = value_text->boundingRect();
int text_x = pos.x() + 8;
if ((text_x + text_rect.width()) > plot_area.right()) {
text_x = pos.x() - text_rect.width() - 8;
}
value_text->setPos(text_x, pos.y() - text_rect.height() / 2);
track_line->setLine(pos.x(), plot_area.top(), pos.x(), plot_area.bottom());
track_ellipse->setRect(pos.x() - 5, pos.y() - 5, 10, 10);
item_group->setVisible(true);
if (track_pt.x() == 0) track_pt = ev->pos();
QString text = QString("<div style=\"background-color: darkGray;color: white;\">&nbsp;Time: %1 &nbsp;<br />%2</div>")
.arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3)
.arg(text_list.join("<br />"));
QPoint pt((int)track_pt.x() + 20, plot_area.top() - 20);
QToolTip::showText(mapToGlobal(pt), text, this, plot_area.toRect());
scene()->update();
} else {
item_group->setVisible(false);
QToolTip::hideText();
}
QChartView::mouseMoveEvent(ev);
}
void ChartView::drawForeground(QPainter *painter, const QRectF &rect) {
qreal x = chart()->mapToPosition(QPointF{can->currentSec(), 0}).x();
qreal y1 = chart()->plotArea().top() - 2;
qreal y2 = chart()->plotArea().bottom() + 2;
painter->setPen(QPen(chart()->titleBrush().color(), 2));
painter->drawLine(QPointF{x, chart()->plotArea().top() - 2}, QPointF{x, chart()->plotArea().bottom() + 2});
painter->drawLine(QPointF{x, y1}, QPointF{x, y2});
if (!track_pt.isNull()) {
painter->setPen(QPen(Qt::darkGray, 1, Qt::DashLine));
painter->drawLine(QPointF{track_pt.x(), y1}, QPointF{track_pt.x(), y2});
painter->setBrush(Qt::darkGray);
painter->drawEllipse(track_pt, 5, 5);
}
}
// SeriesSelector
@ -570,7 +606,7 @@ void SeriesSelector::addSignal(QListWidgetItem *item) {
addSeries(data[0], data[1], data[2]);
}
void SeriesSelector::addSeries(const QString &id, const QString& msg_name, const QString &sig_name) {
void SeriesSelector::addSeries(const QString &id, const QString &msg_name, const QString &sig_name) {
QStringList data({id, msg_name, sig_name});
for (int i = 0; i < chart_series->count(); ++i) {
if (chart_series->item(i)->data(Qt::UserRole).toStringList() == data) {

@ -4,10 +4,7 @@
#include <QDialogButtonBox>
#include <QLabel>
#include <QListWidget>
#include <QGraphicsEllipseItem>
#include <QGraphicsLineItem>
#include <QGraphicsProxyWidget>
#include <QGraphicsTextItem>
#include <QVBoxLayout>
#include <QtCharts/QChartView>
#include <QtCharts/QLineSeries>
@ -66,13 +63,12 @@ private:
void updateTitle();
void updateFromSettings();
void drawForeground(QPainter *painter, const QRectF &rect) override;
void applyNiceNumbers(qreal min, qreal max);
qreal niceNumber(qreal x, bool ceiling);
QValueAxis *axis_x;
QValueAxis *axis_y;
QGraphicsItemGroup *item_group;
QGraphicsLineItem *track_line;
QGraphicsEllipseItem *track_ellipse;
QGraphicsTextItem *value_text;
QPointF track_pt;
QGraphicsProxyWidget *close_btn_proxy;
QGraphicsProxyWidget *manage_btn_proxy;
std::pair<double, double> events_range = {0, 0};
@ -102,17 +98,20 @@ private:
void zoomReset();
void updateToolBar();
void removeAll();
void showAllData();
bool eventFilter(QObject *obj, QEvent *event) override;
ChartView *findChart(const QString &id, const Signal *sig);
QLabel *title_label;
QLabel *range_label;
bool docking = true;
QAction *show_all_values_btn;
QAction *dock_btn;
QAction *reset_zoom_btn;
QAction *remove_all_btn;
QVBoxLayout *charts_layout;
QList<ChartView *> charts;
uint32_t max_chart_range = 0;
bool is_zoomed = false;
std::pair<double, double> event_range;
std::pair<double, double> display_range;

@ -68,9 +68,11 @@ void HistoryLogModel::updateState() {
// HeaderView
QSize HeaderView::sectionSizeFromContents(int logicalIndex) const {
int default_size = qMax(100, rect().width() / model()->columnCount());
const QString text = model()->headerData(logicalIndex, this->orientation(), Qt::DisplayRole).toString();
const QRect rect = fontMetrics().boundingRect(QRect(0, 0, sectionSize(logicalIndex), 1000), defaultAlignment(), text);
return rect.size() + QSize{10, 6};
const QRect rect = fontMetrics().boundingRect({0, 0, default_size, 2000}, defaultAlignment(), text);
QSize size = rect.size() + QSize{10, 6};
return {qMax(size.width(), default_size), size.height()};
}
void HeaderView::paintSection(QPainter *painter, const QRect &rect, int logicalIndex) const {
@ -81,7 +83,7 @@ void HeaderView::paintSection(QPainter *painter, const QRect &rect, int logicalI
painter->fillRect(rect, bg_role.value<QBrush>());
}
QString text = model()->headerData(logicalIndex, Qt::Horizontal, Qt::DisplayRole).toString();
painter->drawText(rect.adjusted(5, 3, 5, 3), defaultAlignment(), text);
painter->drawText(rect.adjusted(5, 3, -5, -3), defaultAlignment(), text);
}
// HistoryLog
@ -90,15 +92,13 @@ HistoryLog::HistoryLog(QWidget *parent) : QTableView(parent) {
model = new HistoryLogModel(this);
setModel(model);
setHorizontalHeader(new HeaderView(Qt::Horizontal, this));
horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch);
horizontalHeader()->setDefaultAlignment(Qt::AlignLeft | (Qt::Alignment)Qt::TextWordWrap);
horizontalHeader()->setSectionResizeMode(0, QHeaderView::ResizeToContents);
horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents);
verticalHeader()->setVisible(false);
setFrameShape(QFrame::NoFrame);
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Expanding);
}
int HistoryLog::sizeHintForColumn(int column) const {
// sizeHintForColumn is only called for column 0 (ResizeToContents)
return itemDelegate()->sizeHint(viewOptions(), model->index(0, 0)).width() + 5;
return -1;
}

@ -128,8 +128,8 @@ def ui_thread(addr):
sm.update(0)
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steeringAngleDeg
if w == 'lqrStateDEPRECATED':
angle_steers_k = sm['controlsState'].lateralControlState.lqrStateDEPRECATED.steeringAngleDeg
elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
else:

@ -45,7 +45,7 @@ fi
eval "$(pyenv init --path)"
echo "update pip"
pip install pip==22.3
pip install pip==22.3.1
pip install poetry==1.2.2
poetry config virtualenvs.prefer-active-python true --local

Loading…
Cancel
Save