diff --git a/cereal b/cereal index 7765176413..439429cad4 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 7765176413c0bb14143fe2469d5390ea0ea61a1e +Subproject commit 439429cad4d1e2ab874520cb5d4db8b8d978cbde diff --git a/poetry.lock b/poetry.lock index 39fb78cf2a..6360f6f3ab 100644 --- a/poetry.lock +++ b/poetry.lock @@ -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"}, diff --git a/pyproject.toml b/pyproject.toml index d26fc7ff1d..a20868d877 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -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" diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 0d5d780bd3..00673a7e28 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -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 diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 879da88123..31cd01b654 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index a6cd2f19b9..d380abf8e3 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -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 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 66a1485dc6..ab944a30aa 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 3f128b1598..ebb67b0e2e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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 diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 8b53e7c378..c492e6a5ff 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -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 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index b9f6b8fc58..e271dc0e7d 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 1e9d0f7f62..41b6a58eca 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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', diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 2add59ccb0..027822cc3f 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -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 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index b5429daef2..a56e63408e 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -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 diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 3b212c6507..eb1a04cee6 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -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] diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index ffda8c1712..cedd414d1c 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a102b005b8..db5382676a 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index ad06a5a726..542204deb6 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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.] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index ca96a437a0..317f1177db 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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', diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index fff5548671..628962de75 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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 diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 263edbfc9d..7ea9aa871b 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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: diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 25a710dbb8..30a51f6fe6 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -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) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 6beb90c092..130f107950 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index fcebeae8d3..6daf5b0bed 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 5578a83a23..9fbf565328 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -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), }, diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index fabb41d99c..edf8f9232a 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 0e5be4a977..d692f80b4b 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -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 diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index cfc71a0e27..0a8a8d6358 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -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; diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 9cb216ff19..ac101bfee7 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -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; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index b23691a56a..5734c68cb9 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -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, diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc index dae87c7ce5..861795e172 100644 --- a/selfdrive/modeld/models/nav.cc +++ b/selfdrive/modeld/models/nav.cc @@ -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); } diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h index b469f75987..b2955ad2c5 100644 --- a/selfdrive/modeld/models/nav.h +++ b/selfdrive/modeld/models/nav.h @@ -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 mean; std::array std; - float prob; }; -static_assert(sizeof(NavModelOutputPlan) == sizeof(NavModelOutputXY)*TRAJECTORY_SIZE*2 + sizeof(float)); - -struct NavModelOutputPlans { - std::array 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 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; diff --git a/selfdrive/modeld/models/navmodel.onnx b/selfdrive/modeld/models/navmodel.onnx index 60dd8c0e7f..397b8d90e2 100644 --- a/selfdrive/modeld/models/navmodel.onnx +++ b/selfdrive/modeld/models/navmodel.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:eab4b986e14d7d842d6d5487011c329d356fb56995b2ae7dc7188aefe6df9d97 -size 12285002 +oid sha256:9028b36a591763724e95205b48f37f09260b4434fb1f3c6f228db1710a401aa8 +size 12258591 diff --git a/selfdrive/modeld/models/navmodel_q.dlc b/selfdrive/modeld/models/navmodel_q.dlc index 7d9b36ed4d..984cf0a553 100644 --- a/selfdrive/modeld/models/navmodel_q.dlc +++ b/selfdrive/modeld/models/navmodel_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:83d53efc40053b02fe7d3da4ef6213a4a5a1ae4d1bd49c121b9beb6a54ea1148 -size 3154868 +oid sha256:bc7ade56bb4f9525c84a46df22252ea1e321a0518cbcef8bdfc76ccd8ad10b41 +size 3154304 diff --git a/selfdrive/modeld/runners/onnxmodel.cc b/selfdrive/modeld/runners/onnxmodel.cc index 447d90fd7e..5616a6c9c7 100644 --- a/selfdrive/modeld/runners/onnxmodel.cc +++ b/selfdrive/modeld/runners/onnxmodel.cc @@ -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) { diff --git a/selfdrive/modeld/runners/onnxmodel.h b/selfdrive/modeld/runners/onnxmodel.h index d5b7bfecf0..9990bf1b45 100644 --- a/selfdrive/modeld/runners/onnxmodel.h +++ b/selfdrive/modeld/runners/onnxmodel.h @@ -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; diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h index c607811401..673ddb50b5 100644 --- a/selfdrive/modeld/runners/runmodel.h +++ b/selfdrive/modeld/runners/runmodel.h @@ -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) {} diff --git a/selfdrive/modeld/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc index ff4adcd8d3..609a7a6657 100644 --- a/selfdrive/modeld/runners/snpemodel.cc +++ b/selfdrive/modeld/runners/snpemodel.cc @@ -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); diff --git a/selfdrive/modeld/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h index 08ae16c2b1..0d84d1d482 100644 --- a/selfdrive/modeld/runners/snpemodel.h +++ b/selfdrive/modeld/runners/snpemodel.h @@ -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 trafficConventionBuffer; float *desire; std::unique_ptr desireBuffer; + float *navFeatures; + std::unique_ptr navFeaturesBuffer; + float *drivingStyle; + std::unique_ptr drivingStyleBuffer; float *calib; std::unique_ptr calibBuffer; }; diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc index 67db01bb95..4fdd7ca466 100644 --- a/selfdrive/modeld/runners/thneedmodel.cc +++ b/selfdrive/modeld/runners/thneedmodel.cc @@ -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; } diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h index f3f34dc7f4..63712f1d00 100644 --- a/selfdrive/modeld/runners/thneedmodel.h +++ b/selfdrive/modeld/runners/thneedmodel.h @@ -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; }; diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index a9c34b5bd5..91f31d06dc 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -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 &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 buffer_kj = kj::heapArray((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 buffer_kj = kj::heapArray((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++; } diff --git a/selfdrive/navd/map_renderer.h b/selfdrive/navd/map_renderer.h index 7f725c93fe..0019030b6d 100644 --- a/selfdrive/navd/map_renderer.h +++ b/selfdrive/navd/map_renderer.h @@ -34,7 +34,8 @@ private: std::unique_ptr vipc_server; std::unique_ptr pm; std::unique_ptr sm; - void sendVipc(); + void publish(const double render_time); + void sendThumbnail(const uint64_t ts, const kj::Array &buf); QMapboxGLSettings m_settings; QScopedPointer m_map; diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index e5e21a6fef..c96e624421 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -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) diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 1f43bb791a..ce861b37e6 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -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 += [ diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index b58cca1aaf..aa204e0f8a 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -4b2c6516cd460ee443b9006f01233168edf3d170 \ No newline at end of file +4ff972367fdb9546be68ee0ba0d45cf4f839dae7 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 4641299458..80385710a9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f17412941a0e8229eea308c33189a5bdb1a17ae8 +6681ca22053b019a65930e76a396535d0cddf39c \ No newline at end of file diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index fed59ef123..95ad813dff 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -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); diff --git a/tools/cabana/binaryview.cc b/tools/cabana/binaryview.cc index aa256b4ce1..b98c75d452 100644 --- a/tools/cabana/binaryview.cc +++ b/tools/cabana/binaryview.cc @@ -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(); } diff --git a/tools/cabana/binaryview.h b/tools/cabana/binaryview.h index c37b378e44..e936efc658 100644 --- a/tools/cabana/binaryview.h +++ b/tools/cabana/binaryview.h @@ -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; }; diff --git a/tools/cabana/chartswidget.cc b/tools/cabana/chartswidget.cc index 79b40133a5..b177bb0b81 100644 --- a/tools/cabana/chartswidget.cc +++ b/tools/cabana/chartswidget.cc @@ -3,9 +3,9 @@ #include #include #include -#include #include #include +#include #include // 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 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::iterator ChartView::removeSeries(const QList::iterator &it) { @@ -261,11 +271,11 @@ QList::iterator ChartView::removeSeries(const QListreplace(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(); 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(" %1 : %2 ").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("
 Time: %1  
%2
") - .arg(time).arg(text_list.join("
"))); - - 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("
 Time: %1  
%2
") + .arg(chart()->mapToValue(track_pt).x(), 0, 'f', 3) + .arg(text_list.join("
")); + 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) { diff --git a/tools/cabana/chartswidget.h b/tools/cabana/chartswidget.h index 3e3277c5b8..3f9cf877a9 100644 --- a/tools/cabana/chartswidget.h +++ b/tools/cabana/chartswidget.h @@ -4,10 +4,7 @@ #include #include #include -#include -#include #include -#include #include #include #include @@ -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 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 charts; + uint32_t max_chart_range = 0; bool is_zoomed = false; std::pair event_range; std::pair display_range; diff --git a/tools/cabana/historylog.cc b/tools/cabana/historylog.cc index 5457555db4..ec9a0b011c 100644 --- a/tools/cabana/historylog.cc +++ b/tools/cabana/historylog.cc @@ -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()); } 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; } diff --git a/tools/replay/ui.py b/tools/replay/ui.py index bbcd49061e..43cbef5bd6 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -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: diff --git a/update_requirements.sh b/update_requirements.sh index 9195799ca3..e5dedea7a6 100755 --- a/update_requirements.sh +++ b/update_requirements.sh @@ -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