From 573db49deb255636d6d02756eba9d520cff3c0ae Mon Sep 17 00:00:00 2001 From: Vivek Aithal Date: Tue, 28 Nov 2023 15:10:30 -0800 Subject: [PATCH 01/87] torqued: Update HYUNDAI TUCSON 4TH GEN offline values (#30513) * update tucson offline values * modify params, compute max accel --- selfdrive/car/torque_data/params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/torque_data/params.yaml b/selfdrive/car/torque_data/params.yaml index 2d2dbc3f0b..7bd83025f1 100644 --- a/selfdrive/car/torque_data/params.yaml +++ b/selfdrive/car/torque_data/params.yaml @@ -37,7 +37,7 @@ HYUNDAI SANTA FE PlUG-IN HYBRID 2022: [1.6953050513611045, 1.5837614296206861, 0 HYUNDAI SONATA 2019: [2.2200457811703953, 1.2967330275895228, 0.14039920986586393] HYUNDAI SONATA 2020: [2.9638737459977467, 2.1259108157250735, 0.07813665616927593] HYUNDAI SONATA HYBRID 2021: [2.8990264092395734, 2.061410192222139, 0.0899805488717382] -HYUNDAI TUCSON HYBRID 4TH GEN: [2.035545, 2.035545, 0.110272] +HYUNDAI TUCSON HYBRID 4TH GEN: [2.960174, 2.860284, 0.108745] JEEP GRAND CHEROKEE 2019: [2.30972, 1.289689569171081, 0.117048] JEEP GRAND CHEROKEE V6 2018: [2.27116, 1.4057367824262523, 0.11725947414922003] KIA EV6 2022: [3.2, 2.093457, 0.05] From 9634e7b8afeebddd5d32d8b659d733dc85ff34c6 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 29 Nov 2023 12:55:52 -0800 Subject: [PATCH 02/87] Subaru: log eyesight fault as a cruise fault (#30546) log cruise fault --- selfdrive/car/subaru/carstate.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index c8a6dfe1e6..b51fcc6846 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -19,6 +19,8 @@ class CarState(CarStateBase): def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() + cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam + throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] ret.gas = throttle_msg["Throttle_Pedal"] / 255. @@ -29,6 +31,14 @@ class CarState(CarStateBase): cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 + if self.car_fingerprint not in HYBRID_CARS: + eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"]) + + if self.CP.openpilotLongitudinalControl: + ret.carFaultedNonCritical = eyesight_fault + else: + ret.accFaulted = eyesight_fault # if openpilot isn't controlling long, an eyesight fault is an acc fault + cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp ret.wheelSpeeds = self.get_wheel_speeds( cp_wheels.vl["Wheel_Speeds"]["FL"], @@ -84,7 +94,6 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 - cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam if self.car_fingerprint in PREGLOBAL_CARS: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] From dcbff66f0c38496c76f7505cf283b9d66def9aab Mon Sep 17 00:00:00 2001 From: DriftedPrism Date: Wed, 29 Nov 2023 17:16:17 -0500 Subject: [PATCH 03/87] Kia: add missing fwdCamea FW for Sorento PHEV 4th gen (#30526) * Update values.py Adding Ecus for KIA Sorento PHEV 4th gen * Update values.py added fwdCamera version * fix --------- Co-authored-by: Shane Smiskol --- selfdrive/car/hyundai/values.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 0a0f66eabd..41ccd576fc 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -1912,7 +1912,8 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00MQ4HMFC AT USA LHD 1.00 1.11 99210-P2000 211217', - ] + b'\xf1\x00MQ4HMFC AT USA LHD 1.00 1.10 99210-P2000 210406', + ], }, CAR.KIA_EV6: { (Ecu.fwdRadar, 0x7d0, None): [ From e61ea6641559ec1d0cbae3b816d4dcf4957382ea Mon Sep 17 00:00:00 2001 From: ebo2k <33906082+ebo2k@users.noreply.github.com> Date: Wed, 29 Nov 2023 14:26:20 -0800 Subject: [PATCH 04/87] Ford: add missing engine FW for Bronco Sport 2021 (#30483) Update values.py Updated to have correct engine firmware --- selfdrive/car/ford/values.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 133038809f..ef8708f01d 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -142,6 +142,7 @@ FW_VERSIONS = { b'M1PA-14C204-GF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'N1PA-14C204-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'N1PA-14C204-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'M1PA-14C204-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.ESCAPE_MK4: { From 4f8a2323828dd1eb3a608b81c48e7bced27abe6f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 29 Nov 2023 16:27:37 -0600 Subject: [PATCH 05/87] Car docs: add '19-'20 Santa Fe video (#30553) * Add '19-'20 Santa Fe video Add link to YouTube video for 2019-2020 Hyundai Santa Fe * update docs --------- Co-authored-by: Steve J --- docs/CARS.md | 2 +- selfdrive/car/hyundai/values.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 044db1f0ec..3a67a0151a 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -99,7 +99,7 @@ A supported vehicle is one that just works when you install a comma device. All |Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai I connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Palisade 2020-22|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai H connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Santa Cruz 2022-23[6](#footnotes)|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai N connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Hyundai|Santa Fe 2019-20|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai D connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Santa Fe 2021-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Santa Fe Hybrid 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Santa Fe Plug-in Hybrid 2022-23|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai L connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 41ccd576fc..222213322f 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -191,7 +191,8 @@ CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { # TODO: this is the 2024 US MY, not yet released CAR.KONA_EV_2ND_GEN: HyundaiCarInfo("Hyundai Kona Electric (with HDA II, Korea only) 2023", video_link="https://www.youtube.com/watch?v=U2fOCmcQ8hw", car_parts=CarParts.common([CarHarness.hyundai_r])), - CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", car_parts=CarParts.common([CarHarness.hyundai_d])), + CAR.SANTA_FE: HyundaiCarInfo("Hyundai Santa Fe 2019-20", "All", video_link="https://youtu.be/bjDR0YjM__s", + car_parts=CarParts.common([CarHarness.hyundai_d])), CAR.SANTA_FE_2022: HyundaiCarInfo("Hyundai Santa Fe 2021-23", "All", video_link="https://youtu.be/VnHzSTygTS4", car_parts=CarParts.common([CarHarness.hyundai_l])), CAR.SANTA_FE_HEV_2022: HyundaiCarInfo("Hyundai Santa Fe Hybrid 2022-23", "All", car_parts=CarParts.common([CarHarness.hyundai_l])), From 44ef143ae7cc855d6f4ce4b8328b90ab4f125fec Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 29 Nov 2023 14:53:43 -0800 Subject: [PATCH 06/87] Subaru: add 2023 forester (#30554) 23 --- selfdrive/car/subaru/values.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 882d46b7c3..057e0ffc0c 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -126,7 +126,7 @@ CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), CAR.OUTBACK_PREGLOBAL_2018: SubaruCarInfo("Subaru Outback 2018-19"), - CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", "All", car_parts=CarParts.common([CarHarness.subaru_c])), + CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022-23", "All", car_parts=CarParts.common([CarHarness.subaru_c])), CAR.OUTBACK_2023: SubaruCarInfo("Subaru Outback 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), CAR.ASCENT_2023: SubaruCarInfo("Subaru Ascent 2023", "All", car_parts=CarParts.common([CarHarness.subaru_d])), } @@ -671,7 +671,8 @@ FW_VERSIONS = { b'=\xc04\x02', ], (Ecu.fwdCamera, 0x787, None): [ - b'\x04!\x01\x1eD\x07!\x00\x04,' + b'\x04!\x01\x1eD\x07!\x00\x04,', + b'\x04!\x08\x01.\x07!\x08\x022', ], (Ecu.engine, 0x7e0, None): [ b'\xd5"a0\x07', From a2bb41e0ec5fd96763e9a01a7116efda059de6e6 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 29 Nov 2023 17:47:09 -0800 Subject: [PATCH 07/87] Subaru: non-obd FW queries logging (#30552) * add subaru logging fw * whitelist + comma * whitelist is empty for the other requests * all whitelisted --- selfdrive/car/subaru/values.py | 18 ++++++++++++++++++ selfdrive/car/tests/test_fw_fingerprint.py | 4 ++-- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 057e0ffc0c..915785d329 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -141,12 +141,30 @@ FW_QUERY_CONFIG = FwQueryConfig( Request( [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], ), # Some Eyesight modules fail on TESTER_PRESENT_REQUEST # TODO: check if this resolves the fingerprinting issue for the 2023 Ascent and other new Subaru cars Request( [SUBARU_VERSION_REQUEST], [SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.fwdCamera], + ), + # Non-OBD requests + Request( + [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], + bus=0, + logging=True, + ), + Request( + [StdQueries.TESTER_PRESENT_REQUEST, SUBARU_VERSION_REQUEST], + [StdQueries.TESTER_PRESENT_RESPONSE, SUBARU_VERSION_RESPONSE], + whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.fwdCamera, Ecu.engine, Ecu.transmission], + bus=1, + logging=True, + obd_multiplexing=False, ), ], ) diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index 6777aeb738..237266dbed 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -227,7 +227,7 @@ class TestFwFingerprintTiming(unittest.TestCase): @pytest.mark.timeout(60) def test_fw_query_timing(self): - total_ref_time = 6.07 + total_ref_time = 6.27 brand_ref_times = { 1: { 'body': 0.11, @@ -237,7 +237,7 @@ class TestFwFingerprintTiming(unittest.TestCase): 'hyundai': 0.72, 'mazda': 0.2, 'nissan': 0.4, - 'subaru': 0.2, + 'subaru': 0.4, 'tesla': 0.2, 'toyota': 1.6, 'volkswagen': 0.2, From b1e2e0f565010191f0f624487c4b74c63008f03a Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 30 Nov 2023 07:57:29 -0800 Subject: [PATCH 08/87] Revert "Subaru: log eyesight fault as a cruise fault" (#30560) --- selfdrive/car/subaru/carstate.py | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index b51fcc6846..c8a6dfe1e6 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -19,8 +19,6 @@ class CarState(CarStateBase): def update(self, cp, cp_cam, cp_body): ret = car.CarState.new_message() - cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam - throttle_msg = cp.vl["Throttle"] if self.car_fingerprint not in HYBRID_CARS else cp_body.vl["Throttle_Hybrid"] ret.gas = throttle_msg["Throttle_Pedal"] / 255. @@ -31,14 +29,6 @@ class CarState(CarStateBase): cp_brakes = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp ret.brakePressed = cp_brakes.vl["Brake_Status"]["Brake"] == 1 - if self.car_fingerprint not in HYBRID_CARS: - eyesight_fault = bool(cp_es_distance.vl["ES_Distance"]["Cruise_Fault"]) - - if self.CP.openpilotLongitudinalControl: - ret.carFaultedNonCritical = eyesight_fault - else: - ret.accFaulted = eyesight_fault # if openpilot isn't controlling long, an eyesight fault is an acc fault - cp_wheels = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp ret.wheelSpeeds = self.get_wheel_speeds( cp_wheels.vl["Wheel_Speeds"]["FL"], @@ -94,6 +84,7 @@ class CarState(CarStateBase): cp.vl["BodyInfo"]["DOOR_OPEN_FL"]]) ret.steerFaultPermanent = cp.vl["Steering_Torque"]["Steer_Error_1"] == 1 + cp_es_distance = cp_body if self.car_fingerprint in (GLOBAL_GEN2 | HYBRID_CARS) else cp_cam if self.car_fingerprint in PREGLOBAL_CARS: self.cruise_button = cp_cam.vl["ES_Distance"]["Cruise_Button"] self.ready = not cp_cam.vl["ES_DashStatus"]["Not_Ready_Startup"] From 4086795c29bacd245a7cc372a16730e1a6ff974f Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 30 Nov 2023 09:55:48 -0800 Subject: [PATCH 09/87] Pytest: more post-test path cleanup (#30556) * test cleanup * missed this one * remove that * static class --- common/api/__init__.py | 4 ++-- common/basedir.py | 9 +------- selfdrive/athena/athenad.py | 7 +++---- selfdrive/athena/registration.py | 6 +++--- selfdrive/athena/tests/test_athenad_ping.py | 5 +++-- selfdrive/athena/tests/test_registration.py | 16 +++++--------- selfdrive/statsd.py | 5 ++++- .../test_longitudinal.py | 1 + system/hardware/hw.py | 21 +++++++++++++++++++ system/loggerd/config.py | 6 ------ tools/lib/auth_config.py | 15 +++++-------- 11 files changed, 48 insertions(+), 47 deletions(-) diff --git a/common/api/__init__.py b/common/api/__init__.py index 0eb8aa7627..79875023a2 100644 --- a/common/api/__init__.py +++ b/common/api/__init__.py @@ -2,7 +2,7 @@ import jwt import os import requests from datetime import datetime, timedelta -from openpilot.common.basedir import PERSIST +from openpilot.system.hardware.hw import Paths from openpilot.system.version import get_version API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') @@ -10,7 +10,7 @@ API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com') class Api(): def __init__(self, dongle_id): self.dongle_id = dongle_id - with open(PERSIST+'/comma/id_rsa') as f: + with open(Paths.persist_root()+'/comma/id_rsa') as f: self.private_key = f.read() def get(self, *args, **kwargs): diff --git a/common/basedir.py b/common/basedir.py index b4486f9f08..c840b86f7f 100644 --- a/common/basedir.py +++ b/common/basedir.py @@ -1,11 +1,4 @@ import os -from pathlib import Path -from openpilot.system.hardware import PC -BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) - -if PC: - PERSIST = os.path.join(str(Path.home()), ".comma", "persist") -else: - PERSIST = "/persist" +BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../")) \ No newline at end of file diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index eb61c6dd58..423479517c 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -31,13 +31,11 @@ import cereal.messaging as messaging from cereal import log from cereal.services import SERVICE_LIST from openpilot.common.api import Api -from openpilot.common.basedir import PERSIST from openpilot.common.file_helpers import CallbackReader from openpilot.common.params import Params from openpilot.common.realtime import set_core_affinity from openpilot.system.hardware import HARDWARE, PC, AGNOS from openpilot.system.loggerd.xattr_cache import getxattr, setxattr -from openpilot.selfdrive.statsd import STATS_DIR from openpilot.system.swaglog import cloudlog from openpilot.system.version import get_commit, get_origin, get_short_branch, get_version from openpilot.system.hardware.hw import Paths @@ -502,10 +500,10 @@ def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local @dispatcher.add_method def getPublicKey() -> Optional[str]: - if not os.path.isfile(PERSIST + '/comma/id_rsa.pub'): + if not os.path.isfile(Paths.persist_root() + '/comma/id_rsa.pub'): return None - with open(PERSIST + '/comma/id_rsa.pub') as f: + with open(Paths.persist_root() + '/comma/id_rsa.pub') as f: return f.read() @@ -641,6 +639,7 @@ def log_handler(end_event: threading.Event) -> None: def stat_handler(end_event: threading.Event) -> None: + STATS_DIR = Paths.stats_root() while not end_event.is_set(): last_scan = 0. curr_scan = time.monotonic() diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 0ab69371c2..7467e7fa86 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -9,9 +9,9 @@ from datetime import datetime, timedelta from openpilot.common.api import api_get from openpilot.common.params import Params from openpilot.common.spinner import Spinner -from openpilot.common.basedir import PERSIST from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, PC +from openpilot.system.hardware.hw import Paths from openpilot.system.swaglog import cloudlog @@ -32,7 +32,7 @@ def register(show_spinner=False) -> Optional[str]: dongle_id: Optional[str] = params.get("DongleId", encoding='utf8') needs_registration = None in (IMEI, HardwareSerial, dongle_id) - pubkey = Path(PERSIST+"/comma/id_rsa.pub") + pubkey = Path(Paths.persist_root()+"/comma/id_rsa.pub") if not pubkey.is_file(): dongle_id = UNREGISTERED_DONGLE_ID cloudlog.warning(f"missing public key: {pubkey}") @@ -42,7 +42,7 @@ def register(show_spinner=False) -> Optional[str]: spinner.update("registering device") # Create registration token, in the future, this key will make JWTs directly - with open(PERSIST+"/comma/id_rsa.pub") as f1, open(PERSIST+"/comma/id_rsa") as f2: + with open(Paths.persist_root()+"/comma/id_rsa.pub") as f1, open(Paths.persist_root()+"/comma/id_rsa") as f2: public_key = f1.read() private_key = f2.read() diff --git a/selfdrive/athena/tests/test_athenad_ping.py b/selfdrive/athena/tests/test_athenad_ping.py index 3ec7cb115c..2958ec2262 100755 --- a/selfdrive/athena/tests/test_athenad_ping.py +++ b/selfdrive/athena/tests/test_athenad_ping.py @@ -40,8 +40,6 @@ class TestAthenadPing(unittest.TestCase): @classmethod def setUpClass(cls) -> None: - cls.params = Params() - cls.dongle_id = cls.params.get("DongleId", encoding="utf-8") cls._create_connection = athenad.create_connection athenad.create_connection = MagicMock(wraps=cls._create_connection) @@ -51,6 +49,9 @@ class TestAthenadPing(unittest.TestCase): athenad.create_connection = cls._create_connection def setUp(self) -> None: + self.params = Params() + self.dongle_id = self.params.get("DongleId", encoding="utf-8") + wifi_radio(True) self._clear_ping_time() diff --git a/selfdrive/athena/tests/test_registration.py b/selfdrive/athena/tests/test_registration.py index 195fca2df9..e7ad63a370 100755 --- a/selfdrive/athena/tests/test_registration.py +++ b/selfdrive/athena/tests/test_registration.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 import json -import os -import tempfile import unittest from Crypto.PublicKey import RSA from pathlib import Path @@ -10,6 +8,7 @@ from unittest import mock from openpilot.common.params import Params from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID from openpilot.selfdrive.athena.tests.helpers import MockResponse +from openpilot.system.hardware.hw import Paths class TestRegistration(unittest.TestCase): @@ -19,16 +18,11 @@ class TestRegistration(unittest.TestCase): self.params = Params() self.params.clear_all() - self.persist = tempfile.TemporaryDirectory() - os.mkdir(os.path.join(self.persist.name, "comma")) - self.priv_key = Path(os.path.join(self.persist.name, "comma/id_rsa")) - self.pub_key = Path(os.path.join(self.persist.name, "comma/id_rsa.pub")) - self.persist_patcher = mock.patch("openpilot.selfdrive.athena.registration.PERSIST", self.persist.name) - self.persist_patcher.start() + persist_dir = Path(Paths.persist_root()) / "comma" + persist_dir.mkdir(parents=True, exist_ok=True) - def tearDown(self): - self.persist_patcher.stop() - self.persist.cleanup() + self.priv_key = persist_dir / "id_rsa" + self.pub_key = persist_dir / "id_rsa.pub" def _generate_keys(self): self.pub_key.touch() diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index 8acf406515..e41f41a621 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -9,11 +9,12 @@ from typing import NoReturn, Union, List, Dict from openpilot.common.params import Params from cereal.messaging import SubMaster +from openpilot.system.hardware.hw import Paths from openpilot.system.swaglog import cloudlog from openpilot.system.hardware import HARDWARE from openpilot.common.file_helpers import atomic_write_in_dir from openpilot.system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty -from openpilot.system.loggerd.config import STATS_DIR, STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S +from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S class METRIC_TYPE: @@ -80,6 +81,8 @@ def main() -> NoReturn: sock = ctx.socket(zmq.PULL) sock.bind(STATS_SOCKET) + STATS_DIR = Paths.stats_root() + # initialize stats directory Path(STATS_DIR).mkdir(parents=True, exist_ok=True) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index a3b307ccba..f24f788a19 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -156,6 +156,7 @@ class LongitudinalControl(unittest.TestCase): os.environ['SKIP_FW_QUERY'] = "1" os.environ['NO_CAN_TIMEOUT'] = "1" + def setUp(self): params = Params() params.clear_all() params.put_bool("Passive", bool(os.getenv("PASSIVE"))) diff --git a/system/hardware/hw.py b/system/hardware/hw.py index 119ca48c86..6c3d90345c 100644 --- a/system/hardware/hw.py +++ b/system/hardware/hw.py @@ -33,3 +33,24 @@ class Paths: if os.environ.get('COMMA_CACHE', False): return os.environ['COMMA_CACHE'] return "/tmp/comma_download_cache" + os.environ.get("OPENPILOT_PREFIX", "") + "/" + + @staticmethod + def persist_root() -> str: + if PC: + return os.path.join(Paths.comma_home(), "persist") + else: + return "/persist/" + + @staticmethod + def stats_root() -> str: + if PC: + return str(Path(Paths.comma_home()) / "stats") + else: + return "/data/stats/" + + @staticmethod + def config_root() -> str: + if PC: + return Paths.comma_home() + else: + return "/tmp/.comma" \ No newline at end of file diff --git a/system/loggerd/config.py b/system/loggerd/config.py index 664e78b378..5c2c89b73f 100644 --- a/system/loggerd/config.py +++ b/system/loggerd/config.py @@ -1,6 +1,4 @@ import os -from pathlib import Path -from openpilot.system.hardware import PC from openpilot.system.hardware.hw import Paths @@ -9,10 +7,6 @@ SEGMENT_LENGTH = 60 STATS_DIR_FILE_LIMIT = 10000 STATS_SOCKET = "ipc:///tmp/stats" -if PC: - STATS_DIR = str(Path.home() / ".comma" / "stats") -else: - STATS_DIR = "/data/stats/" STATS_FLUSH_TIME_S = 60 def get_available_percent(default=None): diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py index bd76761043..e0989f02ea 100644 --- a/tools/lib/auth_config.py +++ b/tools/lib/auth_config.py @@ -1,21 +1,16 @@ import json import os from openpilot.common.file_helpers import mkdirs_exists_ok -from openpilot.system.hardware import PC +from openpilot.system.hardware.hw import Paths class MissingAuthConfigError(Exception): pass -if PC: - CONFIG_DIR = os.path.expanduser('~/.comma') -else: - CONFIG_DIR = "/tmp/.comma" - def get_token(): try: - with open(os.path.join(CONFIG_DIR, 'auth.json')) as f: + with open(os.path.join(Paths.config_root(), 'auth.json')) as f: auth = json.load(f) return auth['access_token'] except Exception: @@ -23,13 +18,13 @@ def get_token(): def set_token(token): - mkdirs_exists_ok(CONFIG_DIR) - with open(os.path.join(CONFIG_DIR, 'auth.json'), 'w') as f: + mkdirs_exists_ok(Paths.config_root()) + with open(os.path.join(Paths.config_root(), 'auth.json'), 'w') as f: json.dump({'access_token': token}, f) def clear_token(): try: - os.unlink(os.path.join(CONFIG_DIR, 'auth.json')) + os.unlink(os.path.join(Paths.config_root(), 'auth.json')) except FileNotFoundError: pass From 7e0f0165f4a036760cb3a0c179facffbe39e15ad Mon Sep 17 00:00:00 2001 From: Greg Hogan Date: Thu, 30 Nov 2023 10:42:18 -0800 Subject: [PATCH 10/87] replay: get api url from env var (#30562) --- tools/replay/route.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/replay/route.cc b/tools/replay/route.cc index 9168d25b35..d0ddf7f3c8 100644 --- a/tools/replay/route.cc +++ b/tools/replay/route.cc @@ -44,7 +44,7 @@ bool Route::loadFromServer() { loop.exit(success ? loadFromJson(json) : 0); }); - http.sendRequest("https://api.commadotai.com/v1/route/" + route_.str + "/files"); + http.sendRequest(CommaApi::BASE_URL + "/v1/route/" + route_.str + "/files"); return loop.exec(); } From 613ccf4b74ce70ac4bff75d1dbdd5ee7f8f4acdc Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 30 Nov 2023 14:53:33 -0800 Subject: [PATCH 11/87] falsify rx checks (#30564) --- selfdrive/car/tests/test_models.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index abc9bbc13a..f26fd363ca 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -221,7 +221,7 @@ class TestCarModelBase(unittest.TestCase): error_cnt += car.RadarData.Error.canError in rr.errors self.assertEqual(error_cnt, 0) - def test_panda_safety_rx_valid(self): + def test_panda_safety_rx_checks(self): if self.CP.dashcamOnly: self.skipTest("no need to check panda safety for dashcamOnly") @@ -256,6 +256,11 @@ class TestCarModelBase(unittest.TestCase): self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}") + # ensure RX checks go invalid after small time with no traffic + self.safety.set_timer(int(t + (2*1e6))) + self.safety.safety_tick_current_safety_config() + self.assertFalse(self.safety.safety_config_valid()) + def test_panda_safety_tx_cases(self, data=None): """Asserts we can tx common messages""" if self.CP.notCar: From 6ac75e492ad49cb16ffc752e64216a234f878442 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 30 Nov 2023 15:28:46 -0800 Subject: [PATCH 12/87] bump panda (#30563) --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index e6fadef4dd..b50455cc76 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e6fadef4ddccb9e665fbdf2d377f12879e5af07a +Subproject commit b50455cc76a0f66aa6faf953dd3ea023783a4b3f From e687be939e7cf67c4a75b87320ab058941d4316f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 30 Nov 2023 18:36:20 -0600 Subject: [PATCH 13/87] FW fingerprinting timing: fix refs (#30566) --- selfdrive/car/tests/test_fw_fingerprint.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index 237266dbed..5721896787 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -227,7 +227,7 @@ class TestFwFingerprintTiming(unittest.TestCase): @pytest.mark.timeout(60) def test_fw_query_timing(self): - total_ref_time = 6.27 + total_ref_time = 6.41 brand_ref_times = { 1: { 'body': 0.11, @@ -237,7 +237,7 @@ class TestFwFingerprintTiming(unittest.TestCase): 'hyundai': 0.72, 'mazda': 0.2, 'nissan': 0.4, - 'subaru': 0.4, + 'subaru': 0.52, 'tesla': 0.2, 'toyota': 1.6, 'volkswagen': 0.2, From d87191c1c1982e5c044e5654d7a69c5cc222986c Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 1 Dec 2023 12:46:31 +0800 Subject: [PATCH 14/87] cabana: fix missing opendbc files (#30555) * fix missing dbc files * remove suffix --- tools/cabana/mainwin.cc | 7 ++++--- tools/cabana/mainwin.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/tools/cabana/mainwin.cc b/tools/cabana/mainwin.cc index 71e5ade9bb..f7f2fba45f 100644 --- a/tools/cabana/mainwin.cc +++ b/tools/cabana/mainwin.cc @@ -77,9 +77,10 @@ void MainWindow::loadFingerprints() { QFile json_file(QApplication::applicationDirPath() + "/dbc/car_fingerprint_to_dbc.json"); if (json_file.open(QIODevice::ReadOnly)) { fingerprint_to_dbc = QJsonDocument::fromJson(json_file.readAll()); - auto dbc_names = fingerprint_to_dbc.object().toVariantMap().values(); - std::transform(dbc_names.begin(), dbc_names.end(), std::inserter(opendbc_names, opendbc_names.begin()), - [](const auto &name) { return name.toString(); }); + } + // get opendbc names + for (auto fn : QDir(OPENDBC_FILE_PATH).entryList({"*.dbc"}, QDir::Files, QDir::Name)) { + opendbc_names << QFileInfo(fn).baseName(); } } diff --git a/tools/cabana/mainwin.h b/tools/cabana/mainwin.h index 2b1c991271..1a889b88c2 100644 --- a/tools/cabana/mainwin.h +++ b/tools/cabana/mainwin.h @@ -86,7 +86,7 @@ protected: QProgressBar *progress_bar; QLabel *status_label; QJsonDocument fingerprint_to_dbc; - std::set opendbc_names; + QStringList opendbc_names; QSplitter *video_splitter = nullptr; enum { MAX_RECENT_FILES = 15 }; QAction *recent_files_acts[MAX_RECENT_FILES] = {}; From afd5877be8260afd840512d477b9164595343eed Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 30 Nov 2023 21:48:11 -0800 Subject: [PATCH 15/87] bump model unit test timeout --- .github/workflows/selfdrive_tests.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 4e622f68a5..94e6304539 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -268,7 +268,7 @@ jobs: ${{ env.RUN }} "scons -j$(nproc)" # PYTHONWARNINGS triggers a SyntaxError in onnxruntime - name: Run model replay with ONNX - timeout-minutes: 3 + timeout-minutes: 4 run: | ${{ env.RUN_CL }} "unset PYTHONWARNINGS && \ ONNXCPU=1 CI=1 NO_NAV=1 coverage run selfdrive/test/process_replay/model_replay.py && \ From 169132c6cc52f598b9b16d9996690f51282082e5 Mon Sep 17 00:00:00 2001 From: Hoya Date: Sat, 2 Dec 2023 07:59:49 +0900 Subject: [PATCH 16/87] Hyundai: add FW for 2019 AZERA_HEV_6TH_GEN (#30548) * added AZERA_HEV_5TH_GEN (2019) * Update values.py * Update interface.py * Update values.py * Update hyundaican.py * Update hyundaican.py * Update interface.py * Update values.py * one line * didnt see harness change * Update selfdrive/car/hyundai/values.py * docs --------- Co-authored-by: Justin Newberry Co-authored-by: Shane Smiskol --- docs/CARS.md | 3 ++- selfdrive/car/hyundai/values.py | 10 +++++++++- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index 3a67a0151a..f0004c5aa0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 268 Supported Cars +# 269 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -73,6 +73,7 @@ A supported vehicle is one that just works when you install a comma device. All |Honda|Pilot 2016-22|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Honda|Ridgeline 2017-23|Honda Sensing|openpilot|25 mph|12 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 Honda Nidec connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Azera 2022|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Hyundai|Azera Hybrid 2019|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai C connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Azera Hybrid 2020|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Custin 2023|All|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai K connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Hyundai|Elantra 2017-18|Smart Cruise Control (SCC)|Stock|19 mph|32 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Hyundai B connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 222213322f..7c6fa7cb4f 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -159,7 +159,10 @@ class HyundaiCarInfo(CarInfo): CAR_INFO: Dict[str, Optional[Union[HyundaiCarInfo, List[HyundaiCarInfo]]]] = { CAR.AZERA_6TH_GEN: HyundaiCarInfo("Hyundai Azera 2022", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), - CAR.AZERA_HEV_6TH_GEN: HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + CAR.AZERA_HEV_6TH_GEN: [ + HyundaiCarInfo("Hyundai Azera Hybrid 2019", "All", car_parts=CarParts.common([CarHarness.hyundai_c])), + HyundaiCarInfo("Hyundai Azera Hybrid 2020", "All", car_parts=CarParts.common([CarHarness.hyundai_k])), + ], CAR.ELANTRA: [ # TODO: 2017-18 could be Hyundai G HyundaiCarInfo("Hyundai Elantra 2017-18", min_enable_speed=19 * CV.MPH_TO_MS, car_parts=CarParts.common([CarHarness.hyundai_b])), @@ -556,18 +559,23 @@ FW_VERSIONS = { CAR.AZERA_HEV_6TH_GEN: { (Ecu.fwdCamera, 0x7C4, None): [ b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.02 99211-G8100 191029', + b'\xf1\x00IGH MFC AT KOR LHD 1.00 1.00 99211-G8000 180903', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00IG MDPS C 1.00 1.00 56310M9600\x00 4IHSC100', + b'\xf1\x00IG MDPS C 1.00 1.01 56310M9350\x00 4IH8C101', ], (Ecu.fwdRadar, 0x7d0, None): [ b'\xf1\x00IGhe SCC FHCUP 1.00 1.00 99110-M9100 ', + b'\xf1\x00IGhe SCC FHCUP 1.00 1.01 99110-M9000 ', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x006T7N0_C2\x00\x006T7VA051\x00\x00TIGSH24KA1\xc7\x85\xe2`', + b'\xf1\x006T7N0_C2\x00\x006T7Q2051\x00\x00TIG2H24KA2\x12@\x11\xb7', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x816H590051\x00\x00\x00\x00\x00\x00\x00\x00', + b'\xf1\x816H570051\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.HYUNDAI_GENESIS: { From 5dba9187e52d243a570bf10386ff9fe889e93abf Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 1 Dec 2023 19:55:27 -0800 Subject: [PATCH 17/87] CI: speedup docs build (#30574) --- .github/workflows/docs.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index 8b5b165536..c1d4eae52f 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -32,7 +32,7 @@ jobs: ${{ env.RUN }} "scons -j$(nproc)" - name: Build docs run: | - ${{ env.RUN }} "apt update && apt install -y doxygen && cd docs && make html" + ${{ env.RUN }} "apt update && apt install -y doxygen && cd docs && make -j$(nproc) html" - uses: actions/checkout@v4 if: github.ref == 'refs/heads/master' && github.repository == 'commaai/openpilot' From e34ee43eeae000d080cb923b722afaf4c06fbc59 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 1 Dec 2023 20:10:07 -0800 Subject: [PATCH 18/87] camerad cleanup (#30573) * misc cleanup * rm those * rm utils * fix build * rm pool * little more * goodbye imgproc --- docs/c_docs.rst | 2 - release/files_common | 5 -- system/camerad/SConscript | 9 +- system/camerad/cameras/camera_common.cc | 6 -- system/camerad/cameras/camera_common.h | 16 +--- system/camerad/cameras/camera_qcom2.cc | 76 ++++++++-------- system/camerad/cameras/camera_qcom2.h | 3 + system/camerad/cameras/camera_util.cc | 14 +-- system/camerad/cameras/camera_util.h | 2 +- system/camerad/imgproc/conv.cl | 110 ------------------------ system/camerad/imgproc/pool.cl | 34 -------- system/camerad/imgproc/utils.cc | 106 ----------------------- system/camerad/imgproc/utils.h | 37 -------- system/camerad/main.cc | 2 +- system/camerad/snapshot/snapshot.py | 1 - 15 files changed, 56 insertions(+), 367 deletions(-) delete mode 100644 system/camerad/imgproc/conv.cl delete mode 100644 system/camerad/imgproc/pool.cl delete mode 100644 system/camerad/imgproc/utils.cc delete mode 100644 system/camerad/imgproc/utils.h diff --git a/docs/c_docs.rst b/docs/c_docs.rst index 5619cc8a51..0b9d23972e 100644 --- a/docs/c_docs.rst +++ b/docs/c_docs.rst @@ -29,8 +29,6 @@ camerad ^^^^^^^ .. autodoxygenindex:: :project: system_camerad_cameras -.. autodoxygenindex:: - :project: system_camerad_imgproc locationd ^^^^^^^^^ diff --git a/release/files_common b/release/files_common index cfc3830e0d..af5d156d41 100644 --- a/release/files_common +++ b/release/files_common @@ -335,11 +335,6 @@ system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc system/camerad/cameras/sensor2_i2c.h -system/camerad/imgproc/conv.cl -system/camerad/imgproc/pool.cl -system/camerad/imgproc/utils.cc -system/camerad/imgproc/utils.h - selfdrive/manager/__init__.py selfdrive/manager/build.py selfdrive/manager/helpers.py diff --git a/system/camerad/SConscript b/system/camerad/SConscript index ffd7278bbb..83f93344d8 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -3,12 +3,7 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) -env.Program('camerad', [ - 'main.cc', - camera_obj, - ], LIBS=libs) +env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": - env.Program('test/ae_gray_test', - ['test/ae_gray_test.cc', camera_obj], - LIBS=libs) + env.Program('test/ae_gray_test', ['test/ae_gray_test.cc', camera_obj], LIBS=libs) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index d2c7a35f6e..987ccf23da 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -11,7 +11,6 @@ #include "third_party/libyuv/include/libyuv.h" #include -#include "system/camerad/imgproc/utils.h" #include "common/clutil.h" #include "common/swaglog.h" #include "common/util.h" @@ -93,12 +92,8 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, debayer = new Debayer(device_id, context, this, s, nv12_width, nv12_uv_offset); -#ifdef __APPLE__ - q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err)); -#else const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; q = CL_CHECK_ERR(clCreateCommandQueueWithProperties(context, device_id, props, &err)); -#endif } CameraBuf::~CameraBuf() { @@ -281,7 +276,6 @@ float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip } } - // Find mean lumimance value unsigned int lum_cur = 0; for (lum_med = 255; lum_med >= 0; lum_med--) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 0f69aa3774..23bfc8b7d5 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -14,17 +14,9 @@ #include "common/swaglog.h" #include "system/hardware/hw.h" -#define CAMERA_ID_IMX298 0 -#define CAMERA_ID_IMX179 1 -#define CAMERA_ID_S5K3P8SP 2 -#define CAMERA_ID_OV8865 3 -#define CAMERA_ID_IMX298_FLIPPED 4 -#define CAMERA_ID_OV10640 5 -#define CAMERA_ID_LGC920 6 -#define CAMERA_ID_LGC615 7 -#define CAMERA_ID_AR0231 8 -#define CAMERA_ID_OX03C10 9 -#define CAMERA_ID_MAX 10 +#define CAMERA_ID_AR0231 0 +#define CAMERA_ID_OX03C10 1 +#define CAMERA_ID_MAX 2 const int YUV_BUFFER_COUNT = 20; @@ -55,7 +47,7 @@ typedef struct FrameMetadata { uint32_t frame_id; // Timestamps - uint64_t timestamp_sof; // only set on tici + uint64_t timestamp_sof; uint64_t timestamp_eof; // Exposure diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 3a9ecb467b..8d20ed27c8 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -665,51 +665,51 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num if (!enabled) return; struct cam_isp_in_port_info in_port_info = { - .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num], + .res_type = (uint32_t[]){CAM_ISP_IFE_IN_RES_PHY_0, CAM_ISP_IFE_IN_RES_PHY_1, CAM_ISP_IFE_IN_RES_PHY_2}[camera_num], - .lane_type = CAM_ISP_LANE_TYPE_DPHY, - .lane_num = 4, - .lane_cfg = 0x3210, + .lane_type = CAM_ISP_LANE_TYPE_DPHY, + .lane_num = 4, + .lane_cfg = 0x3210, - .vc = 0x0, - .dt = dt, - .format = CAM_FORMAT_MIPI_RAW_12, + .vc = 0x0, + .dt = dt, + .format = CAM_FORMAT_MIPI_RAW_12, - .test_pattern = 0x2, // 0x3? - .usage_type = 0x0, + .test_pattern = 0x2, // 0x3? + .usage_type = 0x0, - .left_start = 0, - .left_stop = ci.frame_width - 1, - .left_width = ci.frame_width, + .left_start = 0, + .left_stop = ci.frame_width - 1, + .left_width = ci.frame_width, - .right_start = 0, - .right_stop = ci.frame_width - 1, - .right_width = ci.frame_width, + .right_start = 0, + .right_stop = ci.frame_width - 1, + .right_width = ci.frame_width, - .line_start = 0, - .line_stop = ci.frame_height + ci.extra_height - 1, - .height = ci.frame_height + ci.extra_height, + .line_start = 0, + .line_stop = ci.frame_height + ci.extra_height - 1, + .height = ci.frame_height + ci.extra_height, - .pixel_clk = 0x0, - .batch_size = 0x0, - .dsp_mode = CAM_ISP_DSP_MODE_NONE, - .hbi_cnt = 0x0, - .custom_csid = 0x0, - - .num_out_res = 0x1, - .data[0] = (struct cam_isp_out_port_info){ - .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, - .format = CAM_FORMAT_MIPI_RAW_12, - .width = ci.frame_width, - .height = ci.frame_height + ci.extra_height, - .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, - }, + .pixel_clk = 0x0, + .batch_size = 0x0, + .dsp_mode = CAM_ISP_DSP_MODE_NONE, + .hbi_cnt = 0x0, + .custom_csid = 0x0, + + .num_out_res = 0x1, + .data[0] = (struct cam_isp_out_port_info){ + .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, + .format = CAM_FORMAT_MIPI_RAW_12, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, + .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, + }, }; struct cam_isp_resource isp_resource = { - .resource_id = CAM_ISP_RES_ID_PORT, - .handle_type = CAM_HANDLE_USER_POINTER, - .res_hdl = (uint64_t)&in_port_info, - .length = sizeof(in_port_info), + .resource_id = CAM_ISP_RES_ID_PORT, + .handle_type = CAM_HANDLE_USER_POINTER, + .res_hdl = (uint64_t)&in_port_info, + .length = sizeof(in_port_info), }; auto isp_dev_handle_ = device_acquire(multi_cam_state->isp_fd, session_handle, &isp_resource); @@ -1098,8 +1098,8 @@ void CameraState::set_camera_exposure(float grey_frac) { std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { - gain_bytes = Params().get("CameraDebugExpGain"); - time_bytes = Params().get("CameraDebugExpTime"); + gain_bytes = params.get("CameraDebugExpGain"); + time_bytes = params.get("CameraDebugExpTime"); } if (gain_bytes.size() > 0 && time_bytes.size() > 0) { diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 9e0109ab20..1e7cbfc104 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -107,6 +107,9 @@ private: // Register parsing std::map> ar0231_register_lut; std::map> ar0231_build_register_lut(uint8_t *data); + + // for debugging + Params params; }; typedef struct MultiCameraState { diff --git a/system/camerad/cameras/camera_util.cc b/system/camerad/cameras/camera_util.cc index 74c81a878a..b2cf4115dc 100644 --- a/system/camerad/cameras/camera_util.cc +++ b/system/camerad/cameras/camera_util.cc @@ -30,10 +30,10 @@ int do_cam_control(int fd, int op_code, void *handle, int size) { std::optional device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources) { struct cam_acquire_dev_cmd cmd = { - .session_handle = session_handle, - .handle_type = CAM_HANDLE_USER_POINTER, - .num_resources = (uint32_t)(data ? num_resources : 0), - .resource_hdl = (uint64_t)data, + .session_handle = session_handle, + .handle_type = CAM_HANDLE_USER_POINTER, + .num_resources = (uint32_t)(data ? num_resources : 0), + .resource_hdl = (uint64_t)data, }; int err = do_cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd)); return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt; @@ -41,9 +41,9 @@ std::optional device_acquire(int fd, int32_t session_handle, void *data int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) { struct cam_config_dev_cmd cmd = { - .session_handle = session_handle, - .dev_handle = dev_handle, - .packet_handle = packet_handle, + .session_handle = session_handle, + .dev_handle = dev_handle, + .packet_handle = packet_handle, }; return do_cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd)); } diff --git a/system/camerad/cameras/camera_util.h b/system/camerad/cameras/camera_util.h index b36f404c0f..891ce3e793 100644 --- a/system/camerad/cameras/camera_util.h +++ b/system/camerad/cameras/camera_util.h @@ -1,6 +1,6 @@ #pragma once - #include +#include #include #include #include diff --git a/system/camerad/imgproc/conv.cl b/system/camerad/imgproc/conv.cl deleted file mode 100644 index a7115ae76c..0000000000 --- a/system/camerad/imgproc/conv.cl +++ /dev/null @@ -1,110 +0,0 @@ -// const __constant float3 rgb_weights = (0.299, 0.587, 0.114); // opencv rgb2gray weights -// const __constant float3 bgr_weights = (0.114, 0.587, 0.299); // bgr2gray weights - -// convert input rgb image to single channel then conv -__kernel void rgb2gray_conv2d( - const __global uchar * input, - __global short * output, - __constant short * filter, - __local uchar3 * cached -) -{ - const int rowOffset = get_global_id(1) * IMAGE_W; - const int my = get_global_id(0) + rowOffset; - - const int localRowLen = TWICE_HALF_FILTER_SIZE + get_local_size(0); - const int localRowOffset = ( get_local_id(1) + HALF_FILTER_SIZE ) * localRowLen; - const int myLocal = localRowOffset + get_local_id(0) + HALF_FILTER_SIZE; - - // cache local pixels - cached[ myLocal ].x = input[ my * 3 ]; // r - cached[ myLocal ].y = input[ my * 3 + 1]; // g - cached[ myLocal ].z = input[ my * 3 + 2]; // b - - // pad - if ( - get_global_id(0) < HALF_FILTER_SIZE || - get_global_id(0) > IMAGE_W - HALF_FILTER_SIZE - 1 || - get_global_id(1) < HALF_FILTER_SIZE || - get_global_id(1) > IMAGE_H - HALF_FILTER_SIZE - 1 - ) - { - barrier(CLK_LOCAL_MEM_FENCE); - return; - } - else - { - int localColOffset = -1; - int globalColOffset = -1; - - // cache extra - if ( get_local_id(0) < HALF_FILTER_SIZE ) - { - localColOffset = get_local_id(0); - globalColOffset = -HALF_FILTER_SIZE; - - cached[ localRowOffset + get_local_id(0) ].x = input[ my * 3 - HALF_FILTER_SIZE * 3 ]; - cached[ localRowOffset + get_local_id(0) ].y = input[ my * 3 - HALF_FILTER_SIZE * 3 + 1]; - cached[ localRowOffset + get_local_id(0) ].z = input[ my * 3 - HALF_FILTER_SIZE * 3 + 2]; - } - else if ( get_local_id(0) >= get_local_size(0) - HALF_FILTER_SIZE ) - { - localColOffset = get_local_id(0) + TWICE_HALF_FILTER_SIZE; - globalColOffset = HALF_FILTER_SIZE; - - cached[ myLocal + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE * 3 ]; - cached[ myLocal + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE * 3 + 1]; - cached[ myLocal + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE * 3 + 2]; - } - - - if ( get_local_id(1) < HALF_FILTER_SIZE ) - { - cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 ]; - cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; - cached[ get_local_id(1) * localRowLen + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; - if (localColOffset > 0) - { - cached[ get_local_id(1) * localRowLen + localColOffset ].x = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; - cached[ get_local_id(1) * localRowLen + localColOffset ].y = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; - cached[ get_local_id(1) * localRowLen + localColOffset ].z = input[ my * 3 - HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; - } - } - else if ( get_local_id(1) >= get_local_size(1) -HALF_FILTER_SIZE ) - { - int offset = ( get_local_id(1) + TWICE_HALF_FILTER_SIZE ) * localRowLen; - cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 ]; - cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 1]; - cached[ offset + get_local_id(0) + HALF_FILTER_SIZE ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + 2]; - if (localColOffset > 0) - { - cached[ offset + localColOffset ].x = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3]; - cached[ offset + localColOffset ].y = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 1]; - cached[ offset + localColOffset ].z = input[ my * 3 + HALF_FILTER_SIZE_IMAGE_W * 3 + globalColOffset * 3 + 2]; - } - } - - // sync - barrier(CLK_LOCAL_MEM_FENCE); - - // perform convolution - int fIndex = 0; - short sum = 0; - - for (int r = -HALF_FILTER_SIZE; r <= HALF_FILTER_SIZE; r++) - { - int curRow = r * localRowLen; - for (int c = -HALF_FILTER_SIZE; c <= HALF_FILTER_SIZE; c++, fIndex++) - { - if (!FLIP_RB){ - // sum += dot(rgb_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; - sum += (cached[ myLocal + curRow + c ].x / 3 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 9) * filter[ fIndex ]; - } else { - // sum += dot(bgr_weights, cached[ myLocal + curRow + c ]) * filter[ fIndex ]; - sum += (cached[ myLocal + curRow + c ].x / 9 + cached[ myLocal + curRow + c ].y / 2 + cached[ myLocal + curRow + c ].z / 3) * filter[ fIndex ]; - } - } - } - output[my] = sum; - } -} \ No newline at end of file diff --git a/system/camerad/imgproc/pool.cl b/system/camerad/imgproc/pool.cl deleted file mode 100644 index d674b5f363..0000000000 --- a/system/camerad/imgproc/pool.cl +++ /dev/null @@ -1,34 +0,0 @@ -// calculate variance in each subregion -__kernel void var_pool( - const __global char * input, - __global ushort * output // should not be larger than 128*128 so uint16 -) -{ - const int xidx = get_global_id(0) + ROI_X_MIN; - const int yidx = get_global_id(1) + ROI_Y_MIN; - - const int size = X_PITCH * Y_PITCH; - - float fsum = 0; - char mean, max; - - for (int i = 0; i < size; i++) { - int x_offset = i % X_PITCH; - int y_offset = i / X_PITCH; - fsum += input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]; - max = input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]>max ? input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X]:max; - } - - mean = convert_char_rte(fsum / size); - - float fvar = 0; - for (int i = 0; i < size; i++) { - int x_offset = i % X_PITCH; - int y_offset = i / X_PITCH; - fvar += (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean) * (input[xidx*X_PITCH + yidx*Y_PITCH*FULL_STRIDE_X + x_offset + y_offset*FULL_STRIDE_X] - mean); - } - - fvar = fvar / size; - - output[(xidx-ROI_X_MIN)+(yidx-ROI_Y_MIN)*(ROI_X_MAX-ROI_X_MIN+1)] = convert_ushort_rte(5 * fvar + convert_float_rte(max)); -} \ No newline at end of file diff --git a/system/camerad/imgproc/utils.cc b/system/camerad/imgproc/utils.cc deleted file mode 100644 index a7bbeb9e86..0000000000 --- a/system/camerad/imgproc/utils.cc +++ /dev/null @@ -1,106 +0,0 @@ -#include "system/camerad/imgproc/utils.h" - -#include -#include -#include -#include -#include - -const int16_t lapl_conv_krnl[9] = {0, 1, 0, - 1, -4, 1, - 0, 1, 0}; - -// calculate score based on laplacians in one area -uint16_t get_lapmap_one(const int16_t *lap, int x_pitch, int y_pitch) { - const int size = x_pitch * y_pitch; - // avg and max of roi - int16_t max = 0; - int sum = 0; - for (int i = 0; i < size; ++i) { - const int16_t v = lap[i]; - sum += v; - if (v > max) max = v; - } - - const int16_t mean = sum / size; - - // var of roi - int var = 0; - for (int i = 0; i < size; ++i) { - var += std::pow(lap[i] - mean, 2); - } - - const float fvar = (float)var / size; - return std::min(5 * fvar + max, (float)65535); -} - -bool is_blur(const uint16_t *lapmap, const size_t size) { - float bad_sum = 0; - for (int i = 0; i < size; i++) { - if (lapmap[i] < LM_THRESH) { - bad_sum += 1 / (float)size; - } - } - return (bad_sum > LM_PREC_THRESH); -} - -static cl_program build_conv_program(cl_device_id device_id, cl_context context, int image_w, int image_h, int filter_size) { - char args[4096]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DIMAGE_W=%d -DIMAGE_H=%d -DFLIP_RB=%d " - "-DFILTER_SIZE=%d -DHALF_FILTER_SIZE=%d -DTWICE_HALF_FILTER_SIZE=%d -DHALF_FILTER_SIZE_IMAGE_W=%d", - image_w, image_h, 1, - filter_size, filter_size/2, (filter_size/2)*2, (filter_size/2)*image_w); - return cl_program_from_file(context, device_id, "imgproc/conv.cl", args); -} - -LapConv::LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size) - : width(rgb_width / NUM_SEGMENTS_X), height(rgb_height / NUM_SEGMENTS_Y), rgb_stride(rgb_stride), - roi_buf(width * height * 3), result_buf(width * height) { - - prg = build_conv_program(device_id, ctx, width, height, filter_size); - krnl = CL_CHECK_ERR(clCreateKernel(prg, "rgb2gray_conv2d", &err)); - // TODO: Removed CL_MEM_SVM_FINE_GRAIN_BUFFER, confirm it doesn't matter - roi_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, roi_buf.size() * sizeof(roi_buf[0]), NULL, &err)); - result_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, result_buf.size() * sizeof(result_buf[0]), NULL, &err)); - filter_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, - 9 * sizeof(int16_t), (void *)&lapl_conv_krnl, &err)); -} - -LapConv::~LapConv() { - CL_CHECK(clReleaseMemObject(roi_cl)); - CL_CHECK(clReleaseMemObject(result_cl)); - CL_CHECK(clReleaseMemObject(filter_cl)); - CL_CHECK(clReleaseKernel(krnl)); - CL_CHECK(clReleaseProgram(prg)); -} - -uint16_t LapConv::Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id) { - // sharpness scores - const int x_offset = ROI_X_MIN + roi_id % (ROI_X_MAX - ROI_X_MIN + 1); - const int y_offset = ROI_Y_MIN + roi_id / (ROI_X_MAX - ROI_X_MIN + 1); - - const uint8_t *rgb_offset = rgb_buf + y_offset * height * rgb_stride + x_offset * width * 3; - for (int i = 0; i < height; ++i) { - memcpy(&roi_buf[i * width * 3], &rgb_offset[i * rgb_stride], width * 3); - } - - constexpr int local_mem_size = (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (CONV_LOCAL_WORKSIZE + 2 * (3 / 2)) * (3 * sizeof(uint8_t)); - const size_t global_work_size[] = {(size_t)width, (size_t)height}; - const size_t local_work_size[] = {CONV_LOCAL_WORKSIZE, CONV_LOCAL_WORKSIZE}; - - CL_CHECK(clEnqueueWriteBuffer(q, roi_cl, CL_TRUE, 0, roi_buf.size() * sizeof(roi_buf[0]), roi_buf.data(), 0, 0, 0)); - CL_CHECK(clSetKernelArg(krnl, 0, sizeof(cl_mem), (void *)&roi_cl)); - CL_CHECK(clSetKernelArg(krnl, 1, sizeof(cl_mem), (void *)&result_cl)); - CL_CHECK(clSetKernelArg(krnl, 2, sizeof(cl_mem), (void *)&filter_cl)); - CL_CHECK(clSetKernelArg(krnl, 3, local_mem_size, 0)); - cl_event conv_event; - CL_CHECK(clEnqueueNDRangeKernel(q, krnl, 2, NULL, global_work_size, local_work_size, 0, 0, &conv_event)); - CL_CHECK(clWaitForEvents(1, &conv_event)); - CL_CHECK(clReleaseEvent(conv_event)); - CL_CHECK(clEnqueueReadBuffer(q, result_cl, CL_TRUE, 0, - result_buf.size() * sizeof(result_buf[0]), result_buf.data(), 0, 0, 0)); - - return get_lapmap_one(result_buf.data(), width, height); -} diff --git a/system/camerad/imgproc/utils.h b/system/camerad/imgproc/utils.h deleted file mode 100644 index 94323b15c5..0000000000 --- a/system/camerad/imgproc/utils.h +++ /dev/null @@ -1,37 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "common/clutil.h" - -#define NUM_SEGMENTS_X 8 -#define NUM_SEGMENTS_Y 6 - -#define ROI_X_MIN 1 -#define ROI_X_MAX 6 -#define ROI_Y_MIN 2 -#define ROI_Y_MAX 3 - -#define LM_THRESH 120 -#define LM_PREC_THRESH 0.9 // 90 perc is blur -#define CONV_LOCAL_WORKSIZE 16 - -class LapConv { -public: - LapConv(cl_device_id device_id, cl_context ctx, int rgb_width, int rgb_height, int rgb_stride, int filter_size); - ~LapConv(); - uint16_t Update(cl_command_queue q, const uint8_t *rgb_buf, const int roi_id); - -private: - cl_mem roi_cl, result_cl, filter_cl; - cl_program prg; - cl_kernel krnl; - const int width, height; - const int rgb_stride; - std::vector roi_buf; - std::vector result_buf; -}; - -bool is_blur(const uint16_t *lapmap, const size_t size); diff --git a/system/camerad/main.cc b/system/camerad/main.cc index 35a3329f30..19de21c9bb 100644 --- a/system/camerad/main.cc +++ b/system/camerad/main.cc @@ -8,7 +8,7 @@ int main(int argc, char *argv[]) { if (Hardware::PC()) { - printf("camerad is not meant to run on PC\n"); + printf("exiting, camerad is not meant to run on PC\n"); return 0; } diff --git a/system/camerad/snapshot/snapshot.py b/system/camerad/snapshot/snapshot.py index ede97d1a79..8c1b6084c7 100755 --- a/system/camerad/snapshot/snapshot.py +++ b/system/camerad/snapshot/snapshot.py @@ -13,7 +13,6 @@ from openpilot.system.hardware import PC from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.selfdrive.manager.process_config import managed_processes -LM_THRESH = 120 # defined in system/camerad/imgproc/utils.h VISION_STREAMS = { "roadCameraState": VisionStreamType.VISION_STREAM_ROAD, From f058b5d64eed31878533a85f25eb53386ff9e330 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Fri, 1 Dec 2023 21:13:37 -0800 Subject: [PATCH 19/87] webrtcd: webrtc streaming server (audio/video/cereal) (#30186) * WebRTCClient and WebRTCServer abstractions * webrtc client implementation * Interactive test scripts * Send localDescriptions as offer/asnwer, as they are different * Tracks need to be added after setting remote description for multi-cam streaming to work * Remove WebRTCStreamingMetadata * Wait for tracks * Move stuff to separate files, rename some things * Refactor everything, create WebRTCStreamBuilder for both offer and answers * ta flight done time to grind * wait for incoming tracks and channels * Dummy track and frame reader track. Fix timing. * dt based on camera type * first trial of the new api * Fix audio track * methods for checking for incoming tracks * Web migration part 2 * Fixes for stream api * use rtc description for web.py * experimental cereal proxy * remove old code from bodyav * fix is_started * serialize session description * fix audio * messaging channel wrapper * fix audiotrack * h264 codec preference * Add codec preference to tracks * override sdp codecs * add logging * Move cli stuff to separate file * slight cleanup * Fix audio track * create codec_mime inside force_codec function * fix incoming media estimation * move builders to __init__ * stream updates following builders * Update example script * web.py support for new builder * web speaker fixes * StreamingMediaInfo API * Move things around * should_add_data_channel rename * is_connected_and_ready * fix linter errors * make cli executable * remove dumb comments * logging support * fix parse_info_from_offer * improve type annotations * satisfy linters * Support for waiting for disconnection * Split device tracks into video/audio files. Move audio speaker to audio.py * default dt for dummy video track * Fix cli * new speaker fixes * Remove almost all functionality from web.py * webrtcd * continue refactoring web.py * after handling joystick reset in controlsd with #30409, controls are not necessary anymore * ping endpoint * Update js files to at least support what worked previously * Fixes after some tests on the body * Streaming fixes * Remove the use of WebRTCStreamBuilder. Subclass use is now required * Add todo * delete all streams on shutdown * Replace lastPing with lastChannelMessageTime * Update ping text only if rtc is still on * That should affect the chart too * Fix paths in web * use protocol in SSLContext * remove warnings since aiortc is not used directly anymore * check if task is done in stop * remove channel handler wrapper, since theres only one channel * Move things around * Moved webrtc abstractions to separate repository * Moved webrtcd to tools/webrtc * Update imports * Add bodyrtc as dependency * Add webrtcd to process_config * Remove usage of DummyVideoStreamTrack * Add main to webrtcd * Move webrtcd to system * Fix imports * Move cereal proxy logic outside of runner * Incoming proxy abstractions * Add some tests * Make it executable * Fix process config * Fix imports * Additional tests. Add tests to pyproject.toml * Update poetry lock * New line * Bump aiortc to 1.6.0 * Added teleoprtc_repo as submodule, and linked its source dir * Add init file to webrtc module * Handle aiortc warnings * Ignore deprecation warnings * Ignore resource warning too * Ignore the warnings * find free port for test_webrtcd * Start process inside the test case * random sleep test * test 2 * Test endpoint function instead * Update comment * Add system/webrtc to release * default arguments for body fields * Add teleoprtc to release * Bump teleoprtc * Exclude teleoprtc from static analysis * Use separate event loop for stream session tests --- .gitmodules | 3 + .pre-commit-config.yaml | 6 +- poetry.lock | 81 +++---- pyproject.toml | 4 + release/files_common | 7 + selfdrive/manager/process_config.py | 1 + system/webrtc/__init__.py | 0 system/webrtc/device/audio.py | 110 +++++++++ system/webrtc/device/video.py | 69 ++++++ system/webrtc/tests/test_stream_session.py | 108 +++++++++ system/webrtc/tests/test_webrtcd.py | 60 +++++ system/webrtc/webrtcd.py | 237 +++++++++++++++++++ teleoprtc | 1 + teleoprtc_repo | 1 + tools/bodyteleop/bodyav.py | 159 ------------- tools/bodyteleop/static/js/jsmain.js | 10 +- tools/bodyteleop/static/js/webrtc.js | 144 +++++------- tools/bodyteleop/web.py | 261 +++++++-------------- 18 files changed, 788 insertions(+), 474 deletions(-) create mode 100644 system/webrtc/__init__.py create mode 100644 system/webrtc/device/audio.py create mode 100644 system/webrtc/device/video.py create mode 100755 system/webrtc/tests/test_stream_session.py create mode 100755 system/webrtc/tests/test_webrtcd.py create mode 100755 system/webrtc/webrtcd.py create mode 120000 teleoprtc create mode 160000 teleoprtc_repo delete mode 100644 tools/bodyteleop/bodyav.py diff --git a/.gitmodules b/.gitmodules index 4ba149cb2d..73f832b1d6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,6 +13,9 @@ [submodule "body"] path = body url = ../../commaai/body.git +[submodule "teleoprtc_repo"] + path = teleoprtc_repo + url = ../../commaai/teleoprtc [submodule "tinygrad"] path = tinygrad_repo url = https://github.com/geohot/tinygrad.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b6a46ad507..2109c18b25 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -26,7 +26,7 @@ repos: rev: v2.2.6 hooks: - id: codespell - exclude: '^(third_party/)|(body/)|(cereal/)|(panda/)|(opendbc/)|(rednose/)|(rednose_repo/)|(selfdrive/ui/translations/.*.ts)|(poetry.lock)' + exclude: '^(third_party/)|(body/)|(cereal/)|(panda/)|(opendbc/)|(rednose/)|(rednose_repo/)|(teleoprtc/)|(teleoprtc_repo/)|(selfdrive/ui/translations/.*.ts)|(poetry.lock)' args: # if you've got a short variable name that's getting flagged, add it here - -L bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints @@ -39,12 +39,12 @@ repos: language: system types: [python] args: ['--explicit-package-bases', '--local-partial-types'] - exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(xx/)' + exclude: '^(third_party/)|(cereal/)|(opendbc/)|(panda/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(teleoprtc/)|(teleoprtc_repo/)|(xx/)' - repo: https://github.com/astral-sh/ruff-pre-commit rev: v0.1.6 hooks: - id: ruff - exclude: '^(third_party/)|(cereal/)|(panda/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)' + exclude: '^(third_party/)|(cereal/)|(panda/)|(rednose/)|(rednose_repo/)|(tinygrad/)|(tinygrad_repo/)|(teleoprtc/)|(teleoprtc_repo/)' - repo: local hooks: - id: cppcheck diff --git a/poetry.lock b/poetry.lock index b05dcae11e..1461d5457c 100644 --- a/poetry.lock +++ b/poetry.lock @@ -112,66 +112,39 @@ ifaddr = ">=0.2.0" [[package]] name = "aiortc" -version = "1.5.0" +version = "1.6.0" description = "An implementation of WebRTC and ORTC" optional = false -python-versions = ">=3.7" +python-versions = ">=3.8" files = [ - {file = "aiortc-1.5.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:d1d3f2d6cc22fae5ea57b0371895b7830e878b9e3705fd3742b3453cdfa0fd51"}, - {file = "aiortc-1.5.0-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:2eaf758b5e0bb16f22a9aeb8ab88eb947345f47e2e46cfca18b2815d44726c4e"}, - {file = "aiortc-1.5.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ee76f6b30d7f39442ba7ac25d58114f077ead1460c5632d0c9e18179d01ad419"}, - {file = "aiortc-1.5.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4a766052d93474e9bf4186465298b7c8fb9af062ef7f83ba33f191baa79dac1e"}, - {file = "aiortc-1.5.0-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:fec292636978ed50728f1ce9b7a9f0d7d2e38bd0b920bb53e091e5728b79e231"}, - {file = "aiortc-1.5.0-cp310-cp310-win32.whl", hash = "sha256:27e879b73377d4b94bd86e4c3e8cd8913905fdca1de90a9a4efb0d9d3779dbf4"}, - {file = "aiortc-1.5.0-cp310-cp310-win_amd64.whl", hash = "sha256:a720d0dd53553f6dfc28a53bee2ffce4f13283b4cbbc7db548000054cc63a4f9"}, - {file = "aiortc-1.5.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:a5e8cbfce84badd9a8355819343570bbec1e4eef725996cad6aebe4cc3d03ae8"}, - {file = "aiortc-1.5.0-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:7931512dbb2ff91fb78f5512ad9ca96546452d7bb627f61bd7393bf59ee48ad3"}, - {file = "aiortc-1.5.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:a6abeb857a98014fc97265891ebf4fd989987d2ee091e0844e3c8fc543b6e2f0"}, - {file = "aiortc-1.5.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:dead42dc3a31570fb6f5b94f9be9c78e28b1dc045f71489858116840f299862e"}, - {file = "aiortc-1.5.0-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:3a1a8081ba6d7cabc5896d10462cb50f6db7a8ccf34e6aa3e6c4a0d2d5bc5db5"}, - {file = "aiortc-1.5.0-cp311-cp311-win32.whl", hash = "sha256:cbd5d35bd34b22b8f711c708d266889c973c0dcb38da14a2a9f757266987a181"}, - {file = "aiortc-1.5.0-cp311-cp311-win_amd64.whl", hash = "sha256:6749145e3d527ac98c80837d72fd832b0c403eded3546aeb7cec6f25592b4d5e"}, - {file = "aiortc-1.5.0-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:50e8e8903cf55f6f2cda9b61c115fca8e444d48f299cdd071980a3b5cec594fa"}, - {file = "aiortc-1.5.0-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:15218a1b81f4fa1521f3b839eefdce638b34c46306e8eaf069cee7283fe8c838"}, - {file = "aiortc-1.5.0-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:25bca7c7bbd3619296b5737a810dd0e2fc7f6264e767fca10e65a709a443bf39"}, - {file = "aiortc-1.5.0-cp37-cp37m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:8f1d88ae0f8b3047a279e4da06f09a35777cfbe0a9177ca8b053865a98a67912"}, - {file = "aiortc-1.5.0-cp37-cp37m-win32.whl", hash = "sha256:f86b68b182537022d4ada49a7723c7a56f39372d6fbc31a29f57315d335cdc29"}, - {file = "aiortc-1.5.0-cp37-cp37m-win_amd64.whl", hash = "sha256:e4bbc2f2b97651f7aa6f5e82c69a22590901962454fc02617c4a559a1b51c21a"}, - {file = "aiortc-1.5.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:7243bce7c3b95e47e56ddf961fbf6015702ddbbf3579b0bbf18c6173b6a6357a"}, - {file = "aiortc-1.5.0-cp38-cp38-macosx_11_0_arm64.whl", hash = "sha256:883db8926deaf01fdcd32fbd74fcf055db63e968324ceff41d5a46ec86dff90c"}, - {file = "aiortc-1.5.0-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:bd663f67344e6fe240c6372f620988db5285c9b1b8336306e9fec76ffb4e5493"}, - {file = "aiortc-1.5.0-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:fe2766f5a7a8e10b445cbf83a510b791a88180c7b1f9adef3f730840fa208afc"}, - {file = "aiortc-1.5.0-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:ba212562025843e8d9faf66e6156b682148f8f9995a19e5c66e8ea802f3fa121"}, - {file = "aiortc-1.5.0-cp38-cp38-win32.whl", hash = "sha256:b7432c9c78e68811ee060ade8b0f867ac42a21677e4d1a9136bb88cd93ab8299"}, - {file = "aiortc-1.5.0-cp38-cp38-win_amd64.whl", hash = "sha256:2ac6e285d4035298f3025b5767dc8f8b0a5a81b2b8744aaa19c75a8fe76f3ad8"}, - {file = "aiortc-1.5.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:aa3c9306d892635dd9c38cc83c6ba67fb608c7da289f422d40f9542e104b7a0f"}, - {file = "aiortc-1.5.0-cp39-cp39-macosx_11_0_arm64.whl", hash = "sha256:55dab49a38a212556adadb85ea06f6041d2a9e537e01092f9160b21b186b5039"}, - {file = "aiortc-1.5.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:db93641b6f31315b8fd4c81e14881aef28fbb0700f220926f82909baedfa9888"}, - {file = "aiortc-1.5.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f63fd1168df72498afe0ee06555cc86b8496115ef128519a01d1ea8e404784b8"}, - {file = "aiortc-1.5.0-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:e436f49617887f2009c6ada872c2da201e3c8010b387e7c1057eab229ae438c3"}, - {file = "aiortc-1.5.0-cp39-cp39-win32.whl", hash = "sha256:6f23495d4e11610117d1bad8686d42d529168d463687a1a1e0bec795d1ec33ce"}, - {file = "aiortc-1.5.0-cp39-cp39-win_amd64.whl", hash = "sha256:76206601082e39fdb56d86221729f04f8bd79d65f9fd6b82121947eabf7efd6d"}, - {file = "aiortc-1.5.0-pp37-pypy37_pp73-macosx_10_9_x86_64.whl", hash = "sha256:d3b2a3b4c120a73242ea0b843ecc3efeaea32861682c771e67f7f08f9d18fddc"}, - {file = "aiortc-1.5.0-pp37-pypy37_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:3d3f6511f2442f49dfaf4e69865b47e0d6d95440fee2f66e6a03a8b4fa1b28e3"}, - {file = "aiortc-1.5.0-pp37-pypy37_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:77ae221c734864c8749c27cc8add22d296ef3e06ae5f6982dbcbe2d0976b10e1"}, - {file = "aiortc-1.5.0-pp37-pypy37_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:7732c825ee96e9bc7fb779a4008be768e7663f7f9bf0ab3cccdd412dd7f1c820"}, - {file = "aiortc-1.5.0-pp37-pypy37_pp73-win_amd64.whl", hash = "sha256:56ffdd67161488c6d934b090a8c2d277bba8806906a3a18493f46b42976569c1"}, - {file = "aiortc-1.5.0-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:f73ba04ca3f331b0ddea0b4ff78424ba30bfd7a49d0b8bd926c75a66ad60f447"}, - {file = "aiortc-1.5.0-pp38-pypy38_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:69eedeec467bd7bcac7ace6ad398133e27f18eeae195a3ad0ffda74255a8b812"}, - {file = "aiortc-1.5.0-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:9e095e5fe22f5a2efd4e0657abec1fea7aca864cb32ae3f0816fbcd340a4f2b7"}, - {file = "aiortc-1.5.0-pp38-pypy38_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:dffd6899a5d3db4356d2c17521021032468931ae168545b1ff4815764a5e2873"}, - {file = "aiortc-1.5.0-pp38-pypy38_pp73-win_amd64.whl", hash = "sha256:af3eed686d621af93befd7e68bd73d6d8a8aa3e721e8fa3ce7e21b3225e37c38"}, - {file = "aiortc-1.5.0-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:15e222a308dcfc44351bd9acff21723c8065cdcd75d6649d53b2986ada64b6be"}, - {file = "aiortc-1.5.0-pp39-pypy39_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:d9ecd61c42e6a78c089805a47542a68eeeec6ba98bf7a2e30cafa3d3f4e94a7f"}, - {file = "aiortc-1.5.0-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:2d839437c6000d77511ff1889133150f23fbc8a7365971260c45ce06ff007b0f"}, - {file = "aiortc-1.5.0-pp39-pypy39_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:025847ad6b8c5686f2895394e1de92c043e20e7d90c266de201eef1b1108c8df"}, - {file = "aiortc-1.5.0-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:85583166ab9c9052d2539bee3ba05f27af7f7b93b15c2259c2fc1bd3de5b31d5"}, - {file = "aiortc-1.5.0.tar.gz", hash = "sha256:82b4131d84f862e24e1c3550b73f78412cc9554140a2575577eb3f04675bbad2"}, + {file = "aiortc-1.6.0-cp38-abi3-macosx_10_9_x86_64.whl", hash = "sha256:4675e04d8441797fef6c8a669b3a67d750670d1b897f08886072a084d743e07d"}, + {file = "aiortc-1.6.0-cp38-abi3-macosx_11_0_arm64.whl", hash = "sha256:836c0686fca67f142c52e5af8043206c2bb702ad0ddcdc94ef19caf1c22f8d54"}, + {file = "aiortc-1.6.0-cp38-abi3-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:33f846abd753935881158751994a51f14e345762130688b19c26cab42c01266f"}, + {file = "aiortc-1.6.0-cp38-abi3-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:eb4bd45397945a5323bd077d43c702a3a991d75023f23649c1d18df5d80c221d"}, + {file = "aiortc-1.6.0-cp38-abi3-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:f93561b515ff478b068bb9b047d8b1e896c747dcf3ee465463047c51a7bea24d"}, + {file = "aiortc-1.6.0-cp38-abi3-win32.whl", hash = "sha256:325f847397af2892aa051dc2880a75e9bd79f535cc05ec8f4538b5ed098b3c5d"}, + {file = "aiortc-1.6.0-cp38-abi3-win_amd64.whl", hash = "sha256:98b118d53ae874126b2e9ec6bb1397ea169b85550c4bd5453e279507ff7f0cf9"}, + {file = "aiortc-1.6.0-pp310-pypy310_pp73-macosx_10_9_x86_64.whl", hash = "sha256:6677018833a5d648754c99c70e6c6f6d4f3942682cda07ed5afa73422f8a009a"}, + {file = "aiortc-1.6.0-pp310-pypy310_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:090936c719225e50cd4d66f476e6c17293a8062cf7687a1baa5080f3c90ec8b1"}, + {file = "aiortc-1.6.0-pp310-pypy310_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:4e60d5bc269d4d12f1f5f47e2c17aa3799f3b5c8b73fd6d8d246ddc11bb29776"}, + {file = "aiortc-1.6.0-pp310-pypy310_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:607b9496b4a3c8cd9d32afb6d5bce07f9170831ec44a20ab8af54d53879aafc8"}, + {file = "aiortc-1.6.0-pp310-pypy310_pp73-win_amd64.whl", hash = "sha256:0a41f4c0b31e45548e7c7397ef1aecc4be49ab68afd8fa134c07581fe0b3a9c2"}, + {file = "aiortc-1.6.0-pp38-pypy38_pp73-macosx_10_9_x86_64.whl", hash = "sha256:64a9939016edbc8f300de6189983c983753827813ac9acd9b5be8ce61cc32684"}, + {file = "aiortc-1.6.0-pp38-pypy38_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:813e7985665c94a0e3387b66e39dba6c751e5e588aedbca06d7e52068c6e37fb"}, + {file = "aiortc-1.6.0-pp38-pypy38_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:672d0b4ad35c4d8f014f44a142aa55529ec82cfe2809226e1275e35a71fd4422"}, + {file = "aiortc-1.6.0-pp38-pypy38_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:1526a1904174bb11958b8f7e93f01f37f80df2190e5089f0501984bdef79595e"}, + {file = "aiortc-1.6.0-pp38-pypy38_pp73-win_amd64.whl", hash = "sha256:cecfa5f462e73218cadc9acd8013cb3a0d9007a4515bceba6e7755d77bb80061"}, + {file = "aiortc-1.6.0-pp39-pypy39_pp73-macosx_10_9_x86_64.whl", hash = "sha256:febca6773de18a6bb9e5569ae87c8be55ed184695f1f9fc99aa4744a7b0375f8"}, + {file = "aiortc-1.6.0-pp39-pypy39_pp73-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:b5ba865be9708713397ec584ed1baeb2f15d2fa9c32594ce19a41ffa6e2517cb"}, + {file = "aiortc-1.6.0-pp39-pypy39_pp73-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:f9759a3c00e46ba1c3499dbf5a8513ae37ba65b940a56b0e7fa5070478e9379f"}, + {file = "aiortc-1.6.0-pp39-pypy39_pp73-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:9cff7868663d9d1c74e237b86e45126022466240439a5f63c3440e3acdf0305b"}, + {file = "aiortc-1.6.0-pp39-pypy39_pp73-win_amd64.whl", hash = "sha256:9f20479d0dd06116ac81d332850fab874d83b561a73fd7252d218f55c6bd5b79"}, + {file = "aiortc-1.6.0.tar.gz", hash = "sha256:08cffbcde3401b33731b1d4169b9ff860b0aaaca200b62e10ce5978238671ad7"}, ] [package.dependencies] aioice = ">=0.9.0,<1.0.0" -av = ">=9.0.0,<11.0.0" +av = ">=9.0.0,<12.0.0" cffi = ">=1.0.0" cryptography = ">=2.2" google-crc32c = ">=1.1" @@ -180,7 +153,7 @@ pylibsrtp = ">=0.5.6" pyopenssl = ">=23.1.0" [package.extras] -dev = ["aiohttp (>=3.7.0)", "coverage (>=5.0)", "numpy (>=1.19.0)"] +dev = ["aiohttp (>=3.7.0)", "coverage[toml] (>=7.2.2)", "numpy (>=1.19.0)"] [[package]] name = "aiosignal" diff --git a/pyproject.toml b/pyproject.toml index 1426905d06..0e55e2ac60 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -24,6 +24,7 @@ testpaths = [ "system/proclogd", "system/tests", "system/ubloxd", + "system/webrtc", "tools/lib/tests", "tools/replay", "tools/cabana" @@ -43,6 +44,8 @@ exclude = [ "rednose_repo/", "tinygrad/", "tinygrad_repo/", + "teleoprtc/", + "teleoprtc_repo/", "third_party/", ] @@ -186,6 +189,7 @@ exclude = [ "opendbc", "rednose_repo", "tinygrad_repo", + "teleoprtc", "third_party", ] flake8-implicit-str-concat.allow-multiline=false diff --git a/release/files_common b/release/files_common index af5d156d41..d92be1c3a6 100644 --- a/release/files_common +++ b/release/files_common @@ -281,6 +281,11 @@ system/sensord/sensors/*.cc system/sensord/sensors/*.h system/sensord/pigeond.py +system/webrtc/__init__.py +system/webrtc/webrtcd.py +system/webrtc/device/audio.py +system/webrtc/device/video.py + selfdrive/thermald/thermald.py selfdrive/thermald/power_monitoring.py selfdrive/thermald/fan_controller.py @@ -439,6 +444,8 @@ third_party/qt5/larch64/bin/** scripts/update_now.sh scripts/stop_updater.sh +teleoprtc/** + rednose_repo/site_scons/site_tools/rednose_filter.py rednose/.gitignore rednose/** diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 6f974b0687..4ad2574188 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -84,6 +84,7 @@ procs = [ # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), + PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), ] diff --git a/system/webrtc/__init__.py b/system/webrtc/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/system/webrtc/device/audio.py b/system/webrtc/device/audio.py new file mode 100644 index 0000000000..3c78be6752 --- /dev/null +++ b/system/webrtc/device/audio.py @@ -0,0 +1,110 @@ +import asyncio +import io +from typing import Optional, List, Tuple + +import aiortc +import av +import numpy as np +import pyaudio + + +class AudioInputStreamTrack(aiortc.mediastreams.AudioStreamTrack): + PYAUDIO_TO_AV_FORMAT_MAP = { + pyaudio.paUInt8: 'u8', + pyaudio.paInt16: 's16', + pyaudio.paInt24: 's24', + pyaudio.paInt32: 's32', + pyaudio.paFloat32: 'flt', + } + + def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 16000, channels: int = 1, packet_time: float = 0.020, device_index: Optional[int] = None): + super().__init__() + + self.p = pyaudio.PyAudio() + chunk_size = int(packet_time * rate) + self.stream = self.p.open(format=audio_format, + channels=channels, + rate=rate, + frames_per_buffer=chunk_size, + input=True, + input_device_index=device_index) + self.format = audio_format + self.rate = rate + self.channels = channels + self.packet_time = packet_time + self.chunk_size = chunk_size + self.pts = 0 + + async def recv(self): + mic_data = self.stream.read(self.chunk_size) + mic_array = np.frombuffer(mic_data, dtype=np.int16) + mic_array = np.expand_dims(mic_array, axis=0) + layout = 'stereo' if self.channels > 1 else 'mono' + frame = av.AudioFrame.from_ndarray(mic_array, format=self.PYAUDIO_TO_AV_FORMAT_MAP[self.format], layout=layout) + frame.rate = self.rate + frame.pts = self.pts + self.pts += frame.samples + + return frame + + +class AudioOutputSpeaker: + def __init__(self, audio_format: int = pyaudio.paInt16, rate: int = 48000, channels: int = 2, packet_time: float = 0.2, device_index: Optional[int] = None): + + chunk_size = int(packet_time * rate) + self.p = pyaudio.PyAudio() + self.buffer = io.BytesIO() + self.channels = channels + self.stream = self.p.open(format=audio_format, + channels=channels, + rate=rate, + frames_per_buffer=chunk_size, + output=True, + output_device_index=device_index, + stream_callback=self.__pyaudio_callback) + self.tracks_and_tasks: List[Tuple[aiortc.MediaStreamTrack, Optional[asyncio.Task]]] = [] + + def __pyaudio_callback(self, in_data, frame_count, time_info, status): + if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2: + buff = b'\x00\x00' * frame_count * self.channels + elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size + self.buffer.seek(0) + buff = self.buffer.read(frame_count * self.channels * 4) + buff = buff[:frame_count * self.channels * 2] + self.buffer.seek(2) + else: + self.buffer.seek(0) + buff = self.buffer.read(frame_count * self.channels * 2) + self.buffer.seek(2) + return (buff, pyaudio.paContinue) + + async def __consume(self, track): + while True: + try: + frame = await track.recv() + except aiortc.MediaStreamError: + return + + self.buffer.write(bytes(frame.planes[0])) + + def hasTrack(self, track: aiortc.MediaStreamTrack) -> bool: + return any(t == track for t, _ in self.tracks_and_tasks) + + def addTrack(self, track: aiortc.MediaStreamTrack): + if not self.hasTrack(track): + self.tracks_and_tasks.append((track, None)) + + def start(self): + for index, (track, task) in enumerate(self.tracks_and_tasks): + if task is None: + self.tracks_and_tasks[index] = (track, asyncio.create_task(self.__consume(track))) + + def stop(self): + for _, task in self.tracks_and_tasks: + if task is not None: + task.cancel() + + self.tracks_and_tasks = [] + self.stream.stop_stream() + self.stream.close() + self.p.terminate() diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py new file mode 100644 index 0000000000..1ecb6dbd74 --- /dev/null +++ b/system/webrtc/device/video.py @@ -0,0 +1,69 @@ +import asyncio +from typing import Optional + +import av +from teleoprtc.tracks import TiciVideoStreamTrack + +from cereal import messaging +from openpilot.tools.lib.framereader import FrameReader +from openpilot.common.realtime import DT_MDL, DT_DMON + + +class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): + camera_to_sock_mapping = { + "driver": "livestreamDriverEncodeData", + "wideRoad": "livestreamWideRoadEncodeData", + "road": "livestreamRoadEncodeData", + } + + def __init__(self, camera_type: str): + dt = DT_DMON if camera_type == "driver" else DT_MDL + super().__init__(camera_type, dt) + + self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) + self._pts = 0 + + async def recv(self): + while True: + msg = messaging.recv_one_or_none(self._sock) + if msg is not None: + break + await asyncio.sleep(0.005) + + evta = getattr(msg, msg.which()) + + packet = av.Packet(evta.header + evta.data) + packet.time_base = self._time_base + packet.pts = self._pts + + self.log_debug("track sending frame %s", self._pts) + self._pts += self._dt * self._clock_rate + + return packet + + def codec_preference(self) -> Optional[str]: + return "H264" + + +class FrameReaderVideoStreamTrack(TiciVideoStreamTrack): + def __init__(self, input_file: str, dt: float = DT_MDL, camera_type: str = "driver"): + super().__init__(camera_type, dt) + + frame_reader = FrameReader(input_file) + self._frames = [frame_reader.get(i, pix_fmt="rgb24") for i in range(frame_reader.frame_count)] + self._frame_count = len(self.frames) + self._frame_index = 0 + self._pts = 0 + + async def recv(self): + self.log_debug("track sending frame %s", self._pts) + img = self._frames[self._frame_index] + + new_frame = av.VideoFrame.from_ndarray(img, format="rgb24") + new_frame.pts = self._pts + new_frame.time_base = self._time_base + + self._frame_index = (self._frame_index + 1) % self._frame_count + self._pts = await self.next_pts(self._pts) + + return new_frame diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py new file mode 100755 index 0000000000..2173c3806b --- /dev/null +++ b/system/webrtc/tests/test_stream_session.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 +import asyncio +import unittest +from unittest.mock import Mock, MagicMock, patch +import json +# for aiortc and its dependencies +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +from aiortc import RTCDataChannel +from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE +import capnp +import pyaudio + +from cereal import messaging, log + +from openpilot.system.webrtc.webrtcd import CerealOutgoingMessageProxy, CerealIncomingMessageProxy +from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack +from openpilot.system.webrtc.device.audio import AudioInputStreamTrack +from openpilot.common.realtime import DT_DMON + + +class TestStreamSession(unittest.TestCase): + def setUp(self): + self.loop = asyncio.new_event_loop() + + def tearDown(self): + self.loop.stop() + self.loop.close() + + def test_outgoing_proxy(self): + test_msg = log.Event.new_message() + test_msg.logMonoTime = 123 + test_msg.valid = True + test_msg.customReservedRawData0 = b"test" + expected_dict = {"type": "customReservedRawData0", "logMonoTime": 123, "valid": True, "data": "test"} + expected_json = json.dumps(expected_dict).encode() + + channel = Mock(spec=RTCDataChannel) + mocked_submaster = messaging.SubMaster(["customReservedRawData0"]) + def mocked_update(t): + mocked_submaster.update_msgs(0, [test_msg]) + + with patch.object(messaging.SubMaster, "update", side_effect=mocked_update): + proxy = CerealOutgoingMessageProxy(mocked_submaster) + proxy.add_channel(channel) + + proxy.update() + + channel.send.assert_called_once_with(expected_json) + + def test_incoming_proxy(self): + tested_msgs = [ + {"type": "customReservedRawData0", "data": "test"}, # primitive + {"type": "can", "data": [{"address": 0, "busTime": 0, "dat": "", "src": 0}]}, # list + {"type": "testJoystick", "data": {"axes": [0, 0], "buttons": [False]}}, # dict + ] + + mocked_pubmaster = MagicMock(spec=messaging.PubMaster) + + proxy = CerealIncomingMessageProxy(mocked_pubmaster) + + for msg in tested_msgs: + proxy.send(json.dumps(msg).encode()) + + mocked_pubmaster.send.assert_called_once() + mt, md = mocked_pubmaster.send.call_args.args + self.assertEqual(mt, msg["type"]) + self.assertIsInstance(md, capnp._DynamicStructBuilder) + self.assertTrue(hasattr(md, msg["type"])) + + mocked_pubmaster.reset_mock() + + def test_livestream_track(self): + fake_msg = messaging.new_message("livestreamDriverEncodeData") + + config = {"receive.return_value": fake_msg.to_bytes()} + with patch("cereal.messaging.SubSocket", spec=True, **config): + track = LiveStreamVideoStreamTrack("driver") + + self.assertTrue(track.id.startswith("driver")) + self.assertEqual(track.codec_preference(), "H264") + + for i in range(5): + packet = self.loop.run_until_complete(track.recv()) + self.assertEqual(packet.time_base, VIDEO_TIME_BASE) + self.assertEqual(packet.pts, int(i * DT_DMON * VIDEO_CLOCK_RATE)) + self.assertEqual(packet.size, 0) + + def test_input_audio_track(self): + packet_time, rate = 0.02, 16000 + sample_count = int(packet_time * rate) + mocked_stream = MagicMock(spec=pyaudio.Stream) + mocked_stream.read.return_value = b"\x00" * 2 * sample_count + + config = {"open.side_effect": lambda *args, **kwargs: mocked_stream} + with patch("pyaudio.PyAudio", spec=True, **config): + track = AudioInputStreamTrack(audio_format=pyaudio.paInt16, packet_time=packet_time, rate=rate) + + for i in range(5): + frame = self.loop.run_until_complete(track.recv()) + self.assertEqual(frame.rate, rate) + self.assertEqual(frame.samples, sample_count) + self.assertEqual(frame.pts, i * sample_count) + + +if __name__ == "__main__": + unittest.main() diff --git a/system/webrtc/tests/test_webrtcd.py b/system/webrtc/tests/test_webrtcd.py new file mode 100755 index 0000000000..b48bf6bc19 --- /dev/null +++ b/system/webrtc/tests/test_webrtcd.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python +import asyncio +import json +import unittest +from unittest.mock import MagicMock, AsyncMock +# for aiortc and its dependencies +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +from openpilot.system.webrtc.webrtcd import get_stream + +import aiortc +from teleoprtc import WebRTCOfferBuilder + + +class TestWebrtcdProc(unittest.IsolatedAsyncioTestCase): + async def assertCompletesWithTimeout(self, awaitable, timeout=1): + try: + async with asyncio.timeout(timeout): + await awaitable + except asyncio.TimeoutError: + self.fail("Timeout while waiting for awaitable to complete") + + async def test_webrtcd(self): + mock_request = MagicMock() + async def connect(offer): + body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': [], 'bridge_services_out': []} + mock_request.json.side_effect = AsyncMock(return_value=body) + response = await get_stream(mock_request) + response_json = json.loads(response.text) + return aiortc.RTCSessionDescription(**response_json) + + builder = WebRTCOfferBuilder(connect) + builder.offer_to_receive_video_stream("road") + builder.offer_to_receive_audio_stream() + builder.add_messaging() + + stream = builder.stream() + + await self.assertCompletesWithTimeout(stream.start()) + await self.assertCompletesWithTimeout(stream.wait_for_connection()) + + self.assertTrue(stream.has_incoming_video_track("road")) + self.assertTrue(stream.has_incoming_audio_track()) + self.assertTrue(stream.has_messaging_channel()) + + video_track, audio_track = stream.get_incoming_video_track("road"), stream.get_incoming_audio_track() + await self.assertCompletesWithTimeout(video_track.recv()) + await self.assertCompletesWithTimeout(audio_track.recv()) + + await self.assertCompletesWithTimeout(stream.stop()) + + # cleanup, very implementation specific, test may break if it changes + self.assertTrue(mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test") + _, session = mock_request.app["streams"].__setitem__.call_args.args + await self.assertCompletesWithTimeout(session.post_run_cleanup()) + + +if __name__ == "__main__": + unittest.main() diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py new file mode 100755 index 0000000000..237cae78a1 --- /dev/null +++ b/system/webrtc/webrtcd.py @@ -0,0 +1,237 @@ +#!/usr/bin/env python3 + +import argparse +import asyncio +import json +import uuid +import logging +from dataclasses import dataclass, field +from typing import Any, List, Optional, Union + +# aiortc and its dependencies have lots of internal warnings :( +import warnings +warnings.filterwarnings("ignore", category=DeprecationWarning) + +import aiortc +from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack +from aiortc.contrib.media import MediaBlackhole +from aiohttp import web +import capnp +from teleoprtc import WebRTCAnswerBuilder +from teleoprtc.info import parse_info_from_offer + +from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack +from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker + +from cereal import messaging + + +class CerealOutgoingMessageProxy: + def __init__(self, sm: messaging.SubMaster): + self.sm = sm + self.channels: List[aiortc.RTCDataChannel] = [] + + def add_channel(self, channel: aiortc.RTCDataChannel): + self.channels.append(channel) + + def to_json(self, msg_content: Any): + if isinstance(msg_content, capnp._DynamicStructReader): + msg_dict = msg_content.to_dict() + elif isinstance(msg_content, capnp._DynamicListReader): + msg_dict = [self.to_json(msg) for msg in msg_content] + elif isinstance(msg_content, bytes): + msg_dict = msg_content.decode() + else: + msg_dict = msg_content + + return msg_dict + + def update(self): + # this is blocking in async context... + self.sm.update(0) + for service, updated in self.sm.updated.items(): + if not updated: + continue + msg_dict = self.to_json(self.sm[service]) + mono_time, valid = self.sm.logMonoTime[service], self.sm.valid[service] + outgoing_msg = {"type": service, "logMonoTime": mono_time, "valid": valid, "data": msg_dict} + encoded_msg = json.dumps(outgoing_msg).encode() + for channel in self.channels: + channel.send(encoded_msg) + + +class CerealIncomingMessageProxy: + def __init__(self, pm: messaging.PubMaster): + self.pm = pm + + def send(self, message: bytes): + msg_json = json.loads(message) + msg_type, msg_data = msg_json["type"], msg_json["data"] + size = None + if not isinstance(msg_data, dict): + size = len(msg_data) + + msg = messaging.new_message(msg_type, size=size) + setattr(msg, msg_type, msg_data) + self.pm.send(msg_type, msg) + + +class CerealProxyRunner: + def __init__(self, proxy: CerealOutgoingMessageProxy): + self.proxy = proxy + self.is_running = False + self.task = None + self.logger = logging.getLogger("webrtcd") + + def start(self): + assert self.task is None + self.task = asyncio.create_task(self.run()) + + def stop(self): + if self.task is None or self.task.done(): + return + self.task.cancel() + self.task = None + + async def run(self): + while True: + try: + self.proxy.update() + except Exception as ex: + self.logger.error("Cereal outgoing proxy failure: %s", ex) + await asyncio.sleep(0.01) + + +class StreamSession: + def __init__(self, sdp: str, cameras: List[str], incoming_services: List[str], outgoing_services: List[str], debug_mode: bool = False): + config = parse_info_from_offer(sdp) + builder = WebRTCAnswerBuilder(sdp) + + assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks" + for cam in cameras: + track = LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack() + builder.add_video_stream(cam, track) + if config.expected_audio_track: + track = AudioInputStreamTrack() if not debug_mode else AudioStreamTrack() + builder.add_audio_stream(track) + if config.incoming_audio_track: + self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole + builder.offer_to_receive_audio_stream() + + self.stream = builder.stream() + self.identifier = str(uuid.uuid4()) + + self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) + self.incoming_bridge = CerealIncomingMessageProxy(messaging.PubMaster(incoming_services)) + self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) + + self.audio_output: Optional[Union[AudioOutputSpeaker, MediaBlackhole]] = None + self.run_task: Optional[asyncio.Task] = None + self.logger = logging.getLogger("webrtcd") + self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s", + self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services) + + def start(self): + self.run_task = asyncio.create_task(self.run()) + + def stop(self): + if self.run_task.done(): + return + self.run_task.cancel() + self.run_task = None + asyncio.run(self.post_run_cleanup()) + + async def get_answer(self): + return await self.stream.start() + + async def message_handler(self, message: bytes): + try: + self.incoming_bridge.send(message) + except Exception as ex: + self.logger.error("Cereal incoming proxy failure: %s", ex) + + async def run(self): + try: + await self.stream.wait_for_connection() + if self.stream.has_messaging_channel(): + self.stream.set_message_handler(self.message_handler) + channel = self.stream.get_messaging_channel() + self.outgoing_bridge_runner.proxy.add_channel(channel) + self.outgoing_bridge_runner.start() + if self.stream.has_incoming_audio_track(): + track = self.stream.get_incoming_audio_track(buffered=False) + self.audio_output = self.audio_output_cls() + self.audio_output.addTrack(track) + self.audio_output.start() + self.logger.info("Stream session (%s) connected", self.identifier) + + await self.stream.wait_for_disconnection() + await self.post_run_cleanup() + + self.logger.info("Stream session (%s) ended", self.identifier) + except Exception as ex: + self.logger.error("Stream session failure: %s", ex) + + async def post_run_cleanup(self): + await self.stream.stop() + self.outgoing_bridge_runner.stop() + if self.audio_output: + self.audio_output.stop() + + +@dataclass +class StreamRequestBody: + sdp: str + cameras: List[str] + bridge_services_in: List[str] = field(default_factory=list) + bridge_services_out: List[str] = field(default_factory=list) + + +async def get_stream(request: web.Request): + stream_dict, debug_mode = request.app['streams'], request.app['debug'] + raw_body = await request.json() + body = StreamRequestBody(**raw_body) + + session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode) + answer = await session.get_answer() + session.start() + + stream_dict[session.identifier] = session + + return web.json_response({"sdp": answer.sdp, "type": answer.type}) + + +async def on_shutdown(app: web.Application): + for session in app['streams'].values(): + session.stop() + del app['streams'] + + +def webrtcd_thread(host: str, port: int, debug: bool): + logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()]) + logging_level = logging.DEBUG if debug else logging.INFO + logging.getLogger("WebRTCStream").setLevel(logging_level) + logging.getLogger("webrtcd").setLevel(logging_level) + + app = web.Application() + + app['streams'] = dict() + app['debug'] = debug + app.on_shutdown.append(on_shutdown) + app.router.add_post("/stream", get_stream) + + web.run_app(app, host=host, port=port) + + +def main(): + parser = argparse.ArgumentParser(description="WebRTC daemon") + parser.add_argument("--host", type=str, default="0.0.0.0", help="Host to listen on") + parser.add_argument("--port", type=int, default=5001, help="Port to listen on") + parser.add_argument("--debug", action="store_true", help="Enable debug mode") + args = parser.parse_args() + + webrtcd_thread(args.host, args.port, args.debug) + + +if __name__=="__main__": + main() diff --git a/teleoprtc b/teleoprtc new file mode 120000 index 0000000000..3d3dbc8dea --- /dev/null +++ b/teleoprtc @@ -0,0 +1 @@ +teleoprtc_repo/teleoprtc \ No newline at end of file diff --git a/teleoprtc_repo b/teleoprtc_repo new file mode 160000 index 0000000000..8ec4778685 --- /dev/null +++ b/teleoprtc_repo @@ -0,0 +1 @@ +Subproject commit 8ec477868591eed9a6136a44f16428bc0468b4e9 diff --git a/tools/bodyteleop/bodyav.py b/tools/bodyteleop/bodyav.py deleted file mode 100644 index 3f11f8d4f2..0000000000 --- a/tools/bodyteleop/bodyav.py +++ /dev/null @@ -1,159 +0,0 @@ -import asyncio -import io -import numpy as np -import pyaudio -import wave - -from aiortc.contrib.media import MediaBlackhole -from aiortc.mediastreams import AudioStreamTrack, MediaStreamError, MediaStreamTrack -from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE -from aiortc.rtcrtpsender import RTCRtpSender -from av import CodecContext, Packet -from pydub import AudioSegment -import cereal.messaging as messaging - -AUDIO_RATE = 16000 -SOUNDS = { - 'engage': '../../selfdrive/assets/sounds/engage.wav', - 'disengage': '../../selfdrive/assets/sounds/disengage.wav', - 'error': '../../selfdrive/assets/sounds/warning_immediate.wav', -} - - -def force_codec(pc, sender, forced_codec='video/VP9', stream_type="video"): - codecs = RTCRtpSender.getCapabilities(stream_type).codecs - codec = [codec for codec in codecs if codec.mimeType == forced_codec] - transceiver = next(t for t in pc.getTransceivers() if t.sender == sender) - transceiver.setCodecPreferences(codec) - - -class EncodedBodyVideo(MediaStreamTrack): - kind = "video" - - _start: float - _timestamp: int - - def __init__(self): - super().__init__() - sock_name = 'livestreamDriverEncodeData' - messaging.context = messaging.Context() - self.sock = messaging.sub_sock(sock_name, None, conflate=True) - self.pts = 0 - - async def recv(self) -> Packet: - while True: - msg = messaging.recv_one_or_none(self.sock) - if msg is not None: - break - await asyncio.sleep(0.005) - - evta = getattr(msg, msg.which()) - self.last_idx = evta.idx.encodeId - - packet = Packet(evta.header + evta.data) - packet.time_base = VIDEO_TIME_BASE - packet.pts = self.pts - self.pts += 0.05 * VIDEO_CLOCK_RATE - return packet - - -class WebClientSpeaker(MediaBlackhole): - def __init__(self): - super().__init__() - self.p = pyaudio.PyAudio() - self.buffer = io.BytesIO() - self.channels = 2 - self.stream = self.p.open(format=pyaudio.paInt16, channels=self.channels, rate=48000, frames_per_buffer=9600, - output=True, stream_callback=self.pyaudio_callback) - - def pyaudio_callback(self, in_data, frame_count, time_info, status): - if self.buffer.getbuffer().nbytes < frame_count * self.channels * 2: - buff = np.zeros((frame_count, 2), dtype=np.int16).tobytes() - elif self.buffer.getbuffer().nbytes > 115200: # 3x the usual read size - self.buffer.seek(0) - buff = self.buffer.read(frame_count * self.channels * 4) - buff = buff[:frame_count * self.channels * 2] - self.buffer.seek(2) - else: - self.buffer.seek(0) - buff = self.buffer.read(frame_count * self.channels * 2) - self.buffer.seek(2) - return (buff, pyaudio.paContinue) - - async def consume(self, track): - while True: - try: - frame = await track.recv() - except MediaStreamError: - return - bio = bytes(frame.planes[0]) - self.buffer.write(bio) - - async def start(self): - for track, task in self._MediaBlackhole__tracks.items(): - if task is None: - self._MediaBlackhole__tracks[track] = asyncio.ensure_future(self.consume(track)) - - async def stop(self): - for task in self._MediaBlackhole__tracks.values(): - if task is not None: - task.cancel() - self._MediaBlackhole__tracks = {} - self.stream.stop_stream() - self.stream.close() - self.p.terminate() - - -class BodyMic(AudioStreamTrack): - def __init__(self): - super().__init__() - - self.sample_rate = AUDIO_RATE - self.AUDIO_PTIME = 0.020 # 20ms audio packetization - self.samples = int(self.AUDIO_PTIME * self.sample_rate) - self.FORMAT = pyaudio.paInt16 - self.CHANNELS = 2 - self.RATE = self.sample_rate - self.CHUNK = int(AUDIO_RATE * 0.020) - self.p = pyaudio.PyAudio() - self.mic_stream = self.p.open(format=self.FORMAT, channels=1, rate=self.RATE, input=True, frames_per_buffer=self.CHUNK) - - self.codec = CodecContext.create('pcm_s16le', 'r') - self.codec.sample_rate = self.RATE - self.codec.channels = 2 - self.audio_samples = 0 - self.chunk_number = 0 - - async def recv(self): - mic_data = self.mic_stream.read(self.CHUNK) - mic_sound = AudioSegment(mic_data, sample_width=2, channels=1, frame_rate=self.RATE) - mic_sound = AudioSegment.from_mono_audiosegments(mic_sound, mic_sound) - mic_sound += 3 # increase volume by 3db - packet = Packet(mic_sound.raw_data) - frame = self.codec.decode(packet)[0] - frame.pts = self.audio_samples - self.audio_samples += frame.samples - self.chunk_number = self.chunk_number + 1 - return frame - - -async def play_sound(sound): - chunk = 5120 - with wave.open(SOUNDS[sound], 'rb') as wf: - def callback(in_data, frame_count, time_info, status): - data = wf.readframes(frame_count) - return data, pyaudio.paContinue - - p = pyaudio.PyAudio() - stream = p.open(format=p.get_format_from_width(wf.getsampwidth()), - channels=wf.getnchannels(), - rate=wf.getframerate(), - output=True, - frames_per_buffer=chunk, - stream_callback=callback) - stream.start_stream() - while stream.is_active(): - await asyncio.sleep(0) - stream.stop_stream() - stream.close() - p.terminate() diff --git a/tools/bodyteleop/static/js/jsmain.js b/tools/bodyteleop/static/js/jsmain.js index f521905724..83205a876b 100644 --- a/tools/bodyteleop/static/js/jsmain.js +++ b/tools/bodyteleop/static/js/jsmain.js @@ -1,5 +1,5 @@ import { handleKeyX, executePlan } from "./controls.js"; -import { start, stop, last_ping } from "./webrtc.js"; +import { start, stop, lastChannelMessageTime, playSoundRequest } from "./webrtc.js"; export var pc = null; export var dc = null; @@ -9,10 +9,14 @@ document.addEventListener('keyup', (e)=>(handleKeyX(e.key.toLowerCase(), 0))); $(".keys").bind("mousedown touchstart", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 1)); $(".keys").bind("mouseup touchend", (e)=>handleKeyX($(e.target).attr('id').replace('key-', ''), 0)); $("#plan-button").click(executePlan); +$(".sound").click((e)=>{ + const sound = $(e.target).attr('id').replace('sound-', '') + return playSoundRequest(sound); +}); setInterval( () => { const dt = new Date().getTime(); - if ((dt - last_ping) > 1000) { + if ((dt - lastChannelMessageTime) > 1000) { $(".pre-blob").removeClass('blob'); $("#battery").text("-"); $("#ping-time").text('-'); @@ -20,4 +24,4 @@ setInterval( () => { } }, 5000); -start(pc, dc); \ No newline at end of file +start(pc, dc); diff --git a/tools/bodyteleop/static/js/webrtc.js b/tools/bodyteleop/static/js/webrtc.js index e2f6583c17..165a2ce6c4 100644 --- a/tools/bodyteleop/static/js/webrtc.js +++ b/tools/bodyteleop/static/js/webrtc.js @@ -1,9 +1,34 @@ import { getXY } from "./controls.js"; import { pingPoints, batteryPoints, chartPing, chartBattery } from "./plots.js"; -export let dcInterval = null; -export let batteryInterval = null; -export let last_ping = null; +export let controlCommandInterval = null; +export let latencyInterval = null; +export let lastChannelMessageTime = null; + + +export function offerRtcRequest(sdp, type) { + return fetch('/offer', { + body: JSON.stringify({sdp: sdp, type: type}), + headers: {'Content-Type': 'application/json'}, + method: 'POST' + }); +} + + +export function playSoundRequest(sound) { + return fetch('/sound', { + body: JSON.stringify({sound}), + headers: {'Content-Type': 'application/json'}, + method: 'POST' + }); +} + + +export function pingHeadRequest() { + return fetch('/', { + method: 'HEAD' + }); +} export function createPeerConnection(pc) { @@ -45,16 +70,7 @@ export function negotiate(pc) { }); }).then(function() { var offer = pc.localDescription; - return fetch('/offer', { - body: JSON.stringify({ - sdp: offer.sdp, - type: offer.type, - }), - headers: { - 'Content-Type': 'application/json' - }, - method: 'POST' - }); + return offerRtcRequest(offer.sdp, offer.type); }).then(function(response) { console.log(response); return response.json(); @@ -86,25 +102,6 @@ export const constraints = { }; -export function createDummyVideoTrack() { - const canvas = document.createElement('canvas'); - const context = canvas.getContext('2d'); - - const frameWidth = 5; // Set the width of the frame - const frameHeight = 5; // Set the height of the frame - canvas.width = frameWidth; - canvas.height = frameHeight; - - context.fillStyle = 'black'; - context.fillRect(0, 0, frameWidth, frameHeight); - - const stream = canvas.captureStream(); - const videoTrack = stream.getVideoTracks()[0]; - - return videoTrack; -} - - export function start(pc, dc) { pc = createPeerConnection(pc); @@ -138,71 +135,56 @@ export function start(pc, dc) { alert('Could not acquire media: ' + err); }); - // add a fake video? - // const dummyVideoTrack = createDummyVideoTrack(); - // const dummyMediaStream = new MediaStream(); - // dummyMediaStream.addTrack(dummyVideoTrack); - // pc.addTrack(dummyVideoTrack, dummyMediaStream); - - // setInterval(() => {pc.getStats(null).then((stats) => {stats.forEach((report) => console.log(report))})}, 10000) - // var video = document.querySelector('video'); - // var print = function (e, f){console.log(e, f); video.requestVideoFrameCallback(print);}; - // video.requestVideoFrameCallback(print); - var parameters = {"ordered": true}; dc = pc.createDataChannel('data', parameters); dc.onclose = function() { - console.log("data channel closed"); - clearInterval(dcInterval); - clearInterval(batteryInterval); + clearInterval(controlCommandInterval); + clearInterval(latencyInterval); }; - function controlCommand() { + + function sendJoystickOverDataChannel() { const {x, y} = getXY(); - const dt = new Date().getTime(); - var message = JSON.stringify({type: 'control_command', x, y, dt}); + var message = JSON.stringify({type: "testJoystick", data: {axes: [x, y], buttons: [false]}}) dc.send(message); } - - function batteryLevel() { - var message = JSON.stringify({type: 'battery_level'}); - dc.send(message); + function checkLatency() { + const initialTime = new Date().getTime(); + pingHeadRequest().then(function() { + const currentTime = new Date().getTime(); + if (Math.abs(currentTime - lastChannelMessageTime) < 1000) { + const pingtime = currentTime - initialTime; + pingPoints.push({'x': currentTime, 'y': pingtime}); + if (pingPoints.length > 1000) { + pingPoints.shift(); + } + chartPing.update(); + $("#ping-time").text((pingtime) + "ms"); + } + }) } - dc.onopen = function() { - dcInterval = setInterval(controlCommand, 50); - batteryInterval = setInterval(batteryLevel, 10000); - controlCommand(); - batteryLevel(); - $(".sound").click((e)=>{ - const sound = $(e.target).attr('id').replace('sound-', '') - dc.send(JSON.stringify({type: 'play_sound', sound})); - }); + controlCommandInterval = setInterval(sendJoystickOverDataChannel, 50); + latencyInterval = setInterval(checkLatency, 1000); + sendJoystickOverDataChannel(); }; - let val_print_idx = 0; + const textDecoder = new TextDecoder(); + var carStaterIndex = 0; dc.onmessage = function(evt) { - const data = JSON.parse(evt.data); - if(val_print_idx == 0 && data.type === 'ping_time') { - const dt = new Date().getTime(); - const pingtime = dt - data.incoming_time; - pingPoints.push({'x': dt, 'y': pingtime}); - if (pingPoints.length > 1000) { - pingPoints.shift(); - } - chartPing.update(); - $("#ping-time").text((pingtime) + "ms"); - last_ping = dt; - $(".pre-blob").addClass('blob'); - } - val_print_idx = (val_print_idx + 1 ) % 20; - if(data.type === 'battery_level') { - $("#battery").text(data.value + "%"); - batteryPoints.push({'x': new Date().getTime(), 'y': data.value}); - if (batteryPoints.length > 1000) { + const text = textDecoder.decode(evt.data); + const msg = JSON.parse(text); + if (carStaterIndex % 100 == 0 && msg.type === 'carState') { + const batteryLevel = Math.round(msg.data.fuelGauge * 100); + $("#battery").text(batteryLevel + "%"); + batteryPoints.push({'x': new Date().getTime(), 'y': batteryLevel}); + if (batteryPoints.length > 1000) { batteryPoints.shift(); } chartBattery.update(); } + carStaterIndex += 1; + lastChannelMessageTime = new Date().getTime(); + $(".pre-blob").addClass('blob'); }; } diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py index 929cfa26fe..2ec6eb037c 100644 --- a/tools/bodyteleop/web.py +++ b/tools/bodyteleop/web.py @@ -1,208 +1,121 @@ import asyncio +import dataclasses import json import logging import os import ssl -import uuid -import time +import subprocess -# aiortc and its dependencies have lots of internal warnings :( -import warnings -warnings.resetwarnings() -warnings.simplefilter("always") +from aiohttp import web, ClientSession +import pyaudio +import wave -from aiohttp import web -from aiortc import RTCPeerConnection, RTCSessionDescription - -import cereal.messaging as messaging from openpilot.common.basedir import BASEDIR -from openpilot.tools.bodyteleop.bodyav import BodyMic, WebClientSpeaker, force_codec, play_sound, MediaBlackhole, EncodedBodyVideo - -from typing import Optional +from openpilot.system.webrtc.webrtcd import StreamRequestBody -logger = logging.getLogger("pc") +logger = logging.getLogger("bodyteleop") logging.basicConfig(level=logging.INFO) -pcs: set[RTCPeerConnection] = set() -pm: Optional[messaging.PubMaster] = None -sm: Optional[messaging.SubMaster] = None TELEOPDIR = f"{BASEDIR}/tools/bodyteleop" +WEBRTCD_HOST, WEBRTCD_PORT = "localhost", 5001 + + +## UTILS +async def play_sound(sound): + SOUNDS = { + 'engage': 'selfdrive/assets/sounds/engage.wav', + 'disengage': 'selfdrive/assets/sounds/disengage.wav', + 'error': 'selfdrive/assets/sounds/warning_immediate.wav', + } + assert sound in SOUNDS + + chunk = 5120 + with wave.open(os.path.join(BASEDIR, SOUNDS[sound]), 'rb') as wf: + def callback(in_data, frame_count, time_info, status): + data = wf.readframes(frame_count) + return data, pyaudio.paContinue + + p = pyaudio.PyAudio() + stream = p.open(format=p.get_format_from_width(wf.getsampwidth()), + channels=wf.getnchannels(), + rate=wf.getframerate(), + output=True, + frames_per_buffer=chunk, + stream_callback=callback) + stream.start_stream() + while stream.is_active(): + await asyncio.sleep(0) + stream.stop_stream() + stream.close() + p.terminate() + +## SSL +def create_ssl_cert(cert_path, key_path): + try: + proc = subprocess.run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \ + -days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"', + stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True) + proc.check_returncode() + except subprocess.CalledProcessError as ex: + raise ValueError(f"Error creating SSL certificate:\n[stdout]\n{proc.stdout.decode()}\n[stderr]\n{proc.stderr.decode()}") from ex + + +def create_ssl_context(): + cert_path = os.path.join(TELEOPDIR, 'cert.pem') + key_path = os.path.join(TELEOPDIR, 'key.pem') + if not os.path.exists(cert_path) or not os.path.exists(key_path): + logger.info("Creating certificate...") + create_ssl_cert(cert_path, key_path) + else: + logger.info("Certificate exists!") + ssl_context = ssl.SSLContext(protocol=ssl.PROTOCOL_TLS_SERVER) + ssl_context.load_cert_chain(cert_path, key_path) + return ssl_context +## ENDPOINTS async def index(request): - content = open(TELEOPDIR + "/static/index.html", "r").read() - now = time.monotonic() - request.app['mutable_vals']['last_send_time'] = now - request.app['mutable_vals']['last_override_time'] = now - request.app['mutable_vals']['prev_command'] = [] - request.app['mutable_vals']['find_person'] = False - - return web.Response(content_type="text/html", text=content) - - -async def control_body(data, app): - now = time.monotonic() - if (data['type'] == 'dummy_controls') and (now < (app['mutable_vals']['last_send_time'] + 0.2)): - return - if (data['type'] == 'control_command') and (app['mutable_vals']['prev_command'] == [data['x'], data['y']] and data['x'] == 0 and data['y'] == 0): - return - - logger.info(str(data)) - x = max(-1.0, min(1.0, data['x'])) - y = max(-1.0, min(1.0, data['y'])) - dat = messaging.new_message('testJoystick') - dat.testJoystick.axes = [x, y] - dat.testJoystick.buttons = [False] - pm.send('testJoystick', dat) - app['mutable_vals']['last_send_time'] = now - if (data['type'] == 'control_command'): - app['mutable_vals']['last_override_time'] = now - app['mutable_vals']['prev_command'] = [data['x'], data['y']] - + with open(os.path.join(TELEOPDIR, "static", "index.html"), "r") as f: + content = f.read() + return web.Response(content_type="text/html", text=content) -async def dummy_controls_msg(app): - while True: - if 'last_send_time' in app['mutable_vals']: - this_time = time.monotonic() - if (app['mutable_vals']['last_send_time'] + 0.2) < this_time: - await control_body({'type': 'dummy_controls', 'x': 0, 'y': 0}, app) - await asyncio.sleep(0.2) +async def ping(request): + return web.Response(text="pong") -async def start_background_tasks(app): - app['bgtask_dummy_controls_msg'] = asyncio.create_task(dummy_controls_msg(app)) +async def sound(request): + params = await request.json() + sound_to_play = params["sound"] -async def stop_background_tasks(app): - app['bgtask_dummy_controls_msg'].cancel() - await app['bgtask_dummy_controls_msg'] + await play_sound(sound_to_play) + return web.json_response({"status": "ok"}) async def offer(request): - logger.info("\n\n\nnewoffer!\n\n") - params = await request.json() - offer = RTCSessionDescription(sdp=params["sdp"], type=params["type"]) - speaker = WebClientSpeaker() - blackhole = MediaBlackhole() - - pc = RTCPeerConnection() - pc_id = "PeerConnection(%s)" % uuid.uuid4() - pcs.add(pc) - - def log_info(msg, *args): - logger.info(pc_id + " " + msg, *args) - - log_info("Created for %s", request.remote) - - @pc.on("datachannel") - def on_datachannel(channel): - request.app['mutable_vals']['remote_channel'] = channel - - @channel.on("message") - async def on_message(message): - data = json.loads(message) - if data['type'] == 'control_command': - await control_body(data, request.app) - times = { - 'type': 'ping_time', - 'incoming_time': data['dt'], - 'outgoing_time': int(time.time() * 1000), - } - channel.send(json.dumps(times)) - if data['type'] == 'battery_level': - sm.update(timeout=0) - if sm.updated['carState']: - channel.send(json.dumps({'type': 'battery_level', 'value': int(sm['carState'].fuelGauge * 100)})) - if data['type'] == 'play_sound': - logger.info(f"Playing sound: {data['sound']}") - await play_sound(data['sound']) - if data['type'] == 'find_person': - request.app['mutable_vals']['find_person'] = data['value'] - - @pc.on("connectionstatechange") - async def on_connectionstatechange(): - log_info("Connection state is %s", pc.connectionState) - if pc.connectionState == "failed": - await pc.close() - pcs.discard(pc) - - @pc.on('track') - def on_track(track): - logger.info(f"Track received: {track.kind}") - if track.kind == "audio": - speaker.addTrack(track) - elif track.kind == "video": - blackhole.addTrack(track) - - @track.on("ended") - async def on_ended(): - log_info("Remote %s track ended", track.kind) - if track.kind == "audio": - await speaker.stop() - elif track.kind == "video": - await blackhole.stop() - - video_sender = pc.addTrack(EncodedBodyVideo()) - force_codec(pc, video_sender, forced_codec='video/H264') - _ = pc.addTrack(BodyMic()) - - await pc.setRemoteDescription(offer) - await speaker.start() - await blackhole.start() - answer = await pc.createAnswer() - await pc.setLocalDescription(answer) - - return web.Response( - content_type="application/json", - text=json.dumps( - {"sdp": pc.localDescription.sdp, "type": pc.localDescription.type} - ), - ) - - -async def on_shutdown(app): - coros = [pc.close() for pc in pcs] - await asyncio.gather(*coros) - pcs.clear() - - -async def run(cmd): - proc = await asyncio.create_subprocess_shell( - cmd, - stdout=asyncio.subprocess.PIPE, - stderr=asyncio.subprocess.PIPE - ) - stdout, stderr = await proc.communicate() - logger.info("Created key and cert!") - if stdout: - logger.info(f'[stdout]\n{stdout.decode()}') - if stderr: - logger.info(f'[stderr]\n{stderr.decode()}') + body = StreamRequestBody(params["sdp"], ["driver"], ["testJoystick"], ["carState"]) + body_json = json.dumps(dataclasses.asdict(body)) + + logger.info("Sending offer to webrtcd...") + webrtcd_url = f"http://{WEBRTCD_HOST}:{WEBRTCD_PORT}/stream" + async with ClientSession() as session, session.post(webrtcd_url, data=body_json) as resp: + assert resp.status == 200 + answer = await resp.json() + return web.json_response(answer) def main(): - global pm, sm - pm = messaging.PubMaster(['testJoystick']) - sm = messaging.SubMaster(['carState', 'logMessage']) # App needs to be HTTPS for microphone and audio autoplay to work on the browser - cert_path = TELEOPDIR + '/cert.pem' - key_path = TELEOPDIR + '/key.pem' - if (not os.path.exists(cert_path)) or (not os.path.exists(key_path)): - asyncio.run(run(f'openssl req -x509 -newkey rsa:4096 -nodes -out {cert_path} -keyout {key_path} \ - -days 365 -subj "/C=US/ST=California/O=commaai/OU=comma body"')) - else: - logger.info("Certificate exists!") - ssl_context = ssl.SSLContext() - ssl_context.load_cert_chain(cert_path, key_path) + ssl_context = create_ssl_context() + app = web.Application() app['mutable_vals'] = {} - app.on_shutdown.append(on_shutdown) - app.router.add_post("/offer", offer) app.router.add_get("/", index) - app.router.add_static('/static', TELEOPDIR + '/static') - app.on_startup.append(start_background_tasks) - app.on_cleanup.append(stop_background_tasks) + app.router.add_get("/ping", ping, allow_head=True) + app.router.add_post("/offer", offer) + app.router.add_post("/sound", sound) + app.router.add_static('/static', os.path.join(TELEOPDIR, 'static')) web.run_app(app, access_log=None, host="0.0.0.0", port=5000, ssl_context=ssl_context) From 54517c0638c99b7a40b146376def1ac324d07074 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 2 Dec 2023 00:27:29 -0600 Subject: [PATCH 20/87] Toyota: LTA cleanup (#30579) * have been running this for a few weeks: good * set setme_x3 closer to stock * add comment to values about additional EPS torque rate safety * rename some variables * should use vEgoRaw here to match panda! * switch * more notes * specify * smaller * for * oof --- selfdrive/car/toyota/carcontroller.py | 21 ++++++++++++++------- selfdrive/car/toyota/interface.py | 2 +- selfdrive/car/toyota/toyotacan.py | 12 +++++++++--- selfdrive/car/toyota/values.py | 3 ++- 4 files changed, 26 insertions(+), 12 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2a02a57d86..0fff0fcf6b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -21,8 +21,8 @@ MAX_USER_TORQUE = 500 # LTA limits # EPS ignores commands above this angle and causes PCS to fault -MAX_STEER_ANGLE = 94.9461 # deg -MAX_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes +MAX_LTA_ANGLE = 94.9461 # deg +MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes class CarController: @@ -71,25 +71,32 @@ class CarController: apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg # Angular rate limit based on speed - apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params) + apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw, self.params) if not lat_active: apply_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg - self.last_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE) + self.last_angle = clip(apply_angle, -MAX_LTA_ANGLE, MAX_LTA_ANGLE) self.last_steer = apply_steer - # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; + # toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req)) + + # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle + # cut steering torque with SETME_X64 when either EPS torque or driver torque is above + # the threshold, to limit max lateral acceleration and for driver torque blending respectively. full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and - abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE) + abs(CS.out.steeringTorque) < MAX_LTA_DRIVER_TORQUE_ALLOWANCE) + + # SETME_X64 at 0 ramps down torque at roughly the max down rate of 1500 units/sec setme_x64 = 100 if lta_active and full_torque_condition else 0 - can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64)) + can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle, + lta_active, self.frame // 2, setme_x64)) # *** gas and brake *** if self.CP.enableGasInterceptor and CC.longActive: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9cf86d69ab..ea28d18302 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA # LTA control can be more delayed and winds up more often - ret.steerActuatorDelay = 0.25 + ret.steerActuatorDelay = 0.18 ret.steerLimitTimer = 0.8 else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index ed0237c1be..9c0b0643d6 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,3 +1,8 @@ +from cereal import car + +SteerControlType = car.CarParams.SteerControlType + + def create_steer_command(packer, steer, steer_req): """Creates a CAN message for the Toyota Steer Command.""" @@ -9,13 +14,14 @@ def create_steer_command(packer, steer, steer_req): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64): +def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, setme_x64): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { "COUNTER": frame + 128, - "SETME_X1": 1, - "SETME_X3": 3, + "SETME_X1": 1, # suspected LTA feature availability + # 1 for TSS 2.5 cars, 3 for TSS 2.0. Send based on whether we're using LTA for lateral control + "SETME_X3": 1 if steer_control_type == SteerControlType.angle else 3, "PERCENTAGE": 100, "SETME_X64": setme_x64, "ANGLE": 0, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 4a1011982c..10702269d6 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -27,7 +27,8 @@ class CarControllerParams: # Assuming a steering ratio of 13.7: # Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph # Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph, - # however the EPS has its own internal limits at all speeds which are less than that + # however the EPS has its own internal limits at all speeds which are less than that: + # Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15]) ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26]) From 8e00ce672d78dcb2445b4bdfa1a9e557bb365233 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 1 Dec 2023 22:42:43 -0800 Subject: [PATCH 21/87] controlsd: remove old process replay sentinel (#30581) --- selfdrive/controls/controlsd.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 19ce407932..96a81dd441 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -179,8 +179,6 @@ class Controls: self.v_cruise_helper = VCruiseHelper(self.CP) self.recalibrating_seen = False - # TODO: no longer necessary, aside from process replay - self.sm['liveParameters'].valid = True self.can_log_mono_time = 0 self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0) From b97e5b0e03a5e22ef0ae6d6f5c2523592ee25540 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 1 Dec 2023 22:48:29 -0800 Subject: [PATCH 22/87] rename carEvents -> onroadEvents (#30577) * rename carEvents -> onroadEvents * bump cereal * bump cereal --- cereal | 2 +- selfdrive/controls/controlsd.py | 12 ++++++------ selfdrive/debug/count_events.py | 4 ++-- selfdrive/test/process_replay/README.md | 2 +- selfdrive/test/process_replay/conftest.py | 2 +- selfdrive/test/process_replay/process_replay.py | 6 +++--- selfdrive/test/test_onroad.py | 6 +++--- selfdrive/test/test_time_to_onroad.py | 4 ++-- tools/sim/tests/test_sim_bridge.py | 4 ++-- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/cereal b/cereal index 2cb2bfb015..65b34b4d15 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 2cb2bfb0154874775e56715ecf81f0e6b00538e9 +Subproject commit 65b34b4d152bcea6d49b821bf1da7189f6086b0f diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 96a81dd441..aca074c95a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -64,7 +64,7 @@ class Controls: # Setup sockets self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState', - 'carControl', 'carEvents', 'carParams']) + 'carControl', 'onroadEvents', 'carParams']) self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] @@ -212,7 +212,7 @@ class Controls: self.state = State.enabled def update_events(self, CS): - """Compute carEvents from carState""" + """Compute onroadEvents from carState""" self.events.clear() @@ -823,11 +823,11 @@ class Controls: cs_send.carState.events = car_events self.pm.send('carState', cs_send) - # carEvents - logged every second or on change + # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): - ce_send = messaging.new_message('carEvents', len(self.events)) - ce_send.carEvents = car_events - self.pm.send('carEvents', ce_send) + ce_send = messaging.new_message('onroadEvents', len(self.events)) + ce_send.onroadEvents = car_events + self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() # carParams - logged every 50 seconds (> 1 per segment) diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py index a148996d94..a2c3576b6f 100755 --- a/selfdrive/debug/count_events.py +++ b/selfdrive/debug/count_events.py @@ -32,8 +32,8 @@ if __name__ == "__main__": end_time = max(end_time, msg.logMonoTime) start_time = min(start_time, msg.logMonoTime) - if msg.which() == 'carEvents': - for e in msg.carEvents: + if msg.which() == 'onroadEvents': + for e in msg.onroadEvents: cnt_events[e.name] += 1 elif msg.which() == 'controlsState': if len(alerts) == 0 or alerts[-1][1] != msg.controlsState.alertType: diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index bec3eb9016..008a901010 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -31,7 +31,7 @@ optional arguments: --blacklist-procs BLACKLIST_PROCS Blacklist given processes from the test (e.g. controlsd) --blacklist-cars BLACKLIST_CARS Blacklist given cars from the test (e.g. HONDA) --ignore-fields IGNORE_FIELDS Extra fields or msgs to ignore (e.g. carState.events) - --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. carEvents) + --ignore-msgs IGNORE_MSGS Msgs to ignore (e.g. onroadEvents) --update-refs Updates reference logs using current commit --upload-only Skips testing processes and uploads logs from previous test run ``` diff --git a/selfdrive/test/process_replay/conftest.py b/selfdrive/test/process_replay/conftest.py index f3794d26ac..3f9744ed61 100644 --- a/selfdrive/test/process_replay/conftest.py +++ b/selfdrive/test/process_replay/conftest.py @@ -16,7 +16,7 @@ def pytest_addoption(parser: pytest.Parser): parser.addoption("--ignore-fields", type=str, nargs="*", default=[], help="Extra fields or msgs to ignore (e.g. carState.events)") parser.addoption("--ignore-msgs", type=str, nargs="*", default=[], - help="Msgs to ignore (e.g. carEvents)") + help="Msgs to ignore (e.g. onroadEvents)") parser.addoption("--update-refs", action="store_true", help="Updates reference logs using current commit") parser.addoption("--upload-only", action="store_true", diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index a26e964550..77c4ab1eab 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -442,8 +442,8 @@ def controlsd_config_callback(params, cfg, lr): controlsState = msg.controlsState if initialized: break - elif msg.which() == "carEvents": - initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents] + elif msg.which() == "onroadEvents": + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.onroadEvents] assert controlsState is not None and initialized, "controlsState never initialized" params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) @@ -465,7 +465,7 @@ CONFIGS = [ "modelV2", "driverCameraState", "roadCameraState", "wideRoadCameraState", "managerState", "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope" ], - subs=["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], + subs=["controlsState", "carState", "carControl", "sendcan", "onroadEvents", "carParams"], ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], config_callback=controlsd_config_callback, init_callback=controlsd_fingerprint_callback, diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 17026dc69f..e89ce5c72b 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -408,7 +408,7 @@ class TestOnroad(unittest.TestCase): def test_startup(self): startup_alert = None for msg in self.lrs[0]: - # can't use carEvents because the first msg can be dropped while loggerd is starting up + # can't use onroadEvents because the first msg can be dropped while loggerd is starting up if msg.which() == "controlsState": startup_alert = msg.controlsState.alertText1 break @@ -417,8 +417,8 @@ class TestOnroad(unittest.TestCase): def test_engagable(self): no_entries = Counter() - for m in self.service_msgs['carEvents']: - for evt in m.carEvents: + for m in self.service_msgs['onroadEvents']: + for evt in m.onroadEvents: if evt.noEntry: no_entries[evt.name] += 1 diff --git a/selfdrive/test/test_time_to_onroad.py b/selfdrive/test/test_time_to_onroad.py index cc2cc6514e..aec49cb13a 100755 --- a/selfdrive/test/test_time_to_onroad.py +++ b/selfdrive/test/test_time_to_onroad.py @@ -18,7 +18,7 @@ def test_time_to_onroad(): proc = subprocess.Popen(["python", manager_path]) start_time = time.monotonic() - sm = messaging.SubMaster(['controlsState', 'deviceState', 'carEvents']) + sm = messaging.SubMaster(['controlsState', 'deviceState', 'onroadEvents']) try: # wait for onroad with Timeout(20, "timed out waiting to go onroad"): @@ -40,7 +40,7 @@ def test_time_to_onroad(): # once we're enageable, must be for the next few seconds for _ in range(500): sm.update(100) - assert sm['controlsState'].engageable, f"events: {sm['carEvents']}" + assert sm['controlsState'].engageable, f"events: {sm['onroadEvents']}" finally: proc.terminate() if proc.wait(60) is None: diff --git a/tools/sim/tests/test_sim_bridge.py b/tools/sim/tests/test_sim_bridge.py index b00527b322..2654526fa8 100644 --- a/tools/sim/tests/test_sim_bridge.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -24,7 +24,7 @@ class TestSimBridgeBase(unittest.TestCase): p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) self.processes.append(p_manager) - sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) + sm = messaging.SubMaster(['controlsState', 'onroadEvents', 'managerState']) q = Queue() carla_bridge = self.create_bridge() p_bridge = carla_bridge.run(q, retries=10) @@ -46,7 +46,7 @@ class TestSimBridgeBase(unittest.TestCase): sm.update() not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] - car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] + car_event_issues = [event.name for event in sm['onroadEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: no_car_events_issues_once = True From 531e62fc033b06b54f11c702dcbde5982bfe41e6 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 1 Dec 2023 23:57:54 -0800 Subject: [PATCH 23/87] clear non-release params on startup (#30583) --- common/params.cc | 2 +- common/params.h | 1 + common/params_pyx.pyx | 1 + selfdrive/controls/controlsd.py | 6 +++--- selfdrive/manager/manager.py | 2 ++ 5 files changed, 8 insertions(+), 4 deletions(-) diff --git a/common/params.cc b/common/params.cc index 54168165a1..d1d3513469 100644 --- a/common/params.cc +++ b/common/params.cc @@ -114,7 +114,7 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"ExperimentalLongitudinalEnabled", PERSISTENT}, + {"ExperimentalLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY}, {"ExperimentalMode", PERSISTENT}, {"ExperimentalModeConfirmed", PERSISTENT}, {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, diff --git a/common/params.h b/common/params.h index fbe0bba6b0..aa586a1581 100644 --- a/common/params.h +++ b/common/params.h @@ -10,6 +10,7 @@ enum ParamKeyType { CLEAR_ON_ONROAD_TRANSITION = 0x08, CLEAR_ON_OFFROAD_TRANSITION = 0x10, DONT_LOG = 0x20, + DEVELOPMENT_ONLY = 0x40, ALL = 0xFFFFFFFF }; diff --git a/common/params_pyx.pyx b/common/params_pyx.pyx index de52c490b3..db8f496d30 100644 --- a/common/params_pyx.pyx +++ b/common/params_pyx.pyx @@ -11,6 +11,7 @@ cdef extern from "common/params.h": CLEAR_ON_MANAGER_START CLEAR_ON_ONROAD_TRANSITION CLEAR_ON_OFFROAD_TRANSITION + DEVELOPMENT_ONLY ALL cdef cppclass c_Params "Params": diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index aca074c95a..421139e449 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -14,7 +14,7 @@ from cereal.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE from openpilot.system.swaglog import cloudlog -from openpilot.system.version import is_release_branch, get_short_branch +from openpilot.system.version import get_short_branch from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from openpilot.selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET @@ -90,7 +90,7 @@ class Controls: get_one_can(self.can_sock) num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) - experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") and not is_release_branch() + experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) else: self.CI, self.CP = CI, CI.CP @@ -133,7 +133,7 @@ class Controls: put_nonblocking("CarParamsPersistent", cp_bytes) # cleanup old params - if not self.CP.experimentalLongitudinalAvailable or is_release_branch(): + if not self.CP.experimentalLongitudinalAvailable: self.params.remove("ExperimentalLongitudinalEnabled") if not self.CP.openpilotLongitudinalControl: self.params.remove("ExperimentalMode") diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index a739437de7..9be448499e 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -37,6 +37,8 @@ def manager_init() -> None: params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START) params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + if is_release_branch(): + params.clear_all(ParamKeyType.DEVELOPMENT_ONLY) default_params: List[Tuple[str, Union[str, bytes]]] = [ ("CompletedTrainingVersion", "0"), From c028688a652a8354ac641e185eee80a04633543b Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 2 Dec 2023 01:16:07 -0800 Subject: [PATCH 24/87] bump cereal (#30582) * bump cereal * fix dmonitoringd * update refs * update refs --- cereal | 2 +- selfdrive/monitoring/dmonitoringd.py | 11 +++-------- selfdrive/test/process_replay/ref_commit | 2 +- 3 files changed, 5 insertions(+), 10 deletions(-) diff --git a/cereal b/cereal index 65b34b4d15..60d1ee0490 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 65b34b4d152bcea6d49b821bf1da7189f6086b0f +Subproject commit 60d1ee0490a33830168b30a1e92f8d4e43cfbb15 diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index ff2ee71e74..6ea35a6c63 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -3,7 +3,6 @@ import gc import cereal.messaging as messaging from cereal import car -from cereal import log from openpilot.common.params import Params, put_bool_nonblocking from openpilot.common.realtime import set_realtime_priority from openpilot.selfdrive.controls.lib.events import Events @@ -19,18 +18,12 @@ def dmonitoringd_thread(): driver_status = DriverStatus(rhd_saved=Params().get_bool("IsRhdDetected")) - sm['liveCalibration'].calStatus = log.LiveCalibrationData.Status.invalid - sm['liveCalibration'].rpyCalib = [0, 0, 0] - sm['carState'].buttonEvents = [] - sm['carState'].standstill = True - v_cruise_last = 0 driver_engaged = False # 10Hz <- dmonitoringmodeld while True: sm.update() - if not sm.updated['driverStateV2']: continue @@ -48,7 +41,9 @@ def dmonitoringd_thread(): # Get data from dmonitoringmodeld events = Events() - driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) + + if sm.all_checks(): + driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled) # Block engaging after max number of distrations if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \ diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 40ac91ee70..2ab22ca9ef 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -dbea36698ba48429b201b138846165eb4c329b92 \ No newline at end of file +a42195085f2b92df02d13d60a8cee80354a84c7a \ No newline at end of file From 8971e2c177b6bd9ae1006e4dcbfa4246d6d8096a Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 2 Dec 2023 10:01:28 -0800 Subject: [PATCH 25/87] longitudinal tests: cleanup old hacks (#30585) --- selfdrive/controls/controlsd.py | 4 +--- .../longitudinal_maneuvers/test_longitudinal.py | 16 +--------------- 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 421139e449..a04349cd67 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -69,10 +69,8 @@ class Controls: self.sensor_packets = ["accelerometer", "gyroscope"] self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"] - can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20 - self.can_sock = messaging.sub_sock('can', timeout=can_timeout) - self.log_sock = messaging.sub_sock('androidLog') + self.can_sock = messaging.sub_sock('can', timeout=20) self.params = Params() ignore = self.sensor_packets + ['testJoystick'] diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index f24f788a19..713b7801f8 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -1,10 +1,8 @@ #!/usr/bin/env python3 import itertools -import os -from parameterized import parameterized_class import unittest +from parameterized import parameterized_class -from openpilot.common.params import Params from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.test.longitudinal_maneuvers.maneuver import Maneuver @@ -150,18 +148,6 @@ class LongitudinalControl(unittest.TestCase): e2e: bool force_decel: bool - @classmethod - def setUpClass(cls): - os.environ['SIMULATION'] = "1" - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['NO_CAN_TIMEOUT'] = "1" - - def setUp(self): - params = Params() - params.clear_all() - params.put_bool("Passive", bool(os.getenv("PASSIVE"))) - params.put_bool("OpenpilotEnabledToggle", True) - def test_maneuver(self): for maneuver in create_maneuvers({"e2e": self.e2e, "force_decel": self.force_decel}): with self.subTest(title=maneuver.title, e2e=maneuver.e2e, force_decel=maneuver.force_decel): From a2b48efa2083c86638b536df180fbb638cab7404 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sat, 2 Dec 2023 10:47:08 -0800 Subject: [PATCH 26/87] process replay: check valid flag (#30588) --- selfdrive/test/process_replay/process_replay.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 77c4ab1eab..c3c3670a74 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -466,7 +466,7 @@ CONFIGS = [ "testJoystick", "liveTorqueParameters", "accelerometer", "gyroscope" ], subs=["controlsState", "carState", "carControl", "sendcan", "onroadEvents", "carParams"], - ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], + ignore=["logMonoTime", "controlsState.startMonoTime", "controlsState.cumLagMs"], config_callback=controlsd_config_callback, init_callback=controlsd_fingerprint_callback, should_recv_callback=controlsd_rcv_callback, @@ -478,7 +478,7 @@ CONFIGS = [ proc_name="radard", pubs=["can", "carState", "modelV2"], subs=["radarState", "liveTracks"], - ignore=["logMonoTime", "valid", "radarState.cumLagMs"], + ignore=["logMonoTime", "radarState.cumLagMs"], init_callback=get_car_params_callback, should_recv_callback=MessageBasedRcvCallback("can"), main_pub="can", @@ -487,7 +487,7 @@ CONFIGS = [ proc_name="plannerd", pubs=["modelV2", "carControl", "carState", "controlsState", "radarState"], subs=["lateralPlan", "longitudinalPlan", "uiPlan"], - ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], + ignore=["logMonoTime", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("modelV2"), tolerance=NUMPY_TOLERANCE, @@ -496,14 +496,14 @@ CONFIGS = [ proc_name="calibrationd", pubs=["carState", "cameraOdometry", "carParams"], subs=["liveCalibration"], - ignore=["logMonoTime", "valid"], + ignore=["logMonoTime"], should_recv_callback=calibration_rcv_callback, ), ProcessConfig( proc_name="dmonitoringd", pubs=["driverStateV2", "liveCalibration", "carState", "modelV2", "controlsState"], subs=["driverMonitoringState"], - ignore=["logMonoTime", "valid"], + ignore=["logMonoTime"], should_recv_callback=FrequencyBasedRcvCallback("driverStateV2"), tolerance=NUMPY_TOLERANCE, ), @@ -514,7 +514,7 @@ CONFIGS = [ "liveCalibration", "carState", "carParams", "gpsLocation" ], subs=["liveLocationKalman"], - ignore=["logMonoTime", "valid"], + ignore=["logMonoTime"], config_callback=locationd_config_pubsub_callback, tolerance=NUMPY_TOLERANCE, ), @@ -522,7 +522,7 @@ CONFIGS = [ proc_name="paramsd", pubs=["liveLocationKalman", "carState"], subs=["liveParameters"], - ignore=["logMonoTime", "valid"], + ignore=["logMonoTime"], init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("liveLocationKalman"), tolerance=NUMPY_TOLERANCE, From a1d36961cf9fa0c6a9c41fe92d6504de5d36cbaa Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 3 Dec 2023 10:50:17 -0800 Subject: [PATCH 27/87] new msgs default to invalid (#30587) * new msgs default to invalid * fix lm * set more valid * update rest * update refs * fix logMessage * more valids * cleanup * fix llk test * pigeond is also valid * more valids --- cereal | 2 +- selfdrive/controls/controlsd.py | 4 +++- selfdrive/controls/radard.py | 1 + selfdrive/locationd/calibrationd.py | 26 +++++++++------------- selfdrive/locationd/test/test_locationd.py | 1 + selfdrive/manager/manager.py | 2 +- selfdrive/modeld/dmonitoringmodeld.py | 2 +- selfdrive/monitoring/dmonitoringd.py | 2 +- selfdrive/navd/navd.py | 4 ++-- selfdrive/test/process_replay/migration.py | 2 ++ selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/thermald/thermald.py | 2 +- system/loggerd/uploader.py | 2 +- system/logmessaged.py | 6 ++--- system/micd.py | 2 +- system/qcomgpsd/qcomgpsd.py | 8 +++---- system/sensord/pigeond.py | 2 +- 17 files changed, 34 insertions(+), 36 deletions(-) diff --git a/cereal b/cereal index 60d1ee0490..033dad2b37 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 60d1ee0490a33830168b30a1e92f8d4e43cfbb15 +Subproject commit 033dad2b370dac411479de19381f6e0253e51e01 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a04349cd67..ed4c912608 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -80,7 +80,7 @@ class Controls: 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'testJoystick'] + self.camera_packets + self.sensor_packets, - ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick']) + ignore_alive=ignore, ignore_avg_freq=['radarState', 'testJoystick'], ignore_valid=['testJoystick', ]) if CI is None: # wait for one pandaState and one CAN packet @@ -824,6 +824,7 @@ class Controls: # onroadEvents - logged every second or on change if (self.sm.frame % int(1. / DT_CTRL) == 0) or (self.events.names != self.events_prev): ce_send = messaging.new_message('onroadEvents', len(self.events)) + ce_send.valid = True ce_send.onroadEvents = car_events self.pm.send('onroadEvents', ce_send) self.events_prev = self.events.names.copy() @@ -831,6 +832,7 @@ class Controls: # carParams - logged every 50 seconds (> 1 per segment) if (self.sm.frame % int(50. / DT_CTRL) == 0): cp_send = messaging.new_message('carParams') + cp_send.valid = True cp_send.carParams = self.CP self.pm.send('carParams', cp_send) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index ec42ae66f2..a2d58cfed2 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -270,6 +270,7 @@ class RadarD: # publish tracks for UI debugging (keep last) tracks_msg = messaging.new_message('liveTracks', len(self.tracks)) + tracks_msg.valid = self.radar_state_valid for index, tid in enumerate(sorted(self.tracks.keys())): tracks_msg.liveTracks[index] = { "trackId": tid, diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 566373d7b8..654c72528b 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -164,7 +164,7 @@ class Calibrator: write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) if self.param_put and write_this_cycle: - put_nonblocking("CalibrationParams", self.get_msg().to_bytes()) + put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes()) def handle_v_ego(self, v_ego: float) -> None: self.v_ego = v_ego @@ -227,12 +227,13 @@ class Calibrator: return new_rpy - def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder: + def get_msg(self, valid: bool) -> capnp.lib.capnp._DynamicStructBuilder: smooth_rpy = self.get_smooth_rpy() msg = messaging.new_message('liveCalibration') - liveCalibration = msg.liveCalibration + msg.valid = valid + liveCalibration = msg.liveCalibration liveCalibration.validBlocks = self.valid_blocks liveCalibration.calStatus = self.cal_status liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) @@ -250,19 +251,16 @@ class Calibrator: return msg - def send_data(self, pm: messaging.PubMaster) -> None: - pm.send('liveCalibration', self.get_msg()) + def send_data(self, pm: messaging.PubMaster, valid: bool) -> None: + pm.send('liveCalibration', self.get_msg(valid)) -def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn: +def main() -> NoReturn: gc.disable() set_realtime_priority(1) - if sm is None: - sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry']) - - if pm is None: - pm = messaging.PubMaster(['liveCalibration']) + pm = messaging.PubMaster(['liveCalibration']) + sm = messaging.SubMaster(['cameraOdometry', 'carState', 'carParams'], poll=['cameraOdometry']) calibrator = Calibrator(param_put=True) @@ -286,11 +284,7 @@ def calibrationd_thread(sm: Optional[messaging.SubMaster] = None, pm: Optional[m # 4Hz driven by cameraOdometry if sm.frame % 5 == 0: - calibrator.send_data(pm) - - -def main(sm: Optional[messaging.SubMaster] = None, pm: Optional[messaging.PubMaster] = None) -> NoReturn: - calibrationd_thread(sm, pm) + calibrator.send_data(pm, sm.all_checks()) if __name__ == "__main__": diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 707fdd743f..78de9216dc 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -60,6 +60,7 @@ class TestLocationdProc(unittest.TestCase): msg.cameraOdometry.trans = [0.0, 0.0, 0.0] msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] msg.logMonoTime = t + msg.valid = True return msg def test_params_gps(self): diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 9be448499e..7b23445c8b 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -171,7 +171,7 @@ def manager_thread() -> None: cloudlog.debug(running) # send managerState - msg = messaging.new_message('managerState') + msg = messaging.new_message('managerState', valid=True) msg.managerState.processes = [p.get_process_state_msg() for p in managed_processes.values()] pm.send('managerState', msg) diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 53c0af0ff3..aafe78abde 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -101,7 +101,7 @@ def fill_driver_state(msg, ds_result: DriverStateResult): def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float): model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents - msg = messaging.new_message('driverStateV2') + msg = messaging.new_message('driverStateV2', valid=True) ds = msg.driverStateV2 ds.frameId = frame_id ds.modelExecutionTime = execution_time diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 6ea35a6c63..7455459967 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -54,7 +54,7 @@ def dmonitoringd_thread(): driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill) # build driverMonitoringState packet - dat = messaging.new_message('driverMonitoringState') + dat = messaging.new_message('driverMonitoringState', valid=sm.all_checks()) dat.driverMonitoringState = { "events": events.to_msg(), "faceDetected": driver_status.face_detected, diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index da2b8c06b9..0d71efcc8d 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -196,7 +196,7 @@ class RouteEngine: self.send_route() def send_instruction(self): - msg = messaging.new_message('navInstruction') + msg = messaging.new_message('navInstruction', valid=True) if self.step_idx is None: msg.valid = False @@ -302,7 +302,7 @@ class RouteEngine: for path in self.route_geometry: coords += [c.as_dict() for c in path] - msg = messaging.new_message('navRoute') + msg = messaging.new_message('navRoute', valid=True) msg.navRoute.coordinates = coords self.pm.send('navRoute', msg) diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index ad5192dbba..831bea7c6f 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -85,6 +85,7 @@ def migrate_peripheralState(lr): continue new_msg = messaging.new_message("peripheralState") + new_msg.valid = msg.valid new_msg.logMonoTime = msg.logMonoTime all_msg.append(new_msg.as_reader()) @@ -149,6 +150,7 @@ def migrate_carParams(lr, old_logtime=False): for msg in lr: if msg.which() == 'carParams': CP = messaging.new_message('carParams') + CP.valid = True CP.carParams = msg.carParams.as_builder() for car_fw in CP.carParams.carFw: car_fw.brand = CP.carParams.carName diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2ab22ca9ef..030cbc4e07 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -a42195085f2b92df02d13d60a8cee80354a84c7a \ No newline at end of file +921222d49db204071f0a7006fc895690e1045b5d \ No newline at end of file diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 3ccb9349e1..690618d0b8 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -81,7 +81,7 @@ def read_tz(x): def read_thermal(thermal_config): - dat = messaging.new_message('deviceState') + dat = messaging.new_message('deviceState', valid=True) dat.deviceState.cpuTempC = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]] dat.deviceState.gpuTempC = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]] dat.deviceState.memoryTempC = read_tz(thermal_config.mem[0]) / thermal_config.mem[1] diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index 12c6ecdca9..b5804b1624 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -228,7 +228,7 @@ class Uploader: return success def get_msg(self): - msg = messaging.new_message("uploaderState") + msg = messaging.new_message("uploaderState", valid=True) us = msg.uploaderState us.immediateQueueSize = int(self.immediate_size / 1e6) us.immediateQueueCount = self.immediate_count diff --git a/system/logmessaged.py b/system/logmessaged.py index c53e20e483..5bd83f4b05 100755 --- a/system/logmessaged.py +++ b/system/logmessaged.py @@ -35,13 +35,11 @@ def main() -> NoReturn: continue # then we publish them - msg = messaging.new_message() - msg.logMessage = record + msg = messaging.new_message(None, valid=True, logMessage=record) log_message_sock.send(msg.to_bytes()) if level >= 40: # logging.ERROR - msg = messaging.new_message() - msg.errorLogMessage = record + msg = messaging.new_message(None, valid=True, errorLogMessage=record) error_log_message_sock.send(msg.to_bytes()) finally: sock.close() diff --git a/system/micd.py b/system/micd.py index 72f3b8b490..0b7a4dadfb 100755 --- a/system/micd.py +++ b/system/micd.py @@ -53,7 +53,7 @@ class Mic: self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) def update(self): - msg = messaging.new_message('microphone') + msg = messaging.new_message('microphone', valid=True) msg.microphone.soundPressure = float(self.sound_pressure) msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted) diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index ac862052d3..78539266c0 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -321,7 +321,7 @@ def main() -> NoReturn: print("%.4f: got log: %x len %d" % (time.time(), log_type, len(log_payload))) if log_type == LOG_GNSS_OEMDRE_MEASUREMENT_REPORT: - msg = messaging.new_message('qcomGnss') + msg = messaging.new_message('qcomGnss', valid=True) gnss = msg.qcomGnss gnss.logTs = log_time @@ -370,7 +370,7 @@ def main() -> NoReturn: vNED = [report["q_FltVelEnuMps[1]"], report["q_FltVelEnuMps[0]"], -report["q_FltVelEnuMps[2]"]] vNEDsigma = [report["q_FltVelSigmaMps[1]"], report["q_FltVelSigmaMps[0]"], -report["q_FltVelSigmaMps[2]"]] - msg = messaging.new_message('gpsLocation') + msg = messaging.new_message('gpsLocation', valid=True) gps = msg.gpsLocation gps.latitude = report["t_DblFinalPosLatLon[0]"] * 180/math.pi gps.longitude = report["t_DblFinalPosLatLon[1]"] * 180/math.pi @@ -396,7 +396,7 @@ def main() -> NoReturn: pm.send('gpsLocation', msg) elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT: - msg = messaging.new_message('qcomGnss') + msg = messaging.new_message('qcomGnss', valid=True) dat = unpack_svpoly(log_payload) dat = relist(dat) gnss = msg.qcomGnss @@ -433,7 +433,7 @@ def main() -> NoReturn: pm.send('qcomGnss', msg) elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: - msg = messaging.new_message('qcomGnss') + msg = messaging.new_message('qcomGnss', valid=True) gnss = msg.qcomGnss gnss.logTs = log_time diff --git a/system/sensord/pigeond.py b/system/sensord/pigeond.py index 2e8f151d17..6a02f627b9 100755 --- a/system/sensord/pigeond.py +++ b/system/sensord/pigeond.py @@ -291,7 +291,7 @@ def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0) continue # send out to socket - msg = messaging.new_message('ubloxRaw', len(dat)) + msg = messaging.new_message('ubloxRaw', len(dat), valid=True) msg.ubloxRaw = dat[:] pm.send('ubloxRaw', msg) else: From 5ccff25d885365e4247665fdf8380c6b67c36b28 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 3 Dec 2023 13:59:59 -0800 Subject: [PATCH 28/87] Simplify passive mode handling (#30593) * simplify passive handling * makes more sense --- launch_chffrplus.sh | 91 +++------------------- launch_env.sh | 4 - launch_openpilot.sh | 87 ++++++++++++++++++++- selfdrive/manager/manager.py | 8 +- selfdrive/manager/test/test_manager.py | 1 - selfdrive/test/helpers.py | 1 - selfdrive/ui/installer/continue_dashcam.sh | 4 +- tools/webcam/README.md | 2 +- 8 files changed, 100 insertions(+), 98 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index a91bd677aa..80a6089238 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -1,88 +1,15 @@ #!/usr/bin/bash -if [ -z "$BASEDIR" ]; then - BASEDIR="/data/openpilot" -fi +# TODO: this can be removed after 0.9.6 release -source "$BASEDIR/launch_env.sh" - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" - -function agnos_init { - # TODO: move this to agnos - sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta - - # set success flag for current boot slot - sudo abctl --set_success - - # Check if AGNOS update is required - if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then - AGNOS_PY="$DIR/system/hardware/tici/agnos.py" - MANIFEST="$DIR/system/hardware/tici/agnos.json" - if $AGNOS_PY --verify $MANIFEST; then - sudo reboot - fi - $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST - fi -} - -function launch { - # Remove orphaned git lock if it exists on boot - [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock - - # Pull time from panda - $DIR/selfdrive/boardd/set_time.py - - # Check to see if there's a valid overlay-based update available. Conditions - # are as follows: - # - # 1. The BASEDIR init file has to exist, with a newer modtime than anything in - # the BASEDIR Git repo. This checks for local development work or the user - # switching branches/forks, which should not be overwritten. - # 2. The FINALIZED consistent file has to exist, indicating there's an update - # that completed successfully and synced to disk. - - if [ -f "${BASEDIR}/.overlay_init" ]; then - find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null - if [ $? -eq 0 ]; then - echo "${BASEDIR} has been modified, skipping overlay update installation" - else - if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then - if [ ! -d /data/safe_staging/old_openpilot ]; then - echo "Valid overlay update found, installing" - LAUNCHER_LOCATION="${BASH_SOURCE[0]}" - - mv $BASEDIR /data/safe_staging/old_openpilot - mv "${STAGING_ROOT}/finalized" $BASEDIR - cd $BASEDIR - - echo "Restarting launch script ${LAUNCHER_LOCATION}" - unset AGNOS_VERSION - exec "${LAUNCHER_LOCATION}" - else - echo "openpilot backup found, not updating" - # TODO: restore backup? This means the updater didn't start after swapping - fi - fi - fi - fi - - # handle pythonpath - ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD" - - # hardware specific init - agnos_init - - # write tmux scrollback to a file - tmux capture-pane -pq -S-1000 > /tmp/launch_log +# migrate continue.sh and relaunch +cat << EOF > /data/continue.sh +#!/usr/bin/bash - # start manager - cd selfdrive/manager - ./build.py && ./manager.py +export PASSIVE=1 - # if broken, keep on screen error - while true; do sleep 1; done -} +cd /data/openpilot +exec ./launch_openpilot.sh +EOF -launch +/data/continue.sh diff --git a/launch_env.sh b/launch_env.sh index d07fbe33de..e3eba30b67 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -10,8 +10,4 @@ if [ -z "$AGNOS_VERSION" ]; then export AGNOS_VERSION="8.2" fi -if [ -z "$PASSIVE" ]; then - export PASSIVE="1" -fi - export STAGING_ROOT="/data/safe_staging" diff --git a/launch_openpilot.sh b/launch_openpilot.sh index 1525e1715f..a91bd677aa 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,5 +1,88 @@ #!/usr/bin/bash -export PASSIVE="0" -exec ./launch_chffrplus.sh +if [ -z "$BASEDIR" ]; then + BASEDIR="/data/openpilot" +fi +source "$BASEDIR/launch_env.sh" + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" + +function agnos_init { + # TODO: move this to agnos + sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta + + # set success flag for current boot slot + sudo abctl --set_success + + # Check if AGNOS update is required + if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then + AGNOS_PY="$DIR/system/hardware/tici/agnos.py" + MANIFEST="$DIR/system/hardware/tici/agnos.json" + if $AGNOS_PY --verify $MANIFEST; then + sudo reboot + fi + $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST + fi +} + +function launch { + # Remove orphaned git lock if it exists on boot + [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock + + # Pull time from panda + $DIR/selfdrive/boardd/set_time.py + + # Check to see if there's a valid overlay-based update available. Conditions + # are as follows: + # + # 1. The BASEDIR init file has to exist, with a newer modtime than anything in + # the BASEDIR Git repo. This checks for local development work or the user + # switching branches/forks, which should not be overwritten. + # 2. The FINALIZED consistent file has to exist, indicating there's an update + # that completed successfully and synced to disk. + + if [ -f "${BASEDIR}/.overlay_init" ]; then + find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null + if [ $? -eq 0 ]; then + echo "${BASEDIR} has been modified, skipping overlay update installation" + else + if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then + if [ ! -d /data/safe_staging/old_openpilot ]; then + echo "Valid overlay update found, installing" + LAUNCHER_LOCATION="${BASH_SOURCE[0]}" + + mv $BASEDIR /data/safe_staging/old_openpilot + mv "${STAGING_ROOT}/finalized" $BASEDIR + cd $BASEDIR + + echo "Restarting launch script ${LAUNCHER_LOCATION}" + unset AGNOS_VERSION + exec "${LAUNCHER_LOCATION}" + else + echo "openpilot backup found, not updating" + # TODO: restore backup? This means the updater didn't start after swapping + fi + fi + fi + fi + + # handle pythonpath + ln -sfn $(pwd) /data/pythonpath + export PYTHONPATH="$PWD" + + # hardware specific init + agnos_init + + # write tmux scrollback to a file + tmux capture-pane -pq -S-1000 > /tmp/launch_log + + # start manager + cd selfdrive/manager + ./build.py && ./manager.py + + # if broken, keep on screen error + while true; do sleep 1; done +} + +launch diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 7b23445c8b..d329e19a0a 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -60,12 +60,8 @@ def manager_init() -> None: if params.get(k) is None: params.put(k, v) - # is this dashcam? - if os.getenv("PASSIVE") is not None: - params.put_bool("Passive", bool(int(os.getenv("PASSIVE", "0")))) - - if params.get("Passive") is None: - raise Exception("Passive must be set to continue") + # is this a dashcam build? + params.put_bool("Passive", bool(int(os.getenv("PASSIVE", "0")))) # Create folders needed for msgq try: diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 0fa186baff..306f39aae0 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -21,7 +21,6 @@ BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] @pytest.mark.tici class TestManager(unittest.TestCase): def setUp(self): - os.environ['PASSIVE'] = '0' HARDWARE.set_power_save(False) # ensure clean CarParams diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 552070f024..2103194cb8 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -11,7 +11,6 @@ from openpilot.system.version import training_version, terms_version def set_params_enabled(): - os.environ['PASSIVE'] = "0" os.environ['REPLAY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" os.environ['LOGPRINT'] = "debug" diff --git a/selfdrive/ui/installer/continue_dashcam.sh b/selfdrive/ui/installer/continue_dashcam.sh index 25233fff11..4447c8302f 100755 --- a/selfdrive/ui/installer/continue_dashcam.sh +++ b/selfdrive/ui/installer/continue_dashcam.sh @@ -1,4 +1,6 @@ #!/usr/bin/bash +export PASSIVE=1 + cd /data/openpilot -exec ./launch_chffrplus.sh +exec ./launch_openpilot.sh diff --git a/tools/webcam/README.md b/tools/webcam/README.md index 237e07cdb6..3491f72820 100644 --- a/tools/webcam/README.md +++ b/tools/webcam/README.md @@ -38,7 +38,7 @@ USE_WEBCAM=1 scons -j$(nproc) ## GO ``` cd ~/openpilot/selfdrive/manager -PASSIVE=0 NOSENSOR=1 USE_WEBCAM=1 ./manager.py +NOSENSOR=1 USE_WEBCAM=1 ./manager.py ``` - Start the car, then the UI should show the road webcam's view - Adjust and secure the webcams (you can run tools/webcam/front_mount_helper.py to help mount the driver camera) From bd0ab957b196694c57b125838622e9d0021ca9c0 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 3 Dec 2023 14:09:37 -0800 Subject: [PATCH 29/87] add carParams.passive (#30594) --- cereal | 2 +- selfdrive/controls/controlsd.py | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/cereal b/cereal index 033dad2b37..4fb8352484 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 033dad2b370dac411479de19381f6e0253e51e01 +Subproject commit 4fb8352484c4ef074bee775975c0d1d1e8f57a78 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ed4c912608..33e02aacdd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -113,8 +113,8 @@ class Controls: car_recognized = self.CP.carName != 'mock' controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly - self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly - if self.read_only: + self.CP.passive = not car_recognized or not controller_available or self.CP.dashcamOnly + if self.CP.passive: safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.noOutput self.CP.safetyConfigs = [safety_config] @@ -189,7 +189,7 @@ class Controls: set_offroad_alert("Offroad_CarUnrecognized", True) else: set_offroad_alert("Offroad_NoFirmware", True) - elif self.read_only: + elif self.CP.passive: self.events.add(EventName.dashcamMode, static=True) elif self.joystick_mode: self.events.add(EventName.joystickDebug, static=True) @@ -225,7 +225,7 @@ class Controls: return # no more events while in dashcam mode - if self.read_only: + if self.CP.passive: return # Block resume if cruise never previously enabled @@ -440,7 +440,7 @@ class Controls: if VisionStreamType.VISION_STREAM_WIDE_ROAD not in available_streams: self.sm.ignore_alive.append('wideRoadCameraState') - if not self.read_only: + if not self.CP.passive: self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) self.initialized = True @@ -745,7 +745,7 @@ class Controls: if current_alert: hudControl.visualAlert = current_alert.visual_alert - if not self.read_only and self.initialized: + if not self.CP.passive and self.initialized: # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) self.last_actuators, can_sends = self.CI.apply(CC, now_nanos) @@ -860,7 +860,7 @@ class Controls: self.update_events(CS) cloudlog.timestamp("Events updated") - if not self.read_only and self.initialized: + if not self.CP.passive and self.initialized: # Update control state self.state_transition(CS) self.prof.checkpoint("State transition") From ce4bac8218ebcc8d0480ed09e0c1c6467f705a65 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 3 Dec 2023 15:54:18 -0800 Subject: [PATCH 30/87] remove unused last_actuators arg from lateral controllers (#30595) --- selfdrive/controls/controlsd.py | 2 +- selfdrive/controls/lib/latcontrol.py | 2 +- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- selfdrive/controls/lib/tests/test_latcontrol.py | 4 +--- 6 files changed, 6 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 33e02aacdd..a0bfbcd837 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -614,7 +614,7 @@ class Controls: lat_plan.curvatures, lat_plan.curvatureRates) actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.last_actuators, self.steer_limited, self.desired_curvature, + self.steer_limited, self.desired_curvature, self.desired_curvature_rate, self.sm['liveLocationKalman']) actuators.curvature = self.desired_curvature else: diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 30e1918442..723af7f806 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -17,7 +17,7 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): pass def reset(self): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index b13d41e51c..d363295f0c 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -11,7 +11,7 @@ class LatControlAngle(LatControl): 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): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c41130af95..c673159ebb 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,7 +17,7 @@ class LatControlPID(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 2c77630632..f46ab9eb6c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -36,7 +36,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): + def update(self, active, CS, VM, params, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py index 9580e604ff..5faad914cf 100755 --- a/selfdrive/controls/lib/tests/test_latcontrol.py +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -30,13 +30,11 @@ class TestLatControl(unittest.TestCase): CS.vEgo = 30 CS.steeringPressed = False - last_actuators = car.CarControl.Actuators.new_message() - params = log.LiveParametersData.new_message() llk = gen_llk() for _ in range(1000): - _, _, lac_log = controller.update(True, CS, VM, params, last_actuators, False, 1, 0, llk) + _, _, lac_log = controller.update(True, CS, VM, params, False, 1, 0, llk) self.assertTrue(lac_log.saturated) From 05e932b088cde39dce7a1a32a55e5b4a3fae1253 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 4 Dec 2023 13:58:43 -0800 Subject: [PATCH 31/87] Fix passive mode (#30600) remove passive --- launch_chffrplus.sh | 2 -- 1 file changed, 2 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 80a6089238..95d0fc639d 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -6,8 +6,6 @@ cat << EOF > /data/continue.sh #!/usr/bin/bash -export PASSIVE=1 - cd /data/openpilot exec ./launch_openpilot.sh EOF From 99d51bf02c5a13f3064b012da55634f8a319091e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 4 Dec 2023 15:05:05 -0800 Subject: [PATCH 32/87] Revert "Simplify passive mode handling (#30593)" --- launch_chffrplus.sh | 91 ++++++++++++++++++++-- launch_env.sh | 4 + launch_openpilot.sh | 87 +-------------------- selfdrive/manager/manager.py | 8 +- selfdrive/manager/test/test_manager.py | 1 + selfdrive/test/helpers.py | 1 + selfdrive/ui/installer/continue_dashcam.sh | 4 +- tools/webcam/README.md | 2 +- 8 files changed, 99 insertions(+), 99 deletions(-) diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 95d0fc639d..a91bd677aa 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -1,13 +1,88 @@ #!/usr/bin/bash -# TODO: this can be removed after 0.9.6 release +if [ -z "$BASEDIR" ]; then + BASEDIR="/data/openpilot" +fi -# migrate continue.sh and relaunch -cat << EOF > /data/continue.sh -#!/usr/bin/bash +source "$BASEDIR/launch_env.sh" + +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" + +function agnos_init { + # TODO: move this to agnos + sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta + + # set success flag for current boot slot + sudo abctl --set_success + + # Check if AGNOS update is required + if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then + AGNOS_PY="$DIR/system/hardware/tici/agnos.py" + MANIFEST="$DIR/system/hardware/tici/agnos.json" + if $AGNOS_PY --verify $MANIFEST; then + sudo reboot + fi + $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST + fi +} + +function launch { + # Remove orphaned git lock if it exists on boot + [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock + + # Pull time from panda + $DIR/selfdrive/boardd/set_time.py + + # Check to see if there's a valid overlay-based update available. Conditions + # are as follows: + # + # 1. The BASEDIR init file has to exist, with a newer modtime than anything in + # the BASEDIR Git repo. This checks for local development work or the user + # switching branches/forks, which should not be overwritten. + # 2. The FINALIZED consistent file has to exist, indicating there's an update + # that completed successfully and synced to disk. + + if [ -f "${BASEDIR}/.overlay_init" ]; then + find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null + if [ $? -eq 0 ]; then + echo "${BASEDIR} has been modified, skipping overlay update installation" + else + if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then + if [ ! -d /data/safe_staging/old_openpilot ]; then + echo "Valid overlay update found, installing" + LAUNCHER_LOCATION="${BASH_SOURCE[0]}" + + mv $BASEDIR /data/safe_staging/old_openpilot + mv "${STAGING_ROOT}/finalized" $BASEDIR + cd $BASEDIR + + echo "Restarting launch script ${LAUNCHER_LOCATION}" + unset AGNOS_VERSION + exec "${LAUNCHER_LOCATION}" + else + echo "openpilot backup found, not updating" + # TODO: restore backup? This means the updater didn't start after swapping + fi + fi + fi + fi + + # handle pythonpath + ln -sfn $(pwd) /data/pythonpath + export PYTHONPATH="$PWD" + + # hardware specific init + agnos_init + + # write tmux scrollback to a file + tmux capture-pane -pq -S-1000 > /tmp/launch_log + + # start manager + cd selfdrive/manager + ./build.py && ./manager.py -cd /data/openpilot -exec ./launch_openpilot.sh -EOF + # if broken, keep on screen error + while true; do sleep 1; done +} -/data/continue.sh +launch diff --git a/launch_env.sh b/launch_env.sh index e3eba30b67..d07fbe33de 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -10,4 +10,8 @@ if [ -z "$AGNOS_VERSION" ]; then export AGNOS_VERSION="8.2" fi +if [ -z "$PASSIVE" ]; then + export PASSIVE="1" +fi + export STAGING_ROOT="/data/safe_staging" diff --git a/launch_openpilot.sh b/launch_openpilot.sh index a91bd677aa..1525e1715f 100755 --- a/launch_openpilot.sh +++ b/launch_openpilot.sh @@ -1,88 +1,5 @@ #!/usr/bin/bash -if [ -z "$BASEDIR" ]; then - BASEDIR="/data/openpilot" -fi +export PASSIVE="0" +exec ./launch_chffrplus.sh -source "$BASEDIR/launch_env.sh" - -DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" - -function agnos_init { - # TODO: move this to agnos - sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta - - # set success flag for current boot slot - sudo abctl --set_success - - # Check if AGNOS update is required - if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then - AGNOS_PY="$DIR/system/hardware/tici/agnos.py" - MANIFEST="$DIR/system/hardware/tici/agnos.json" - if $AGNOS_PY --verify $MANIFEST; then - sudo reboot - fi - $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST - fi -} - -function launch { - # Remove orphaned git lock if it exists on boot - [ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock - - # Pull time from panda - $DIR/selfdrive/boardd/set_time.py - - # Check to see if there's a valid overlay-based update available. Conditions - # are as follows: - # - # 1. The BASEDIR init file has to exist, with a newer modtime than anything in - # the BASEDIR Git repo. This checks for local development work or the user - # switching branches/forks, which should not be overwritten. - # 2. The FINALIZED consistent file has to exist, indicating there's an update - # that completed successfully and synced to disk. - - if [ -f "${BASEDIR}/.overlay_init" ]; then - find ${BASEDIR}/.git -newer ${BASEDIR}/.overlay_init | grep -q '.' 2> /dev/null - if [ $? -eq 0 ]; then - echo "${BASEDIR} has been modified, skipping overlay update installation" - else - if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then - if [ ! -d /data/safe_staging/old_openpilot ]; then - echo "Valid overlay update found, installing" - LAUNCHER_LOCATION="${BASH_SOURCE[0]}" - - mv $BASEDIR /data/safe_staging/old_openpilot - mv "${STAGING_ROOT}/finalized" $BASEDIR - cd $BASEDIR - - echo "Restarting launch script ${LAUNCHER_LOCATION}" - unset AGNOS_VERSION - exec "${LAUNCHER_LOCATION}" - else - echo "openpilot backup found, not updating" - # TODO: restore backup? This means the updater didn't start after swapping - fi - fi - fi - fi - - # handle pythonpath - ln -sfn $(pwd) /data/pythonpath - export PYTHONPATH="$PWD" - - # hardware specific init - agnos_init - - # write tmux scrollback to a file - tmux capture-pane -pq -S-1000 > /tmp/launch_log - - # start manager - cd selfdrive/manager - ./build.py && ./manager.py - - # if broken, keep on screen error - while true; do sleep 1; done -} - -launch diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index d329e19a0a..7b23445c8b 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -60,8 +60,12 @@ def manager_init() -> None: if params.get(k) is None: params.put(k, v) - # is this a dashcam build? - params.put_bool("Passive", bool(int(os.getenv("PASSIVE", "0")))) + # is this dashcam? + if os.getenv("PASSIVE") is not None: + params.put_bool("Passive", bool(int(os.getenv("PASSIVE", "0")))) + + if params.get("Passive") is None: + raise Exception("Passive must be set to continue") # Create folders needed for msgq try: diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 306f39aae0..0fa186baff 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -21,6 +21,7 @@ BLACKLIST_PROCS = ['manage_athenad', 'pandad', 'pigeond'] @pytest.mark.tici class TestManager(unittest.TestCase): def setUp(self): + os.environ['PASSIVE'] = '0' HARDWARE.set_power_save(False) # ensure clean CarParams diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 2103194cb8..552070f024 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -11,6 +11,7 @@ from openpilot.system.version import training_version, terms_version def set_params_enabled(): + os.environ['PASSIVE'] = "0" os.environ['REPLAY'] = "1" os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019" os.environ['LOGPRINT'] = "debug" diff --git a/selfdrive/ui/installer/continue_dashcam.sh b/selfdrive/ui/installer/continue_dashcam.sh index 4447c8302f..25233fff11 100755 --- a/selfdrive/ui/installer/continue_dashcam.sh +++ b/selfdrive/ui/installer/continue_dashcam.sh @@ -1,6 +1,4 @@ #!/usr/bin/bash -export PASSIVE=1 - cd /data/openpilot -exec ./launch_openpilot.sh +exec ./launch_chffrplus.sh diff --git a/tools/webcam/README.md b/tools/webcam/README.md index 3491f72820..237e07cdb6 100644 --- a/tools/webcam/README.md +++ b/tools/webcam/README.md @@ -38,7 +38,7 @@ USE_WEBCAM=1 scons -j$(nproc) ## GO ``` cd ~/openpilot/selfdrive/manager -NOSENSOR=1 USE_WEBCAM=1 ./manager.py +PASSIVE=0 NOSENSOR=1 USE_WEBCAM=1 ./manager.py ``` - Start the car, then the UI should show the road webcam's view - Adjust and secure the webcams (you can run tools/webcam/front_mount_helper.py to help mount the driver camera) From 36ccbc8bbef23cf464c38f29ef748a906561ba12 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 4 Dec 2023 17:02:48 -0800 Subject: [PATCH 33/87] Bump submodules (#30596) bump submodules Co-authored-by: jnewb1 --- opendbc | 2 +- panda | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc b/opendbc index 2b96bcc456..ed3af3da1b 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 2b96bcc45669cdd14f9c652b07ef32d6403630f6 +Subproject commit ed3af3da1b7d52aed910f22cc5540e31f92ba254 diff --git a/panda b/panda index b50455cc76..892ca5a0f1 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit b50455cc76a0f66aa6faf953dd3ea023783a4b3f +Subproject commit 892ca5a0f19c07a4786373e360027ab6ec57f723 From 33ee7530b39bf01051c11cb888695b0962c92f95 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Tue, 5 Dec 2023 09:12:22 +0800 Subject: [PATCH 34/87] cabana: add test case for parsing all opendbc files (#30584) * test opendbc files * bump opendbc * bump opendbc --------- Co-authored-by: Shane Smiskol --- tools/cabana/tests/test_cabana.cc | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/tools/cabana/tests/test_cabana.cc b/tools/cabana/tests/test_cabana.cc index f6884a2dc9..c7dc91e7aa 100644 --- a/tools/cabana/tests/test_cabana.cc +++ b/tools/cabana/tests/test_cabana.cc @@ -1,5 +1,7 @@ #undef INFO +#include + #include "catch2/catch.hpp" #include "tools/replay/logreader.h" #include "tools/cabana/dbc/dbcmanager.h" @@ -85,3 +87,17 @@ CM_ SG_ 160 signal_2 "multiple line comment REQUIRE(msg->sigs[1]->size == 1); REQUIRE(msg->sigs[1]->receiver_name == "XXX"); } + +TEST_CASE("parse_opendbc") { + QDir dir(OPENDBC_FILE_PATH); + QStringList errors; + for (auto fn : dir.entryList({"*.dbc"}, QDir::Files, QDir::Name)) { + try { + auto dbc = DBCFile(dir.filePath(fn)); + } catch (std::exception &e) { + errors.push_back(e.what()); + } + } + INFO(errors.join("\n").toStdString()); + REQUIRE(errors.empty()); +} From 6a354ddab75fee435da85f6dc89df90ef2d3baec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 4 Dec 2023 19:14:11 -0800 Subject: [PATCH 35/87] webrtcd: stop cereal proxy runner when data channel is closed (#30601) Stop the proxy when channel reaches invalid state --- system/webrtc/webrtcd.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index 237cae78a1..3f2ef2ceb4 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -15,6 +15,7 @@ warnings.filterwarnings("ignore", category=DeprecationWarning) import aiortc from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack from aiortc.contrib.media import MediaBlackhole +from aiortc.exceptions import InvalidStateError from aiohttp import web import capnp from teleoprtc import WebRTCAnswerBuilder @@ -97,6 +98,9 @@ class CerealProxyRunner: while True: try: self.proxy.update() + except InvalidStateError: + self.logger.warning("Cereal outgoing proxy invalid state (connection closed)") + break except Exception as ex: self.logger.error("Cereal outgoing proxy failure: %s", ex) await asyncio.sleep(0.01) From 10eb70daf707760f785490f69b2d17621698ed75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 4 Dec 2023 20:43:19 -0800 Subject: [PATCH 36/87] webrtcd: endpoint for message schema retrieval (#30578) * Capnp json schema conversion * Schema get endpoint * Type annotation for generate_field * Filter empty services --- system/webrtc/schema.py | 43 ++++++++++++++++++++++++++++++++++++++++ system/webrtc/webrtcd.py | 12 ++++++++++- 2 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 system/webrtc/schema.py diff --git a/system/webrtc/schema.py b/system/webrtc/schema.py new file mode 100644 index 0000000000..f659b34293 --- /dev/null +++ b/system/webrtc/schema.py @@ -0,0 +1,43 @@ +import capnp +from typing import Union, List, Dict, Any + + +def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]: + data_type = next(type_walker) + if data_type.which() == 'struct': + return generate_struct(next(schema_walker)) + elif data_type.which() == 'list': + _ = next(schema_walker) + return [generate_type(type_walker, schema_walker)] + elif data_type.which() == 'enum': + return "text" + else: + return str(data_type.which()) + + +def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]: + return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")} + + +def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]: + def schema_walker(field): + yield field.schema + + s = field.schema + while hasattr(s, 'elementType'): + s = s.elementType + yield s + + def type_walker(field): + yield field.proto.slot.type + + t = field.proto.slot.type + while hasattr(getattr(t, t.which()), 'elementType'): + t = getattr(t, t.which()).elementType + yield t + + if field.proto.which() == "slot": + schema_gen, type_gen = schema_walker(field), type_walker(field) + return generate_type(type_gen, schema_gen) + else: + return generate_struct(field.schema) diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index 3f2ef2ceb4..12f9328532 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -23,8 +23,9 @@ from teleoprtc.info import parse_info_from_offer from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker +from openpilot.system.webrtc.schema import generate_field -from cereal import messaging +from cereal import messaging, log class CerealOutgoingMessageProxy: @@ -205,6 +206,14 @@ async def get_stream(request: web.Request): return web.json_response({"sdp": answer.sdp, "type": answer.type}) +async def get_schema(request: web.Request): + services = request.query["services"].split(",") + services = [s for s in services if s] + assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name" + schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} + return web.json_response(schema_dict) + + async def on_shutdown(app: web.Application): for session in app['streams'].values(): session.stop() @@ -223,6 +232,7 @@ def webrtcd_thread(host: str, port: int, debug: bool): app['debug'] = debug app.on_shutdown.append(on_shutdown) app.router.add_post("/stream", get_stream) + app.router.add_get("/schema", get_schema) web.run_app(app, host=host, port=port) From 3fed87dbb701c263f38d64a29f7bbf186ad25e39 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 4 Dec 2023 23:16:12 -0600 Subject: [PATCH 37/87] Toyota: rename LTA torque wind down signal (#30603) * bump * rename * bump --- opendbc | 2 +- panda | 2 +- selfdrive/car/toyota/carcontroller.py | 8 ++++---- selfdrive/car/toyota/toyotacan.py | 4 ++-- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/opendbc b/opendbc index ed3af3da1b..5b0c73977f 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit ed3af3da1b7d52aed910f22cc5540e31f92ba254 +Subproject commit 5b0c73977f1428700d0344d52874a90a4c5168fb diff --git a/panda b/panda index 892ca5a0f1..ea78657bef 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 892ca5a0f19c07a4786373e360027ab6ec57f723 +Subproject commit ea78657bef236c6a74b44549e541caf1d7fee618 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0fff0fcf6b..badfd33e0b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -88,15 +88,15 @@ class CarController: # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle - # cut steering torque with SETME_X64 when either EPS torque or driver torque is above + # cut steering torque with TORQUE_WIND_DOWN when either EPS torque or driver torque is above # the threshold, to limit max lateral acceleration and for driver torque blending respectively. full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and abs(CS.out.steeringTorque) < MAX_LTA_DRIVER_TORQUE_ALLOWANCE) - # SETME_X64 at 0 ramps down torque at roughly the max down rate of 1500 units/sec - setme_x64 = 100 if lta_active and full_torque_condition else 0 + # TORQUE_WIND_DOWN at 0 ramps down torque at roughly the max down rate of 1500 units/sec + torque_wind_down = 100 if lta_active and full_torque_condition else 0 can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle, - lta_active, self.frame // 2, setme_x64)) + lta_active, self.frame // 2, torque_wind_down)) # *** gas and brake *** if self.CP.enableGasInterceptor and CC.longActive: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 9c0b0643d6..e14e3e53a0 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -14,7 +14,7 @@ def create_steer_command(packer, steer, steer_req): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, setme_x64): +def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, torque_wind_down): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { @@ -23,7 +23,7 @@ def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, # 1 for TSS 2.5 cars, 3 for TSS 2.0. Send based on whether we're using LTA for lateral control "SETME_X3": 1 if steer_control_type == SteerControlType.angle else 3, "PERCENTAGE": 100, - "SETME_X64": setme_x64, + "TORQUE_WIND_DOWN": torque_wind_down, "ANGLE": 0, "STEER_ANGLE_CMD": steer_angle, "STEER_REQUEST": steer_req, From 09a9ba6de1603fbcda0f883d5298b3f80f8295e1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 4 Dec 2023 23:16:33 -0600 Subject: [PATCH 38/87] joystick: bump max angle (#30602) --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a0bfbcd837..6f2ae6aa3a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -633,7 +633,7 @@ class Controls: if CC.latActive: steer = clip(joystick_axes[1], -1, 1) # max angle is 45 for angle-based cars, max curvature is 0.02 - actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 45., steer * -0.02 + actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 90., steer * -0.02 lac_log.active = self.active lac_log.steeringAngleDeg = CS.steeringAngleDeg From 2ae7d99143751f2e81890ea2556c9fe0175b54d5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 5 Dec 2023 00:59:35 -0600 Subject: [PATCH 39/87] Revert "webrtcd: endpoint for message schema retrieval" (#30606) Revert "webrtcd: endpoint for message schema retrieval (#30578)" This reverts commit 10eb70daf707760f785490f69b2d17621698ed75. --- system/webrtc/schema.py | 43 ---------------------------------------- system/webrtc/webrtcd.py | 12 +---------- 2 files changed, 1 insertion(+), 54 deletions(-) delete mode 100644 system/webrtc/schema.py diff --git a/system/webrtc/schema.py b/system/webrtc/schema.py deleted file mode 100644 index f659b34293..0000000000 --- a/system/webrtc/schema.py +++ /dev/null @@ -1,43 +0,0 @@ -import capnp -from typing import Union, List, Dict, Any - - -def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]: - data_type = next(type_walker) - if data_type.which() == 'struct': - return generate_struct(next(schema_walker)) - elif data_type.which() == 'list': - _ = next(schema_walker) - return [generate_type(type_walker, schema_walker)] - elif data_type.which() == 'enum': - return "text" - else: - return str(data_type.which()) - - -def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]: - return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")} - - -def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]: - def schema_walker(field): - yield field.schema - - s = field.schema - while hasattr(s, 'elementType'): - s = s.elementType - yield s - - def type_walker(field): - yield field.proto.slot.type - - t = field.proto.slot.type - while hasattr(getattr(t, t.which()), 'elementType'): - t = getattr(t, t.which()).elementType - yield t - - if field.proto.which() == "slot": - schema_gen, type_gen = schema_walker(field), type_walker(field) - return generate_type(type_gen, schema_gen) - else: - return generate_struct(field.schema) diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index 12f9328532..3f2ef2ceb4 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -23,9 +23,8 @@ from teleoprtc.info import parse_info_from_offer from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker -from openpilot.system.webrtc.schema import generate_field -from cereal import messaging, log +from cereal import messaging class CerealOutgoingMessageProxy: @@ -206,14 +205,6 @@ async def get_stream(request: web.Request): return web.json_response({"sdp": answer.sdp, "type": answer.type}) -async def get_schema(request: web.Request): - services = request.query["services"].split(",") - services = [s for s in services if s] - assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name" - schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} - return web.json_response(schema_dict) - - async def on_shutdown(app: web.Application): for session in app['streams'].values(): session.stop() @@ -232,7 +223,6 @@ def webrtcd_thread(host: str, port: int, debug: bool): app['debug'] = debug app.on_shutdown.append(on_shutdown) app.router.add_post("/stream", get_stream) - app.router.add_get("/schema", get_schema) web.run_app(app, host=host, port=port) From f46f00b373fac85a9b0c673feb4e809b57802216 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Mon, 4 Dec 2023 23:51:52 -0800 Subject: [PATCH 40/87] webrtcd: endpoint for message schema retrieval vol. 2 (#30607) * webrtcd: endpoint for message schema retrieval (#30578) * Capnp json schema conversion * Schema get endpoint * Type annotation for generate_field * Filter empty services * Add schema.py to release --- release/files_common | 1 + system/webrtc/schema.py | 43 ++++++++++++++++++++++++++++++++++++++++ system/webrtc/webrtcd.py | 12 ++++++++++- 3 files changed, 55 insertions(+), 1 deletion(-) create mode 100644 system/webrtc/schema.py diff --git a/release/files_common b/release/files_common index d92be1c3a6..3fd5df5693 100644 --- a/release/files_common +++ b/release/files_common @@ -283,6 +283,7 @@ system/sensord/pigeond.py system/webrtc/__init__.py system/webrtc/webrtcd.py +system/webrtc/schema.py system/webrtc/device/audio.py system/webrtc/device/video.py diff --git a/system/webrtc/schema.py b/system/webrtc/schema.py new file mode 100644 index 0000000000..f659b34293 --- /dev/null +++ b/system/webrtc/schema.py @@ -0,0 +1,43 @@ +import capnp +from typing import Union, List, Dict, Any + + +def generate_type(type_walker, schema_walker) -> Union[str, List[Any], Dict[str, Any]]: + data_type = next(type_walker) + if data_type.which() == 'struct': + return generate_struct(next(schema_walker)) + elif data_type.which() == 'list': + _ = next(schema_walker) + return [generate_type(type_walker, schema_walker)] + elif data_type.which() == 'enum': + return "text" + else: + return str(data_type.which()) + + +def generate_struct(schema: capnp.lib.capnp._StructSchema) -> Dict[str, Any]: + return {field: generate_field(schema.fields[field]) for field in schema.fields if not field.endswith("DEPRECATED")} + + +def generate_field(field: capnp.lib.capnp._StructSchemaField) -> Union[str, List[Any], Dict[str, Any]]: + def schema_walker(field): + yield field.schema + + s = field.schema + while hasattr(s, 'elementType'): + s = s.elementType + yield s + + def type_walker(field): + yield field.proto.slot.type + + t = field.proto.slot.type + while hasattr(getattr(t, t.which()), 'elementType'): + t = getattr(t, t.which()).elementType + yield t + + if field.proto.which() == "slot": + schema_gen, type_gen = schema_walker(field), type_walker(field) + return generate_type(type_gen, schema_gen) + else: + return generate_struct(field.schema) diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py index 3f2ef2ceb4..12f9328532 100755 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -23,8 +23,9 @@ from teleoprtc.info import parse_info_from_offer from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker +from openpilot.system.webrtc.schema import generate_field -from cereal import messaging +from cereal import messaging, log class CerealOutgoingMessageProxy: @@ -205,6 +206,14 @@ async def get_stream(request: web.Request): return web.json_response({"sdp": answer.sdp, "type": answer.type}) +async def get_schema(request: web.Request): + services = request.query["services"].split(",") + services = [s for s in services if s] + assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name" + schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} + return web.json_response(schema_dict) + + async def on_shutdown(app: web.Application): for session in app['streams'].values(): session.stop() @@ -223,6 +232,7 @@ def webrtcd_thread(host: str, port: int, debug: bool): app['debug'] = debug app.on_shutdown.append(on_shutdown) app.router.add_post("/stream", get_stream) + app.router.add_get("/schema", get_schema) web.run_app(app, host=host, port=port) From 56b8a1a5db81d1dd3b4e1a97b90be5b92d1af510 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 5 Dec 2023 12:24:03 -0800 Subject: [PATCH 41/87] pytest: add durations to tici (#30609) add durations to tici --- selfdrive/test/pytest-tici.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/pytest-tici.ini b/selfdrive/test/pytest-tici.ini index a553018309..98e75d0661 100644 --- a/selfdrive/test/pytest-tici.ini +++ b/selfdrive/test/pytest-tici.ini @@ -1,5 +1,5 @@ [pytest] -addopts = -Werror --strict-config --strict-markers +addopts = -Werror --strict-config --strict-markers --durations=10 markers = slow: tests that take awhile to run and can be skipped with -m 'not slow' tici: tests that are only meant to run on the C3/C3X From 6c62a3146607e87c1c4f796eb5a7c988f733d6c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Tue, 5 Dec 2023 14:42:44 -0800 Subject: [PATCH 42/87] joystickd: remove WEB (#30612) Remove WEB option from joystickd --- tools/joystick/joystickd.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index 82847e3fa1..c6024bf5e4 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -82,9 +82,6 @@ def send_thread(joystick): dat.testJoystick.buttons = [joystick.cancel] joystick_sock.send(dat.to_bytes()) print('\n' + ', '.join(f'{name}: {round(v, 3)}' for name, v in joystick.axes_values.items())) - if "WEB" in os.environ: - import requests - requests.get("http://"+os.environ["WEB"]+":5000/control/%f/%f" % tuple([joystick.axes_values[a] for a in joystick.axes_order][::-1]), timeout=None) rk.keep_time() def joystick_thread(joystick): @@ -101,7 +98,7 @@ if __name__ == '__main__': parser.add_argument('--gamepad', action='store_true', help='Use gamepad configuration instead of joystick') args = parser.parse_args() - if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ and "WEB" not in os.environ: + if not Params().get_bool("IsOffroad") and "ZMQ" not in os.environ: print("The car must be off before running joystickd.") exit() From 7948a61b0eaf98b3ace03e26027d8493f749e74f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Tue, 5 Dec 2023 15:00:05 -0800 Subject: [PATCH 43/87] bodyteleop: toggle joystick debug mode (#30611) * Enable joystick debug mode in web * Remove mutable vals * Rename thread back to main --- tools/bodyteleop/web.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py index 2ec6eb037c..53077af67e 100644 --- a/tools/bodyteleop/web.py +++ b/tools/bodyteleop/web.py @@ -12,6 +12,7 @@ import wave from openpilot.common.basedir import BASEDIR from openpilot.system.webrtc.webrtcd import StreamRequestBody +from openpilot.common.params import Params logger = logging.getLogger("bodyteleop") logging.basicConfig(level=logging.INFO) @@ -23,14 +24,14 @@ WEBRTCD_HOST, WEBRTCD_PORT = "localhost", 5001 ## UTILS async def play_sound(sound): SOUNDS = { - 'engage': 'selfdrive/assets/sounds/engage.wav', - 'disengage': 'selfdrive/assets/sounds/disengage.wav', - 'error': 'selfdrive/assets/sounds/warning_immediate.wav', + "engage": "selfdrive/assets/sounds/engage.wav", + "disengage": "selfdrive/assets/sounds/disengage.wav", + "error": "selfdrive/assets/sounds/warning_immediate.wav", } assert sound in SOUNDS chunk = 5120 - with wave.open(os.path.join(BASEDIR, SOUNDS[sound]), 'rb') as wf: + with wave.open(os.path.join(BASEDIR, SOUNDS[sound]), "rb") as wf: def callback(in_data, frame_count, time_info, status): data = wf.readframes(frame_count) return data, pyaudio.paContinue @@ -61,8 +62,8 @@ def create_ssl_cert(cert_path, key_path): def create_ssl_context(): - cert_path = os.path.join(TELEOPDIR, 'cert.pem') - key_path = os.path.join(TELEOPDIR, 'key.pem') + cert_path = os.path.join(TELEOPDIR, "cert.pem") + key_path = os.path.join(TELEOPDIR, "key.pem") if not os.path.exists(cert_path) or not os.path.exists(key_path): logger.info("Creating certificate...") create_ssl_cert(cert_path, key_path) @@ -106,11 +107,13 @@ async def offer(request): def main(): + # Enable joystick debug mode + Params().put_bool("JoystickDebugMode", True) + # App needs to be HTTPS for microphone and audio autoplay to work on the browser ssl_context = create_ssl_context() app = web.Application() - app['mutable_vals'] = {} app.router.add_get("/", index) app.router.add_get("/ping", ping, allow_head=True) app.router.add_post("/offer", offer) From f32e3ae799aa26a132bfd61e1d98cf7273b5c239 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 5 Dec 2023 15:48:26 -0800 Subject: [PATCH 44/87] Docker: pull image before building (#30614) * pin to a specific version * pull latest version * dont need version debug --- selfdrive/test/docker_build.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/docker_build.sh b/selfdrive/test/docker_build.sh index e0ba54f058..bd8c34c472 100755 --- a/selfdrive/test/docker_build.sh +++ b/selfdrive/test/docker_build.sh @@ -17,7 +17,7 @@ fi source $SCRIPT_DIR/docker_common.sh $1 "$TAG_SUFFIX" -DOCKER_BUILDKIT=1 docker buildx build --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR +DOCKER_BUILDKIT=1 docker buildx build --pull --platform $PLATFORM --load --cache-to type=inline --cache-from type=registry,ref=$REMOTE_TAG -t $REMOTE_TAG -t $LOCAL_TAG -f $OPENPILOT_DIR/$DOCKER_FILE $OPENPILOT_DIR if [ -n "$PUSH_IMAGE" ]; then docker push $REMOTE_TAG From abe39e50766c418818bf72d2567db78978dbd466 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 5 Dec 2023 18:10:01 -0800 Subject: [PATCH 45/87] Soundd: move to python (#30567) soundd python --- docs/c_docs.rst | 6 - release/files_common | 5 +- selfdrive/assets/sounds/warning_immediate.wav | Bin 62760 -> 68306 bytes selfdrive/manager/process_config.py | 2 +- selfdrive/test/test_onroad.py | 2 +- selfdrive/ui/SConscript | 6 - selfdrive/ui/__init__.py | 0 selfdrive/ui/soundd.py | 160 ++++++++++++++++++ selfdrive/ui/soundd/.gitignore | 1 - selfdrive/ui/soundd/main.cc | 18 -- selfdrive/ui/soundd/sound.cc | 67 -------- selfdrive/ui/soundd/sound.h | 41 ----- selfdrive/ui/soundd/soundd | 4 - selfdrive/ui/tests/test_sound.cc | 75 -------- selfdrive/ui/tests/test_soundd.py | 80 +++------ system/hardware/base.h | 1 - system/hardware/pc/hardware.h | 8 - system/hardware/tici/hardware.h | 8 - 18 files changed, 186 insertions(+), 298 deletions(-) create mode 100644 selfdrive/ui/__init__.py create mode 100644 selfdrive/ui/soundd.py delete mode 100644 selfdrive/ui/soundd/.gitignore delete mode 100644 selfdrive/ui/soundd/main.cc delete mode 100644 selfdrive/ui/soundd/sound.cc delete mode 100644 selfdrive/ui/soundd/sound.h delete mode 100755 selfdrive/ui/soundd/soundd delete mode 100644 selfdrive/ui/tests/test_sound.cc diff --git a/docs/c_docs.rst b/docs/c_docs.rst index 0b9d23972e..6e8808ec20 100644 --- a/docs/c_docs.rst +++ b/docs/c_docs.rst @@ -41,12 +41,6 @@ ui .. autodoxygenindex:: :project: selfdrive_ui -soundd -"""""" -.. autodoxygenindex:: - :project: selfdrive_ui_soundd - - replay """""" .. autodoxygenindex:: diff --git a/release/files_common b/release/files_common index 3fd5df5693..ddc3e31d4d 100644 --- a/release/files_common +++ b/release/files_common @@ -306,10 +306,7 @@ selfdrive/ui/*.h selfdrive/ui/ui selfdrive/ui/text selfdrive/ui/spinner -selfdrive/ui/soundd/*.cc -selfdrive/ui/soundd/*.h -selfdrive/ui/soundd/soundd -selfdrive/ui/soundd/.gitignore +selfdrive/ui/soundd.py selfdrive/ui/translations/*.ts selfdrive/ui/translations/languages.json selfdrive/ui/update_translations.py diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav index 9f6f672e2829c0924fc9bdb175050240ef9259e0..b1815a95867b9e4f5e2f5ec41ad29f80ffae228f 100644 GIT binary patch literal 68306 zcmZ6z1(;Mv*EQPi?mF%YL4reY_YmBI2Y2@X!7V^=mkI8{f@^?rm!5Wa*V{#X{SWUQ zo<1C@y=(2YrK)GD&d@$RdwwJ#5x@L2qQ|s3ixU_K1OgfTxBhYTWEm2HLNE|hCM}+H zG1>-!h(r+4JK+du5sILr*OBNk82yC5J%RVa-|k_)Jw-;3pe+vJk6@!Mfh70=cOwy0 z^a<+!^nSY?eH#9DCm6v;B|r}V4>$rD^tbj<7UBW=f>F&t5BPw-Bav_fg9t=F02OEl z5n-eE08-Rg{H6)^@x6tRH_#Fj zwG8(S3(y8Fz#}{nBElQJAN-~nA`1LJ%K$pS6u=4iMnsQ*Q@}FN3Ht-q0({VJ=q)G* zt%eK&i=f@$2tE)SkOUqCn1D{$3Os{n|9=_4J3tHk3;K^e2Ymru5ItlPpoX4?yv2^t ze$X251|tdD^1oh=0*dMcEr3^JXRu#rHI#z!6N^n~2YdoF0HiPqnwW>4060L8z;1vX z@Cti@d_YgahzGtwM&kn*Bj_2n z0g~`}i~-OSxC59348){k{(>V(1~CO|4^=LFQmRgq05P1~L^$LeIqd z1AV|;1~Kx#?O?Bvqgd=g>@k+0G~^vT0f=DK!s-Dk1E_S+`=AAA3y!hKiZK;K2XYf= z2bw^8z#_x}<-R{c9Uzlp6%%L&u?4CctfAjm5zrpyLQDqq3Q&U&%oW%dKmc`tYakcH z1M~yFfE=iwzHb^qD=?~j&nhx{hlp^0tEJeeR3Jh@m?J!dF2Y7Qqx$HG+DHe)<;Z%3 zIARFyM3ka`!Tn2kL_9&-O9(IO(#7f?j^`^H!xCE&&c@TbKgEswd;d3*BNwl zaI#&u+!EiAU|nP}`YtYy_=vQW{5v^~{0}LG6i3{GUxZbn$Ov(0s9)pZyDOa%=NE_1 z@!0vJd%bsmAS0qfb;Q{SS4dmPSIC3NM$$;qC}I)*5jG#)A2BKPyFcU+xbHhXj!TZG zjvY>mtB-e4Kos$ye#Hrh^`w8ua`GXvn7oqo8?g?514~6y5cp84|GejwYpL^J$41BB zj!jO1`!8=*;7VjQx*oTM*o>?tBPhqo3FHN&0mQ%XgR%Qj+aq&>2Yk&v^IiGQy^d*) zJC47d)7&E8zTj*`OUyRBlenBrrg);3G08nh1Y$G%7mNv66#gfm^B#2Doi&bOj<$}g z4wbXPE%)sX9Y7Ao?jyV;p(yhyT__*OiDV&Z5kZV=6Ai0N!9~8C?)}b8$FKHYcDa3= zQ|(^te;m$4kHw!S-J?9A{!8sn9YgsMq;K7=ANnkJ*YreSD{DH0MHKLI@;A`K*aUOUFxLY9Y?*Vc}Xh+7|6jx^&AkU!CS zF~%|`(a%$Dr1khI==yMHztT0@Zn3;H+su2d?;OLu{tz4Ul$bz!$ym#L%iz=3QJ6#} zCJT`mnB|^l|I3nKjyK=8*z6ZQheIR`m-LplnVH8L$DB@|Mma}F#|R>2zJQZzn`Z81 zsy3xtbDej6BM@ACfWl^eWSwAPm_uksNL-u^u`nQTAGVD(e=*vP26Kgdu$K@K<2F;$ zn7!G3*ncz4v^eq_Tn=(=0Ojs()0u`C4;b&6EA7X;TO$p)LDXvIf9!4SN6ag<-J~;E zE~2x)z!`7lo6LrT#*r3^bF%+7l1R*;|<5_K=Z6+G`8akT@ zwg>Llp~cw!l=sX*oN1ivtWESbWInDNqNRU~^RQ)^@kay8D7O6Q^!uNnhLKh<&>R`( zEW3eGPN~I-|4rM&-o~|ZPO_fR-V$rkzlOZ-b~d~5l75^1nQ@ejS=ria<)I3$!4D6uI`I| zu6ea{Vi1MpQB&CxUMrrE(~;pJ{fjY&gdURZH=|njT$gGbVB79ZMLr?cF$Qrj@D_4A zvZhfd;1449`vUf3rhGkDcU6zHl)DCnyW(5Y?{kp+1-z?lI-N}%fodM;z8a!tQci_$GI{%4m4mSQrf#q5s!P5d9Zl?*EhhdCP@?edtX>)UDH zYNr^B?6U)nm^;*woa_90{!H$B#%a=Wv@3Ad+27ottJB=kUNnw(bP4LP7ik=>NMPV+ zbGtFT#Jea$;GScvX^8HmMy<^=jdiXKaq+L{Mcj#k8vZ@bF*=&K2dVX4u-`B~)vnaE z)G^KHU8u-af}gRCw@=vbOTk(HkfZHdjz0xW;SG3Xv~jDuQ{g`Q~G~A2eSz zI}GFOYXgI@8)?J1=LBa2TCS5ZlT?m!`e!&2Ob@kK%_ObaIK^2Jx{Y7Vn9oxR@&t0u z68dg}3PJGQv=tgq+K@V4x72*dJpqwHn!wu47YYC2e_)qTPvM@2tKFY1BlQC{3{ARz zqP4lN75XKm7iX0qLFna;V};3^FbjguoL*D1u1LL6v(|9b?g>zFY4kcSR#+tnbBh=r zVkcB<|9SfyW19Aix|=q|BykmnKN6-h6Ziv!qXkbmXnHB$5{Y>3T5bAenwRRqx-82Z z&n@IK@?^F|Fim)lKY}f#9>?wsU3T3tZ_&L{d)52&lWhh5|1ceBleh?BU*Rp@zf32o z8JZs$?I<*M(2i3d)bun`oNq!Nd;t~A?g>}9p+A+ ziO6x}ZS3iS=R&ODGP^JJ5_WEAb(G<@x~=MW>P`9`wurwu_5|$)w~ug*@Hy`^a}Vht zYKs4$U1}`Qm{lh=w~Q_)Intg;V13|+g$4nI8=_6YCq-=Tjg}(aA~i#ut$Sl3`R1Ve zQPJF9Lbvc5zm#PpYtTCb2OUq0D$T#DYV{VwLdWJ%Ek4AU!e@x)3Ef;GV*{Z*0_7cN z?WSL^ex{nCy=8vlNkz4xwC4zfKZv>uF0dOZI?UqWIA;%2ruMk%XZ3CUI{WIN2lp!@ zgWpD^5boi9Ve}-*5lg%etPAwB)#Fv=8oi0(*^7KZj^ktscZ;3~GC4D;ld$E%)6OX- zmUfORtZJ#Z+k$}|I01vmA0c9kti0yTyTqQzPu`$)tA3igv+8%v8q;n!6*-1n!p;(& z7a0X>IZvqnV4H{3&H_`mc9iOss#>?tb|TObC#QRPb3|iB?fC1NF5-Hm)l0E$)OS?- zlr1%FP2=5e#0av3JxJIf$`k&>aZq1lvqLq`B_^*XRkd4nU$@4#H_#aupbPj%M9W1Z z`1_c6(q^Q?Yq!qTXRB+KbdAW=+g*f z2J$4iJLiC~xp<|pjH9QX#%hCGoN=bpn*Wr8R0nl?ZC?VLaGe=5`Jba&HuBFh8;G5d ze|dLXO}gu<<;q5NN7FI)7~~i7D$Z+RKk@IvB2E=`4fcMpowL|DLbF|&t~#oFV50{c zaHkm8`8;uD6w6lT8KMKx)7!;*Mz>KlL8(=bHI=*XBXcM(I6>iW;%&k^oc+`U?C7A< zvC1gY%vL6=%5@p`KY|nR1g3?bE`B8H&Tqx+KsN=}_QW7<M zxZOl6#B+s?)yGd{h+Ai=*Gd{To8Knwj!6eg#K4 zP}{?L)Bh5yq#x&R6JHfy5Zq@yAvGY4-aoASbS~vW#bZ^X@tW&CVgq>#XP78ivP|@X zdz41Ug@bLKi;b7mXB8~vO)b{8HE;!YkI_o-N&G-OM$m#KBBdakdrK^S?Lp-b1x{UK zT;bk^>`(cDdsWmzGDtL$i>D35_6Y8GXbhdz^A%l{ZFT?HW(8N{M=}oxoZ`R43_$^N z32{AQyQi&XwYI(TXT>k-38p@tNhmJ$BNr#`E6Ee#xYMauF{cCd_5p@Bs%*tjP zdv0g|A(fRb>?S!YF5;&$KM-C=%G`gLeHw+LjpD32VxoE5pzEpQd85TsB~IaXP7%eA zmiyb;pX$e|2#Qt8RDCl?B*Y`WW^EEqm+Tf#<^RdZCL~94+4E?FUN&d*{T$FB-+ zcg-;&HDeT=6$3OaEr)zhF`eks`6c4@lJmj`?7QTbs0%*7^{8&2@}69-OfnpDUWr^J zF*(CUw2-0x^($oKqlrA?YGs!DG>X!F3D`cJ?-=s6Fyu6=LlQD=p9$_ZMT3 zpsQq-L@a#HT2ESoobCP3@<m$nP@&yW(L+3EV(RG@w=|3kiAz0|DmcE=#-OZkH(t)&Y^vpKyf zEzoKo#kNQnuf)p5%CY+Qj!3vA$-}-V8Y0b;{K@akSb^^xrn+7jGu4abYvpxnlI60G zhJ8Wj3NA@{Nq-VyInT&DQSE$FtnajE73Ga>l+X05ov$Nk@*_^V_>i=dWElT7{UdH= zXrL2m+@V6qm&sdcZd%s(P1uu+rvjpMl9VFa#O9LUM7?;(lA--kajS8mvcF-V>kdLr zp3dDW{v=J37pG@ zcul%fdRX`utClzm;c>S&o77|Ezctn=CmO%H7ou)ZO}wp=ALF#*Odg5$4AUxrv%7RG z<)FrE^5xopZ1~VI0){DstU;_+Nrv@XB#owlCQM&w1ZYI$E>Gsnm9Qqm#Laq*tG z?b4?LEb|jy8ZK}WjLlVD8+$jtQ1vyB_hn)SG8}?W(i3rW#r2#V$}v>X`_&TCY?EJ= z38E3@cU+H5BhTgLON!#=OZy7OGZ^?$p&Cbj!v&>JHmtFOnr8XQABW3j9ul^WI~FGt z_vB0`S0OXJbjvTAWce1^&x##-z4IcXA0>fzNaBv`Dy`u^qxZ+13}!mc==sV^vXPC) z)o(2M0U;jGYA4zfcR$V``pAAvLLx_b`k8O3A2v>wZCAt_?zt`_`%#^|Olhn54$^si z7JV1?X`sKoPPbVxUiMofPqWX;4<-}LtQt{c+_ShdqK<4Iu@z#Ady9#o?$-E|tXT2f zaKU{A)roeSe?q!4o+|b8#znno_8+pP>AZ4*Y+K_|&3Kz6ltO&XUM6lH|1z$d=pd^z z@z=+S&5*hCdqsc3&ZE1{F5h%<#e3pvM=YR*-^ma!X>I*{*` z{+4ho?ukEnR-UUfIx0J;{1Jh$m)oMoYFP z%t?4HzQisk7DOJnW*N^b|7!TX{*Zi-!RVfeo-1Yy~5;)FD86V{2}f#uYs}yHN<eHL}&L2V@Kt@`Zu+gWruXDTnkZ3+Dzflg!-gq2}^|4^g&pazrp%IvsQMmmRqk@ z)>w`OmlNZ;Wzv~Ry^}gh7jXon!;#*uqlTXqPwURsN*WjIU%B6+J1~}sLJ8@~KPJ2q zRMAFas(s~_x9T?y6KdPm-%{DF4WUn@&b+_l)Jc7lu1L1Cg~XlV-p(`nmGU3zj@905 zTxsxnSlF4&N#e_iKPP{Q5AkKxerTCjWv*A@8!)xM*RNI!?C}vSWjKFB{EX!GNo2`& z)(?b3p<#~Kx@(Pp)vl>cmM=7teJydjS^XsglP)D6iyy+rP{*Ro9-Aqm>{)-M=2?AP zO}?WeB8fU)&?%uVxku7rv5J|2|1;Rleoy;cMyXv_`>%YfX_S92{uz6uG?er#IW2w- zk4gCzsdl@J6y=e+c{Q{Krna|pJ918xIecRC6nv6dgk<){-3-jODK&K2pEX-+e^$&k z&ktNAyyE1>ZA@-V{uEctZAo5;+lIKB)3ss6YRi$}N8(p5I-Z?UpFA=S$?Z)#5s|ub4ddklYsoe0hA+Am z?h^Dl#$fTfBu&ck#ACuf^yQdJ-yRD~J+b~q_1oHW%B9wOAqLsPQ^%i6@g+A(+j3?Q zuZ5dAo9ibx?ysq+9xi*SKkT7k|6|UP^h|D^Ix*2Em`GcX)_aee1*&~@9jcLa3su`~ zmT(#+#Q&VoGBuFgOFEMM8{uB4jia@0lgwCMT3sxAZ20JHkF&D|N^Qy0QVEGI1Y4*F zQ6A4FlSKKd)>zfPuDkl8orjn}6$mdU7Niy>7f3d;w&ULgd)qr`k2j2~eqX(`@rF_5 zTaHiROpUvivM+UC!g&5h$`vHVbIzEk2-jSwx=@#(dF}Wad6?Elv?+<7b}2b5KE(VB z_dYPp)>BhZ|Fue9og*(aB?Yb#dU8j_w@W>i%1hYIyG*`@5V#*0(&XK1=2bE3`C5&0 z52}PdSUfIyVcNpvzT%6FW7vBCAZvT|^Sa?x_!_rdV;&Xs5V!CqC%96trS6VD%-un{ z6rs7!>k}G(uO3!)qMoa3;jTjSn2RLsQl6zzlb4Gw(NAE?eZ4H%DrK#ya$wCn1;ctF z)QS9pKO=Ef+KJT0xXYZA#JgdF^Mp<;tElQ$)utg!zu40fJDjycib(aOT}#>{yhPiJ zuJv{?{zMgHngtlP$g^VXt?N| zg4@eJ756YTB|SgsmEa-u6sp?O!eo&1Ycea5wO3Rd?EMfPY8%m*<_EIzJ#Faa$RyOW1bq*vGb9kDBob(py`h+^(Ir3q| z7gtdKRkp86R*_iuKx1&$p_ViDN#awJGWMi=6hCJi#6I#@S@^1?+FKP>RWs!Ontu<@ zCN1H+5?`cC(g!A(xu-~HBDbA|x*H9}l{FQQ>MFE9xm#oEnD3>Ow3!*wlnT*f`gY6( z-#xQhkyA6UqGk061a;&H{zz5|chF{`w|SSC?#q9y##LOXo}e6S8y4wL?Iem%d7p71tsw3(djVl? zXrXa6`5 z;&0kkac*i>X8*J=(gM~j{F2}(Tb6oSt+~8)Wh>b<;~4)+0*|*Paa_ipjMStW{u0Uv z_hZk4$$0 zb;RxbTS?}Or5Q65U+~6~`y)EL>U22`?<$U!AFO?%>Fe5tKEiAt$4S4R`BQ3zcs1h( zY+Jw8d`K~Rs8|sxgWc){|ikWrGNpO3G5EpN?O}p%D?` za(i3N``Vf1mF3;)yXo`2TX1JNwuCzwUox%9cZA(&OfQ;@dOj?oBu+?zD$0l^(^-Efv`6e?jd6$4ltw+9g z6AiZ;_Eh{)R#<&g)yHuZiDAr<3ewMH)uf%2wqYr8m4Q*#y~=~ttIF0_bdZ^hxWFFb zUH-CUQP!T!QAtbq7IF#VsOz=WX zMVl(YrLWGOlQA*gz}`amIds#uMWw3_m9{OPQE%6CeUQI!^ zt=Bhicv614)KTSC-nKVGQW+j8FXKV>i}au3?y^SU6N5vnamo)>Yf7(|Z*G`kJnx@J zOcP8k`>C5bi&3wco8pr)>#{lNane;xF|O3# zXfBXns>~`)uec_wHT4W;kxPWPQ*UHvW}%br^U}%M$RKBfW>am;vZB)BnroV;u2q-~ zY;htt%aGkaO(Sm4a9|GlMw!wY_fFD=vZotvJoi$PChKeVl+@EAAMFr2!z(k~Z#Y-}x#U=7cZJ=0D)O1u zU7D4#AcvXxGG4?vMVJs8V^b)dRfy6JWzM>5dV-Hm$m1uboX-9u``467!h6&is5kDR z`d;<#%087$uezz+Z6A+3&d|oS&b*K_E@N+;j5QN42=vR+(Z8? z^ZUlL75z&#mrs>FHjN6NB>yWaOM9L3I;%%g5w9)T5^*_tYEIM)FHJ7(SF>F6(Dek9 z$iXIYvv20SOv{k`${=7D`fSDpvghUbC7mmFG^SbPp|R9n5>q-Ww|iFa#Oqux>7Vcc zd#P$(^^203r9*0KwOu@uaE;vM$tBs_a_Upvq95pm=zQ-c1Fxa9j8fvSXpo<^ZjW&3 zRw+4iWbXA$e!?=2iLfa2qiwQsSXIB0|4Jv-b=1G}N(smLij++`LvqY1&xK^#G1O~! zM0c+)TuLhWRGF`AZ*Pq(W9*J+Wi8INWQ5|Hv+vQEtKK@Cr~+2AL~iyUxyc!ir6|@K+#7#K z^W;WK#ie3>Sz|*#6D#0T1>Z}Wi0q$s0ad@G9 zvg%6}qvTCVS&TzSkEpSTfC%VoqV}TY^R5n2hZr}ob^<_u3oh`Q45qE5Ka z>-crUOBWZ9sH{++>;sVlnLQJXS^0U(GrPvmX3O!S;1bIK`J#%SieHufSijjY+uxpq z5;43kn;=WaVRL>j|bO*LMQJ$Tcw=*LsZN}P$yXY@9 zy^@V9M-l(% zwz?g)|CD%(ycNe4O51YeO{Og2SoV#)51E_eFSBiUUm(L$(r7M!P&B^ORyW!(-@lx+ zL3B0!*F0^`x)i@4jyeJ9a$eB1sG*gl7Y8dzs$GsY=#A_-Nh@+5=J_%@#f@d1!=3W) zH;s|imHkn~Dr?i=H0A`yQ+|;+Gd|^w&l#3{jsJm+L(F!d)w-&M#aoM|)%(?hT{+mb zTuw^6+!cBDben|99Do)39ERF@ed+w7y0YW4(`G_=FfBDMJ&TxsHd~rBg14GyPaY9D=;k?JWuTn>f#?khoQr&m8 z-kL5Y2a4`gtWePH<;Vf7ONkwGn&k&F_s5@Qf5rbBIBV|PIJCTLk+Af0-5kSRe~8pa zyf_1q-#PbOibsH_#v>0o*_zYUn~MJ{dS6LXeQ>-&cjt6X7Uxp)iJ3j(2D0YjI{Epg zM-2nZ=tV`PDGhAXuAq~`lwQspmVYE?YVryGMe>tK#6C~eqsm;AUF@p5p+4ohhi%0x zPO;_|=Fu|T(U`+ypL^#RTGkIMB^TW)J1MI-uMUT4t~f>3&iv|ZK~ig8FH*bkFk6uV zQ8}yVUU93MU$hH6r}51MlhXF*t;;i{Z5OYnpF`(*Ug+M}E-8sCx>cSm-*26OurU5g z&}1LacVvA@uyZ^FOR&l^T5c-OENWdcy;i2n_iZ3zL^slr`HtL=se?oVX=72(T^+O= zYhDz$E;>-LM={9$6UxLIl%&eppU=$N62FqYA3r*fYQ8ReRMuGNE-9}&XAlLpkSoQZ zj5+yZa!;n%1V)M+(ad>VEvk+$K3epoGWu24$;Uk5lqMg{9h9G(IV`RxYjAYQ>1E@< z25aemg||y34cVr*!M@ae(&Vf&`Ok7zC*S0sARmi7w>MKcD$f)dir!WoRX=gbu^V_l zq;|?9=C{ZYNPP?+=D4?o!Bs~neOI`wY>Din`HyfbdPaQ9>=*fg>>){`d1Io?^|GB& zlvcDZnpy0qPS-B-JjTxwoK5?jcQ;R%{zZJ9ei>A%w3S3E4x~Fp=4X#ECVZWfy|cl z&ODWWA-5qVUWlUV5n0alYG;+cXmrtnN}bZ@XoazJcq#R{d-5k{ZjKwx8iOk25s{*Heq73$v8@!JK=^#r!MeKO*PsCgrWlu|;=^4pnVdzi{z!#k@_a zf8-6$pOn#F>StIni@hQJ)w-gR!-ajyhRKS}kHYim2jUN8lbf~5S)4S7*OQbOX4$4F zwpN4+I~ISgW^4C)g7_N(WO`bDMPBc8jrb1z7&_vfrt4qpD?U-!w!E@&m-P^060<|% zjvRcmaoPEabgqqXGq~7N(KxI8d|_3wsP?^XyiY^iB^sTvD}O>>X4+cO09td@8dtf- zT|KtTx1|2;xOm6QKPTu4EU72U&=CMZNG`>|v zT*HylIfd;?U)M8Crr;TBKzcok(@d0Go~+?tBF~I0vOiU>s%%rVuV`)67PZdR7f0vI zQeWf^&z~P<&dn&pjPX|L&(sx^Oe;(*>n4+%%ffrp*I zb1RI6(&8u80__eDpYT;6O8+6>mNzrqCcaPKj<&nU>IT*ZijNeE%bzyxw*HM+!W@|R zN0hmR*+UZ(xIRKbaD(M*cpYV~L%HJ5%y!u>mLh%n(TU9fh!!dU`Gm}el`sek}I3az&3}RJ2wkb&# zSKgx-Q{GC3HTMm*rM{88&Ulim%|a$^;%P}^BMFXF^~q{O$u;HekWuaAQ#tL3oN$J{P{pX} zR<^5rV*L^QN$&>yG2V#e$yuVTAConLDry+@z;njHmDN?q%OceaRhR8Gh@15D(qGbh zWHHi^(h;l^cvr&O_n!@rYl`a|DwAXJS{KR)A`7)I0?-H)?M3e&L8+W3iqs&uT zRk5Z{uDR)4hkDF-FP)e+F5_zQ8PQ0FF#3C&!`6YS5w)W$c{QsP{Vel?d89)A&ZPC} z$5V&LE#-_NO-59?Zs@nk+ErVtEDfXe&)f*i4(317;i;3;dL)h(%%|POJoZN{{Z-;R zVfC0gta^hjExk zgCED>5Lk$#o+suf>f*+ijknZfi_yCr-IG?vzb~B^*Hy%0w;|7su7x2*e}~;)NsyN- zZy0Kwwc+l>NoYz#Txlh)awxJ$KF1G;I|%3a2*Nw!yyzbCWiiKSMG@V&TUz z?+_=U>isY5A^j9pi2|dkH=S_v5msXF=--f&C3}Te*n26HuzkbV+^@~K+Lua`D%XH> zkb=vwdfEq`UNlQo!p)`kCCoy0^Ix#9G@MaCQ2n4YS~_`~A?J~>?2m$OqWRIa8ZJ@~ z%*oI=H{X(^o2jO1dznMiLkYaV}#Acb2+|Cu-h?Fv%e7cAwvo!VzQjb({@N*GUgjyfJzBIZX1BNP#TBo0v= zX@U3}$v~(g)abfpSA>BuM1%-UgcChSA#l+n9bt(uqaR9i4Kp%&#~W?wh>)VIolyv| zx*DvF2fJeMQF*Y+9j<}~>(9ZeY48uO{>Oh?DUFUkkF5g-Yr)})YPjYdTtO+gavZM0 z2J*4hxUcmZtu(u$#79OsUhwJ$PFF-Tgj{?v^PT?*Acnw#@gH`NsojY7x4_CoM zK43ezhXUe+`y9T<3T%Vgp>NixCDgX`K@H{lBMEu}*a&4nd(aBz3S>3LF?a^q0n#uAfh6!6kcT<}KfuOVb^!?xkOS{T!RnP65IU*uAn7Q6KD;0+yS3LnHVM* z7eL$hwu7BgzW>|P19#rQdf8uj90KP37kP&!2j52dl1j4|I-}! zhUt6nfL0I#L<1RxI}&4c5bzqa40^rEffCR7!*aH#4DhT2*CJj)<;u^*b)BzB}_yM?peGn-+stZOb;0^Et@*eDW z1U`gVKJ0Q3if7Ff`PJf<^-7PN=9LP?+npn>rU#x8Ij zz5Ip(Mh|2O;Q79PXhkdop-$KyatJg-PC!k8BU&t`|NoU6VgYgxJ?I~D1a-x5e81B$ zrWNJ`#0{en)-cE{sPuq;P_^Kg02l%-z<2=9nxZo16@VOMD8vIXKso3s80FAg0B0=6 zpv9m~tQ{Q9fh=eh;~rKUuv;u90W%-Q9EbrZ52_caRUo(F%qA91u#z=J0?-fpja6?r z2Z?bCH8;&HK)*4+LQg?HVXX%JLoPrYXl+xp!@dA(v3LM&Vm<|DK(j@DSDK)IN9hyciZA2Cfa3T%yyTmVbVA~?Rl_=g?< zGO-UBp}wBaztC?>9Y(aoaRwf%OAQ#n^)X$NGek_I>^XL@`t# zhd_>lhyo)NjIE|jj6GU5S)Q?5CxD0Q0L%2R9y6aEbc*cz)=V0Sjt^j=yA+)Dy#DOxf#zU?EYj8FT#%0(O%->iIgS`Px z0Ak1s@F5sUzR2{^Zbu>kJCpI9F;Ya!?02<(O$ zW7z?`1kf~j5g>)LHgFcJr0_1dhS3aq0^C8HV=)+e-vK}XBLQlFYyr!le5{XHq{7&S z855h&f;K=4sNZn31^EN&8@LPd1I7c;9YX;yeIHp6Bj~fqa`-J8_e~105c|y@)B$aW zc>w!|c@{ed*2iKPUZ+~#~0sVq;9L!VTd=+R0zkee|-?9Kk zH;}2`2pM6E(4u2KF4~Vb`kPEmBneR-$w!n$BnSb5AF_m*=&{%=!W`ms(go6TQg_lV zVj9tmpMd)b^930fSrw%FU7nTh(XJZje&;>sV%HnD#rrV8kE}plz$Oq1iBCyk(tlCO zAh92jO85oG!E8dl4x@wHd>cGL7t*!dIncS?In_n<{N~#oBqDa8Z{b3O&7>d6-N;Dt zK~jK-C$7P-#A49x5F~K!`V+>Ic1Pc| zaG%_pe4kWFoKIMZ^PpKse3%-T=k4dNavB}m90wdp&eg6%`61~Eu>&C)w*`Fwu{5;EZ}-?;JDm5TZ(7*y=Wb#H5jTpeS$8eD!L(Ba;Jgr(8*#ES@a+qCtzRsZ=ND8iim`2%1Eu$`> zKB4?do=g0I+l977+6AwB54zeo9@-eTpKW*T$*vFHp`mA@x!8T??aVY^s-( zO1Ol%j#w9*ZLDbe4!7a*~t$HC79`mSAqVX z8IF@ygypHZ%Cgg5;6?>WC=b4vlE9e7OlLl)FQ5)5jl{h}mIT$F0>@12VzbBOHQ%wB zT$=)o$b7<7YAqv`^@iD;v5M*^{(~(>{2AEn?rcx6EHf=PwXj^aKlG5oN=zErOJBh% zW!+^CrQ69@@%K=DL!}@cd1wUfJ&oH!9(5G1?1SV=~$E=zATcC;VxC7||@<}z1t7x56> zf0&gN9j*;xw9jOJZd#{buVWheT5PTsVHfT(Er+v($K`Egr_ByG-vl;dm(!@+Zi0Nl zD(-nkr|A0+B!MfA=_bDJ7tK+v&p6PzH#7v#V2F7I0=l4U3G2bY-&Ns6=&~ovMBey*ptseayO&5(__rcQ3OGO>7z zjP-!B5W726>LOVzx=xzI8nWTMZI^!;<{kA*^c@NJ1)X`TqJJygj6NN}IR7+-v`7tJ zJKA{4feRtZ0k8d}ey;VJsP59pQ=)*iir-b}<)?DG z(fIhZNE;8=x>Da)qf>v?X)QOr_fZ2V5%w5CM`2t3E7lZB14bErbH*U^Al-lJ(VA34 zo_%>>H}*a45H}!j2s-fMm_tb?P)Gb&#|7gr+6!vFw%&NfIWjzqKxW!_gM_;TDDG%_ z8$w4!d#}vu(@)iGQ-`%5%u_tkzlf5@vRey&7gq4ku*Xqrv2Q}XG_X z+id?yOd@SGH&HlQSi-AkGRV!*#6VZaePgn=hkCc>C!@q^2;~w4%ohB=g<8P`?w|Dj zgggYpd&YWKAFmmvmT4QzS3RGRf0GN?8wD?-b{}Drs6(;b5YNRj*J|_BXVn+=8rz|O z89R#JgLgrATzG=lfr%sOk>`BHwv~q7nlGww~mKp^^8aGNuOb{01* zoZ_xFx6!4j1?oMzjaICG9VVT2jk`k_63*hcWhImG=v)3Tc8+nL=AufdX>VNZybwkZ z>zPXakD^w>Ronse%lO_AmwT1vpe{{)LshBW7k#J0M)Xo@Kki}SSdoWsVL8b+&^-fb zjv>ZXn&GOG>NSQp4s>`e;S5vHUoQGv=;4;qa|l-={XLhWT7s$>stH6&NCkE*MBl4DFrO4!Ym2>hZ!qMN)8jO_$VWV~mCrH5{n>Z9_y zX02J^HKTH=qq$O%RICYcTz-$XNcW{kn^$F!Y)qQ=UV{fQ4;Wo3afGVCWy2hKs zSVkz1wD!!m@;`x22ic3v$#A_l2|FIWuKy?VHO1v9cse|^#090CwO(D@#0&8AKBZ<1{BwS!**F;q6#VgRjoCga!!h* zkp{4*2~UfsiL!aa=s3J2yxMimbXT)PIY~KKi?z1%x4{miWFD9w z|1P5^p-*(zdoOd12C4i*k)+vWe&nq{_owaTb&YD-C**JjQ#PR2`)h1#^}|)I6^+Vs z`e%+S;fusp>_@_$l6GPyKb~<8e<^(1b$8a6qXMAnh2*lD7Ll4S7(-adL5rwHL(6jQl+fnt-QSfjG^ z@~_0cr(fgC#O);ULN1#_&PIuS`>k_y$CY+@XVoo3TUQT6bMj)&deQHaplC3UP9GnA zcfl0rQsZyxmWq9fZ0!OoHP9Njl#wamOPWdCf)}h^r0vM)ib0E7+e%q2U#4nnbh>az zJ(LQB50XoQOjZT4A^Pru z;g&gCr{c5xmFlbUiu)7tD&;&kUi?h*LbQ*opvGY%ftHR5h6AcE@(&7x?vrg-@HPG` zlObF$NsvqxykQO@_Ca*;d@!3eXB9Rq}!V|FQKJ&~04N*07kF zS+-=hV~3fUu}Rx7r(tZEnX%zCX_AK1Bu&GdG>m4+mMvyxhEMZmX{GP~pVqQ1&D?wL zIs3qj%T9O5OZBRj>H2?}`w7y}8t(p}d>Jr$NE z`l;HV^~ByxKr!3&P5Vt#Y}jsp5X^;jB+g;s1%C_o@S{uw$&A<%o8_Hr-Ku|~!Rt<#A9?EI z{V?lkUhWd1NQmHi)66vT)Xt#Up)~H-sx@m26vx_73`Qg2*s}!n!dd)d%;m%mh>y{y zo=+C6ew&7;!&v5cPbPTSf%J8}uW2ctILoQ$Fs)M+fi8~TMvC?y&0mHaj(*`fxQcY0 ztx2;n4B?liceNdZ)1xSl)UsALP}54c#=`gQNHTGy3@M)~;tFSS9266JYBJ$pZhvT~ z(%jW>jU$}7k*|n%T+;k(^g08PhsZM&0r$SJk0{0K$GCvchV2V)a(yx#(<;zdK7sPb5BmhR9eRQ0y5@%Qw5v9H1+|v?7Z)QMF1pW? z(S5j|q0d9_oy|p$2=2g_i$h^1_i&?UMhsAg>< z5s+hIDbIDwcHIfJPIKGT$#X5f9aBJ8^UjI-h#{60eq1z4^puOIeZU}7 zV*^|4gAHiSHuXb2GtEX)ir7a!%Sj45iRTM~%;vm#bE&CmKuL@mN>PQ2ILlGx2TlFZ_S#EpTliQ-UoV%MA0? z{nO-_t!=Y{FJLE0KX8_aCTB&3$JrN2|03>3vps*9TWRxEd(?I#&r^_?iS5B?CSYd$ zEk>ug3npQEr`7}p+285iDyeF)&S6^^lEHr?i_$HjH?s7?ChQo|4Yx-IyT6(CYi=v| zs7IOHp1kBtTrZ|y@JE(G{F%3ib_g>sdB{J<=G9G91(Z8=H|;aSA;et@hId*V%=#=m z!)imsrZ)mjcO{KF^+e?vHN>p&c1i8TcVk@@(z8wCsk~>@H|T?jo4(W5?mC&WT)&fE)j2>R;lp33>y^WvM$IM@RGawHL!`bUa`?G^$C=W)h zKpl(U^`5p&(soeZP>#`$bsmcLK-*}Q{Kl*$*&~HBn1}I$poO75jxL53s>_X7O;3y3 zzZbSp4rE)zo49=`VdUM|BhOv)Ma_%GE6N6aSJ&Oxa!iDNM=&k>bT(P|g7Fsj z14J6!W&cV4S^0BgI}Ofi58Q`Q$-}rw@id7-oZ!qQlacqM58cmAchz}~CzSII9o^3O zU2Ie41tC`QG5d}nMX$mBm|_HW*_P<&N@?S78n=}mio$=UOyoVzx-a=hJd(4WL`iob zKX6qTA?gkCG-8iTMuhjH^v$mTCJ^LxD#>( zbu52ewm~vee3kt>kq*BWe&TF2G*``PNGMIlHQssY{m9eVeZ)b@hwL-_!L(}hs07#d zhvl0l(2!_2r?uNRM0TO>(0T|E5|Wf4u4J7d5Mb9rj~oI0c4b+^RMj`rW#1#nO5!XI zHfxl0SGJ!wo?4C?8ZYqPGl$g68xW1dbRoyHXa%~8-br{;(m{Ge1Z6$O3!ry`RrVZx zWurlHUUkWQ&o6^sAkE^w%leR(GK065Vn&XQwe);3<*S1VLZe<6ayG^s7zcwSYAT&3 z9Vi;ie2rs3E(PA%M7o^DzZE3)VM{Vl1hCAKW}#qYFE)9!89qFAGzVugnWA>5RO{C1KlInZng z_YDb;SQ8oSf*Jo%W+|pN4l}?!O_SsB6!sf&yPVO|PQqJsD7IhHk&c6cg8DM$J=2f=S5O_P1+R_dNG?sfnqN$9iCP-NdkRfomEG!pQ9RcUaUO}G zuq&8d#m*dF?lv)%)dK$`ot)=#u!dZ7*dB!{`kbrS0qxk}q;654fj0#QeOyh zj#zRm&z*BiIGNrF^Go8Rccyu`>UiDzx=CpU3QFV+>J$CEXkzXkc^9+0bA}KH!hQ?k z93k2e-`wvkHCdoJ< zX64<=6KBt34jAomAkdE zd*cFmq|U3sIRA>R!cJqo&ia`5Aa8Bfcvf?KN64zc8>?KcuA3=8-`LjtW1uzsHf6g& znR7dzk~4rmmeLV9D|*qDZJ5|Vl>b!!R@>5Lir>O*WB;2yH2-{_OuUlW1=l~d)Bo90 zp%T^&mbX@Rv789*LO^MEg|~9O`NySmd9%r_5x+!^Ia9g~ifgjV_3w2*yW1p@gbL1A zi7bD49#_1X(F;2ux!HHb{7Sh={nmDA?||u+nC6{gy47fqEtbDg?X|UybVlE2h_ia; zOAFRWHgb9rd9dE0p7zU{n{|bWx7fNL%=gIZq3;xwC}hXbf~gyv&ntJkc;( zc0+z#t+k(u-o<3IhGx6-c?GrEtJqBmRH!uF$G=9ckrQRZ8$!kr{t2)y6uIDAE~;>D z?lQqZDiI}$QQcnsDTSdnF5j(TI>Rv>ZZmta_+2SbBq?fjKGYqqUs5}&?vQq@YkFcm-pV;E%`Gg?Z=W@d z3Be&#Dc@mpsdA%F^HPqjT~+^5&-V6$Y$sLnYjR0N{QS|P0{T<*`}k&$+fb({t6eVJqjK76 zBc+&p7FHrGoL;Dt3{H1LUxuCua%=&WPNu2JQuqvue2<}6N}2FeUZHk$p6u5Tt&%; zgx`6GbMF-$Dk#i~Gqzw?B=J6h>6eCvnzpqU8t+-Ig^E#E7(ZvvDcnN5vaW#DwW>Vs{=Yye%Vqnes+7n8utu*ow z-H~-)l3%p2=!+D~T|(>uiv;nuUsYt8wz{OAXQ=dcgGR_tgfH{Q7H`a_i~gi_MN8s8 zyNBxO_3~Q_6ta&v_PS(y(Ga!5=bmSk- z`%*l=@LkpvW;OOw(&@cw+^SGlkE*dYbhFGzb4T!*9VP3EHWl~EJ zmQ2X+B0NZypk76{I%!&@{9x7F+Ewau$B*$H_}RQ!d6P;G6}6Og*1%_M|;W{3v39?yLKgsTEy0{t>t%+#Ct^6+~ zO^Wie_c7zxQ^`NPdc&4_Z`HBtJq>irl+bciBlBnJ+2VmEr*fb35ab7NT6m+axr$W# zQ&n?0S*LQJPvOb0gdYl+O%@b}!~+;_(=pfDvrNCJZdVnp=4j&_t064GY-OL!Xt-8T%)u>E{lA#)75=zFH~04_R<)gRf&1TzXTf!{%mrq zXkE6A*&p{dxzW4V0I%;^1*=XfPML9``KVHsOFFp(*Q8BeC;ohL2+j@nw%t`us5w@- zO}0EFvMDrZ+q8cEj2;a(U&dvY$b$*Yp4 zxgKr_=^^az;C;)W#)H*YDu0!Sb<}jWrn&I4t`w!33@$7XkE2(kh4J;SU$tjt6DuRt zBa~Nc^P)tYockf~c$48JjGTuY9bqQ4d!WEn42ROMXP+u|HCb2S z5{hWYQIDc{Cr)#swrSij1aaJhKgOpN;7cotandpDJ9q))z3;T~M14sWrRvxE z8sn&d0ik5{kj^iWHTf<7Z-JT8A2~cS!9GuQss>XzxYnS4?=+?v5T*+U6%H?z6o1R+ zvS#A`PB!x*4Fl`WR?e$BsTh-HKzNRN&-#>8*yL!FwRuzcN74)k=1`rLqI^}Is3?{7 zOZV}gO#MTqiGM0uQM#n)uPhCtHFk00fqRXvqr82kr|Lt4#&SKp5VMXW&3oUZUz6Rr zO?f|(p1@WGJ6rx}sHq}XvgEgQc<%=2wKN05g5qPPe-@4xZ%-rVj8R-_O|n*5v8S4; z++f=jU4)y%Ta(|j^i9c_oC;2Y@B=jDziIkO(XncLC9f{maNO4szLY*lGPLB^(su>T zMEz(dP`^j-IJT;xwO1?3YKE%ZX$FLT1cBgH!LiboCHiOp}q8SFH$k7~&toBOH>sZ^eKp8t@XOmj#4n(i-dUVW@WS4+`$ zaequPDHp_{qEu;Bkt_?rY>(}enByktF3KKM9IV>d@Y3=&T#4axzvTT~dc4WA+(EqA zq%*MQL8ZB z(~<{0Bz+tCnTk7=1&XHTexaFY1Dl&Sx^zaVHI3YH@>}@$kk=A!I9=7W;zBJ?yTaWA z@|-d`>uK@QvP6*~%fWzR-SLQPmKG)JS~0w8S%cY95}A)RaF66WOFe1si1xe=q@l2v zfj;J4if@&ZD(=cI>AHKfVJ~P>$=#AgW&ENw;!X4u==rh1PK`R<_+5^xu5UbOi$$m4 zukz~)Mwbn0vNy-d2@sIbk^Yay?)5z@|E#!^W^v@PQl_7)9Hccb&7 z@9is9@S2b1x6;V9b5zI25w{5?MVHDhmW+{ZW^cxCOI`F*3@Z7B3UXzn?ya#V5JT=` zwa9H&8Y;yWc+n%~qG=e1-RxwTka{CAnHxS3=yYap(9vX$qg zu2wd*VoxPm(bv2v^Z-4Yb2RU1>5=p;*?s;g@}rI`QqBJW`}z&1VVk4 zMJah$R#=SAhNrvHW%1{(R@w)(sd7qH|Ar5i(#R<+gQv-FRz@j}=1xv$>qyw+K$dyD z;!Gv4V!mvSu8p@f%uV}6;w!mc)}`p3_zwLq^t@OT=N;@$ja zh5O3(HmS_vauGy-x*L7FF_6~FQrEmVL%Dou#ixqXb(_-M5!iHvT9rGm zv{~5?1%*NcHHFv`X4xtl6;&6?N7VeN{^Y!soI+kCo>lBED=wKTS(xtQFH2^5R_hMQ zh!wpmtJB;O>qA=fagHeuQT8>>fS~5zB%gy5LhCHk8}L;(%Dbi+5H`9GLi$k|+2c!6 zWs}nk2vnvQb2om-WzlS`{Zy{26g9lC{1EwuUBc^Au%T>tDL-#ZngO95>_EV1q9`U+ zI?8*?hU&U`=fPUjA4-NcF_uj&GKpW(FQA9SAkGEqxiuThw^eOuTx^rZUf{b3b{2js zd(_02lgAAaWRSRTknvUBvx@c=DeQMgw$gw_{T6Nd+< z<)X@K^(6DwP!j!|!_Dtf<}IC`pW@#p?}F=s{VnYq8Y&N#i)z1UF1a5=wo<2NUoK%b zom$)`o4{0I&c zHDTsgT@dPW7W7NHsFawYqaEuz&$k4lq8i?XjXP1saofoFtXD}P++t?Ja! z%pwRkpg*&x=Eh2{lq}4h&wEJf1|JeyX=N*C){Lt}-%1bt8^atz-PJZsS;<-geNdd=A_y+A7)LMEdSJn=#$x*a1 z*7{b!u26pye=9&0)ABY6wxyXXkH+q}?&|RMk86IDZ`E*}*!TlnU+%k{yrP_f30ZDt z0e%OhZ{R1(R^>g}Fxkb%Cu!z50^$U1zxZK3rtqq?n0Jia1i3DXcb(CFtzRs2)syu- z+!cv9{x!EWXHEKDwn_Mvz5+Wq^@~4j)+(d&#dT%syfo+PF612g2Jwl!a9&w<1N#Q? z34BRpw)33sv0_l&$j05qjou9@3K7CvBYmB>SGu47nCeAmB`^9On(MhHqKMS@wY;%0nJ!&Xc{G;-j5Bo7t3@MwpQ{u{LlHH1{Ag@J{ z5=rl6Gg}RBkT>!TCtPb{ILtD}4bdm*4aqdYN%~}5Pw1=AIY)avR9Vz0*0!?!82lM_ zj}+xj%SKB*A{ZM*5+j-MN#3JollpWcR=v$s?dh0kfsHc?Mg1f*vf#W$)aMvV`u`>) z9f$P&RHdq=`Ynzj;rH+rWDU1(7A||MpeD_EUJq{_i+S{ByJn{9XYEJJE8odv2%BQ` z6zQcjvee@A|4jxP9;!QO{xu$P86uAnb>xX$i-;)h&ihPzje7-K7ro_a zVu@?ZGzau>`}Tk#br0ua^cQ>R)=bLy6W|;IGeGAejGTFnkE-)Ys1WN~ZAlw^PhhN4Z^0@rJ z*=wl#aZliP;;jS!IMk+{h6AQ$j?sa_@f#+i-UW(fmJiT5I~OucodigcCP?OfF0_9*5?5GE=1u+1-gut~x{XiW78^8BqfVzP*GI{}{f$o8f z+>F$Wjo@*Rc>&N1+|>_^0&L9a{Jjq&gHq<3|1fFkK#vpH=>}?(o}7MUIzIs9z#a#{ zR^Uu#*FJy|C^>^Acpg9l>{17wWf&_mngKe2R%QA&KwE+18GSRW10DfJ0jq+K|Mooq zoj`!i;Ewz6BftABGYdEZ;0o@)2fGTum;xgxa|Y}^05k<;f?ca%FIT3g0Wg5CTX%t23seJX(8G94>G z&VW5EAR=It0~r8#0PMj?rZX1o#|8ZfS^~r>I10#+O!Nct09!L&0ek@X8q8Dx9Uv|N z4FL_n-Y~E)0*F1(kD0%~EHH+@j{qctxde8*0G|4I!sN0-Z4#b_ZZJKmr)+nJfoofmxKX z4A26U3XBHV23G=B1#|*X1fvSn1dKyq-TD& zu|3m$2IvAj09gP^pyz=#0ULnTK?E{>`7cIbpDLg&;3;4Ypb}(!mC2e+P6Dd{76YCG za0KlJaRkNyvom!A&triTq!1B`@>o}h<;*%@>*UI1bcs3Dntune|f zJ^4N#?AHSk0d)Y5fI5RRz%{{)19b!Wd%&3>q8a=%{lu9n{=ISl^TB8a<$*SXI0ID< zusw4f0<_CyFL><)yT3r(02F|q?{V;dKLb3Pu?1W^Qysw=19byr0yY4@K~#VU_`l)- ztTmtyK+k3{2fPGI1FIN_aYim+DKHYeQiG@g*ROe0vdvvfgB{6&OmVW{~d*Ze?Z&-v_T&Lx&dDM z@9N*LpI}`9kp*`pl1LC01N;Lgq>W=my3fFz&k?z#AYd1Hcvo9MAX!)ELkX>^lVz0j_Nsy)$Rwin}abTVS*8l$>zziTu zGnxT2GX4O)4rb8zdm;eC|K=U&3t&Y6&+kVN+Q@diBw*a2Xgsdr!% z&b$WJ&Uh2_7@%E7KA5RMg#_+6fd~Sw=Ac%9T<}T+Ry5!|cn(+>yeflL1H=%Fp^OwD zMuC-qwK6pW91YCcqi@)mxj0EEb zSRc>|SOwHAqakP!fChMv2Sz`bX8;C()J#@=_c1U#Q^zy!eSlFJ>p;uF`y=q`nTcPp z0)UwX%FbLN!D^I27|;hepW&^@+{0Z=3GY5-^fMj4nvnfqJd z%J99;fp`IuGO?3c1@HkN5wI0N9#|8U4|)uEm5DzmAPL5=Y!q^qyVdczRO(O zGLe_b^2|H*Oq_z2g6{!A`@kpxC1tL1pp;Df0Gfbt21vyq*A20KWSItN|o`&yW9a%>n!a=m(4geuFvzu>xK(LES+tGuZ=T4n|Q1 zw~Swal>mPL$1`^mnN0e>q64faz>M!_K}&)8;JYwTUM4dB?@yjGC;?glIR#!Hz;`qm zP9UJ3fhz&%fOl*_UI245j|`r`Yfz_5&!VY`v8l;_h?{c z1@i*LJ!2hk42S}tHh`aJWvWC58L%>Bd<5tT#xUUJ%>6VNDZm-P(0f96Bfsq8cZFe5FqA+-{6 zDv?Z#fqg^#j_!-w&SvK%dwL%A5g~;*|3SJGjUSX66zl;4(#)f@Xz)a z1davE!{xCKsrN7pY9FQxPLO7_pyRLO3UGa}YIGcV7_Nsz5(TlR;m5%tfu(+opWxr@ zzY(|=T9js|SP2avOz1c^2fr6T1Mk6Yz`e$Q+PO`5kC{Jz@c!*Fn^(P5Z$4LNnUJ8_@`i{zuLFZx73I8{}5Oi z+7s=b6v192A7LKjYVkC}d;E|1x44hkzL=J%L+}HTd5QVa&!P8$ss2O0HohNxQvam@ zJ-j3~G}Ra0h`NlOkMB*GLm&~h;h*9bVK<<4h)U@8WV2XN_)0+Od+P1so$sCP8yy%M z?jQdP!bf^B|KR%(<-{XIE8#8Pj9rNS0r4C{Pt1?B3;yLxc(OcAJp;XK{F+dgI0xE@ zOkx9sy(BAXD`_rqGhT*Si97{em>3kYLY+(0Std&qsMBdIwQJc)rHj;@6T z6XEbyKhg8b>2sWPdfW~Eyy$#L1!@AJCj~)EP|r|WkUryHqkPb}@slBr@3HHaW4Pm* z^S)R%t#tfpKkS(AW(G=Q#V{^bMw&+3 zLVrX%LHU{31ltmEBMFPV^&NGE?QQMd9Yfr00{dd0VJ7VFWE9;-*U`38LWE_Q&hRaX zzeDT1`Vjsybf!?&i^(J_oQwBzK1_=_kuG%q$Tu)&?>IA+7xuQ*5h zZbkB;FENGWuJq%KI{G5&BO(R64}L1KJ;d@x95-!)Yz2;N4>~kDS%w-!oI<qLHz8;e|)>Ka+fUJ(L@o#Xo zx2vrG+G?F>e>7@^UB<~MKQYA2FLX8KAHq8HZfIGI7SOnUw7;`Xu=RIN@%4-T0BeIg zN14Z%#4KQRr(%ee=#S9JF=@c+>S8}*EwvGyoqeomb67s^CS@yQ9kUZ-5>-lkjs6!p zIED|rbxG{Itt=bG+158QItw-p*FgD`v7gz4F@S0%97eB(qGDhC2V8LbGHX*?C+A+@ z>u4Az!!@IRWNc<;F-X+wgl=dobVGEoU+B7G8)a=_+vueEH^kn-pWzMEHp~Z12IDOy zhp;@&&moXcepXgAE0at@)np_i_?WwotTdfwl zz1saHQ~-H_;ZhnHOIVwk$7p2IaLh}nCfeWsuajWgU|C}oID7hgr!N=9#8&iftf{Qd zj6M`OZUJI>Vqb8kyTUfzGRxA-{;y|VWH>B@L(;@73VRu|n!2BmjcT95g|~R-*vD9G zW{It>`)p`8SE4iH)mDQjuD5WyF%RI=H5^CC6)u`ZPp)LmxKFL&oD8V%1ayYa%Dk z>Oen3tVIn^b5DGBPqei&e>VMM#k;$OpF^d1Ic*Ai4d*fIZ`wD)CuDK*NbsBMqP5bb zHASr=&(+9a_@9I-x`Mrg)0btSzQ^xH$m3lDH=R|MO(u-_zHNzjVyqb1hlFQNjI?2>P&BVsNyO*2>83;Y`r{TBu#v3C?r2kKT!tjUJa;9a`qrT2GnUnP66vYj(IZ>>GX~J)CCLfU%#@ zwh}rarzidjD4a4&Q`2UX!1mJfHTns`AaR&iIMtj1tRJawamDZsu@gSA;$K(%~03;mV=Q(71VR~k4H1~Aw56*)0!)>75VGrkO*%7*v z_$TUS@P>bs30w#l!g^1!V#zRQbhcMw51E!4bIh$Ay#raP0_?xkU)k%pOF8ctM$#YX zBdNoo-tN^_s%ed}qh*S#IQ%m#C(X-ol5>K4id{;-O+X>#agQJE447vdV@9J@=)Dj# zAcvDhthwBg+(|4v6@x>=>LV?^a@$=~J0ry0$+0D{A+;MTqCH{1;GW~WVq7Qj(Vk>= zaDz)>S#RuaoNGb42Sv`nrxOv(GOmTYo_&_~68{0dB(^Edxq&b%4N2o)wsXG22``#T zoxo0T`*UlUA@XueGl(lR-aW{Az*uQmX1?xx9?F5`61p%D+yY)l&JlV&L66uO-{PO+ z=wW_mSYrHOo#q`Je~98!Feh#5H6>z?zm7O1!63RWAQfQM#Jp@y-ipyEgm&l~G@ zV>iPjQ+r2;;7v#zcZlxioaLdoR~cjy2DK{DAz*QgHg7V#Gd!|1_hiTJBWIE?vbylP z^LnwLQ3v9}&;^lgo~U)RG0Q+V>Fkw(A0dx$)9G=}XWj^If-#&l5OqBnd?<^Zd+m6G)=0@guURoy^%k`g) z=j=BFEg=tZd+05>1pZ!b7v_qzF4q#R(>>%2Gu+V5(8XeLN1{)Vf0F&Ii@c}2r)&;w zCQb|Oo@N4AYo!}6>2DgZrFB^Yf#ToMCv!{rSGmoZV@buR6^W#OhojNdpqCmtSx_Ef z%#M6brm>&#zViNMLuo(bK0<{Nx@U$pX*jBXY`krE1r9>6_!{~YZVCS{ZVTpQk{C51 z;q{MoJTN`h6AkSxT+hH58xeXthEcY&Y5Z{7qW4rQwik3`c%l1) zWuKvs{vTt3V_$F*REJ;9P;e*k_i$AV8Sz)-&3GICReO$Ux;~=+&2rBpipNl&DMofz zzK?f^y^y*M`v#Hxm|e^JC}(tQqkTb1Ux%e>S%Z zLrEBcSQc}5ciVn2>U1>2P%GMZHE{@2OsnPG=UaJ9_Et(S%#zfZ;9+Mk^Af#BH$2S{ z@+kZQo=2L(it`@uJEnX1I}%pH4@U9cuhvI~KXq*k8mr9rS8^W~PoK%v@!#{#v-ITs z=(EYM0h{BdsX*`7{cHNgbvkkt0VfAoJ^8QsQBFVlI{d$|50UAf&eq9>!@B8)CAKU6 zQ>pp5yYzZ4RPcm1iZzkkh%zO(f##03ru6rs=vZc~dwujM@*DX8`w0IV{{g3g=D?9) zl*n=Saf?SkTKAg)VqXzh4H=qdh3L&|E;z|^F+Y=@>H;P@UUw*4{lh8A>HY7`u6G1+#fOnaxOBkelNV zd~sV><78c)K4o6#k;c2BdsB~ah=OPQ?i?NUF!m1QN@#>@r@5`ZK-b(@=)i_@VMfB= zOcZ~bU<9v-d6Za%_z*+-CfX_uJY6gOa!aW<5a(jTR0g-F;11ux9!^bSJSl(hz7uP< z>T-2!jFpb+G!wvSVslns{!#&-_a`GoXn_#Nrh6N$3k^`+7`@eE@jgjZV~*1{a3>4S z@K3Yr(mXGHQ^SH;X;z5yx*XkW<4EV8@Kg9O(lgfHw3H}!G-D&-D12%3t7oVcZuq9% zrk`j%=G&aygYA^Yf1ltoe++vp74 zrGLg7VP7Lzo)4D0`iI(wdaHG?zhi1FPD$^^yD8YnkFs8qv(UJtH~?|{X%y>P=vJDJ zxNy-y$cNe3$_Xf++{Q=js*3G zrn-ijuj$rl*BCa~rv=wRR}juKyYu4$B45sIOB#uk##{M*w9PVX)o$0lGq3dwj_*MK zMJ?q1Em$fz$B9!nVW&X)hyHcGHO)y&`D`FNvO_<>+NK#=*6~R~7jF*pGVwNIRcw#< zsg-E>S$j^`%2Mp5CB|c>(N1%Nf+d0}oZZv_=3(k+FxT1Dgw;*al8t{l>Owj21o1r! zD(E2m#v?IFL@MG{RFmF|wq2j6{iHi%Y4Dm7C0H?Cz$+9k5(qd*>Ie)gr3ZYMMt-8^W9DcuyXnd-W1^k zK^1#9S6AnyP+ANr`ZzzDUiGP0nEL8m9VcMk3E+>7Nv?m^v$)=4DU2QX~&qj zZg}hnsyVeicdGCYA&xhUeiJteY7XICmrM_|6EzF;hi$I{J)oNj5hhK*7IhJPW;K!? zBb&#g-dENQ`uUn!+VqveZHetc=g^GY*TPG}7uXdtx~D zHXX?yExIr4!u^omX(>pZ4;bvP4F@%Onr4O{9Bso`L_2aJXNK^EsE&V%i6?G@t0Mon zADZF1C+d4zh6Uz3lSh{@QSQV>O~?pk88=__kElO?6XP--2AdLY>zZp4YG*M+7y9~iG|daK9l zEY=NyrO-U0ft4lvK|E7&B}39tP-(!*|dw8TS;_)ZkOv9sJ*JGdX!^g_y8hB z7IS|SeG?l5QdSOe5PU`??s{i3YnG@^YL=LlUQ;rNTf=-I@QV$iJKUYrTC_XS%HP{o zq&KO`Raf+n9Ni+@k!Wg+n=fW%-4|S9PA4pf?GBf@;HGYxV%6UojOCdxoT3qCvK9-6 zWSK<6xbG-vv^(D3H^n+#w@&pzB}j97Opo42VQ5cyzljU7<_aLpllYy`3!!e#e#U!h ztJ0zQ%W}_8fij7m*;LV$EUU=RDWbGN<;SOa4_N-urj(aeiwq-NV`ERySo$G;QP$F| zzJiU6dfZLOzrk^i*#@@yjFPS$VZ9hAfwd-e;@lSzvePk@W`!7uY#Upb=9WNccPg)` ze1`V!dGR_7n=w*wE9+2}pRc6jaf(!ZV7L7r{aV#j<>~akHLeUlvVa8;D2Xvm@tRPUA}T<;IUHhKcLFmh6ge%OZ(hX1~em!(T@m zh=C@}zLQp{uBS5ENYGuiSA{Pkk5h5{$62ZD#UcfZOuPY`6h7mG7@w;;G^$h&OEe8P%bxXf(mX>|T=g>2J)-C)|gQ z2px5B3?8MWfvw(RZs%u0Zxd&8u8Xfps5n;E#V8mf zJuE;;7E7vxvzcx2S0FQjSL{vo{gi(+ELKmpObd*HX-RXrQ?p`{-?MITqvV-LSWN4_ zXPT!z-e7NhqQB|d5TB3DW!4F|Nj6Ky2=_5s(`9ff6^cR1?nb3Yl~m&Uu$PGU zNViCS5a?-xF#jYL`951NYSuRNZLnxM+U;R9Y9{R`!AwbwR3YBQE+j68g+eBWHQmy% zOL0KSGtqsmAtl6VoM~AR=}5^AbR%%;01knlTll0y~wA3M; zDt^xDN7x7Dgjf!teph1$g+^IxqWgzK+mps}+1Vp=dP<7;EvO4n6|n=JfC;Bwq8Qa6 z(Um!yrdd%gGo+%6Qc_M#gkb%G{|nM8SY+>^`__P0G*dk{mj>3tej*R$)n;GM!ArLC zhNpYGozXws3{!g*O0lfrrLL`OLi`N&2{SC}m?O#ADr(MLgL{_h5a?_hsm*GrtUssP zYUv)l1wTj`!#^Y;bpD{x%omNVC|x=~dByrG3* zg6BmNP0+JHW(~+K$k`@5NWY1xOib|gvn*EE)*r0DtX^*08z!M@X%mF(oFlodvZryb z5?{f%;TMhq{hfx7b?`>Gajw^x>Pw_@pJji`#pI|3f6#8Az3J|P-sUZ;4)y2iduTS; zA4mG5sf-^*-*R5$zRx#EWuOxj$q*Npq{nAqBygwnA-|qIX?gqs{c% zKOOdhJcz$mdMxjhbP@jno@^d6oJ z|KKF)HHzu-xr$YKisxZ+9HELcMk2|-l{;DFp})seB#M0-%y1RD4qEp>#kL&`&q1A| zzZL0oN9K=^RB>(-ufueqB8N$fR}{)^icN;WUMA!fQNa5}dOCl0?p~pq_7A!`PW8?) z;nNHi?dle)+uA=z&Z56DK8ks&ado3KqaD;(48vi0vfAa3&M(Y{v)|&MLc9Tl?YVkI z-3wW&VRst;`>?T;9fJ0`0}5W{`1u~n9b|d5-j!uwH|&sIt}D`>an6d5!7XJYB=-FN z`3tjPtS7jCQgVN_<-F=|`2*SM#v5jOP=xqQtrHgH?I`G!6Xty)A4c4cyl`4|97VPa zQ(vNUxV|LH@r|4^>HGq2{$;V9aS(evc`eP#u|kQGKbMVF>MZL+yOAB}60tY$M!{Wa zl>30R3w|`b!*NerRNqj$ufB)Ag{N(*jM$miF2`8#A@8H8fxZKCFmW)=$}yvHmdqe4 zQn75G!*P}Q)2tkW8ot$1f{~H{dlO+5bbb2!U5BX`$(PsO zR?IWXd;utx;t({?J5`9yvkUG}7orx$I=CJBfr|093*=bM0mpB#a_lPh+cZPPzJm4H zX4Y){cu1c>t%a&OCtFlIz2U5>M_@Vp59)A{G{3TNQLc@Dgwhu|G#YXq&~>aQ)T-r7 zZNy1VjKQP0@SOXF6ACV7J!N*obx!yBuQkgWt+m^0yEod+H-k=ulzv^zC)9y6C)6wH4qKF{chZI$$UyO`cIOt1a7He1QE z62sF_rx>xUuLZ%v?3^m@2vRYe6<%d$YG%lH*Es5W=>PP@Q}am)euunGMJMuKh?dgv z7r^c{Z@umhY?kZR>@59@BO$`l3<)&bYt;_f!kX*#I}8lp z9B6<%TG%xIOA$8zy>KKgg!0B_x{v5L)Q_u~FB7Q$wL7DWu$S38q{T&l7tWRJV>1ah z$eqAT%K_z=+7&e`6;F(-{2JIw>K9R7L7-@1-d#aksu}q;Dsc7DZK_LF%Vaqkk#lSu zhHuV&p7XM3O5yhGUs+CEU23I&mU%!UzV=|v;0BC25}1vk(7R+23yH=5=APxFD9;dw zB2OJI%~JW(>M8O;+6%5&;u=B4i{&;ijux!XYQy}3J)PwETt-U6f|`#tT^eUv)`jS( zkBnQ{xen2zV!o0>^lD3IhIDc)GHM4Y5=#!O87 z?b&CjtJhWwY6mG*R&3-IdIgIiJy#?v!sWC`NA6S@GNiUjRHthdo1Uv@$L%+Wlak8j}C z>9Rp(kr0(}+EjtniLB<|r0dV{86x;8-Sv`k@vuA4(v_ z^~I}m@9;dN+wg*Lwe6B>OYOa?4f5_fll%8nC25>+K*7NhX5mM10pp)E6V;ENZ~FSW zXw~={L*r#DJi^7aV&~=DF8;H4RnAiGBjPexZE%FOr}Amd%c{|JTl6!$Eui}-W>Npb z`jYhp*F`bfcC<9^bxqQJlOL+8tcfYv_ASx-SR-du?u3$Q#rveKINJ%$)7%nI&7T@- zs^3<1s>d36eiZyy+Lf$HMRg@-@_!bdp>{&OkB)IxY0k?AR;{k>roQCp7C($X$E(cy zTB0qQBXO}t;M3oZ>+f$mqUc>stZt_G!E`Wi1(CxbNwSK+mdNtD3N}(W$mJ2AeS!LR zExRiF|CMzf&~a7Q`j^!uTaqPPlC5GDOIDqc1EC}igce#zfH)L`Lo)dfjDjh}aCj9wPq zp8C6`17(}a_Z3EGo|@DfQRF?9BWq`{W-3@;_g>qd2Ckn@Od8BCD7n`6dC7|0+ZRs7 z-gK~H--WyU^<`CWSO2u>2YWslOO1UteQ&|W@^i|k3j&Md61Rr`ePn3&=BBagxT<%z zr|&8lTt26D{>Gf*(nov=C9yfDEci0UKlyh5wzh=r{|Fvib4T+#J(o`Gid(n%%w>o8 zBFej$ZdlYZ?~*yIhkxCDX5;ee;;L8bH*{gqhua97ous@EEZI?o$^ zJ97QPzvRc1-{V_d^oY0q^=DBxjc?i8wllu=lHdci39bL?`}E+03Ef!@#Y4UWrElg< zr(O}iZR+yDb2?tB-y3|Z>hm4H>7F}!bM)nDrx$!$ev)s^vU4)mC(VgW9n0;xwmG@x ztl;Xpi`p(2xOMubr0186mE`+>QhImpXA6ttZag?T(A<9C_S=HrsqSp--~Hm)d9g<> z`k-))FWh%|VNAx46aNza(#Xc$QB5aTUlRP`_OHArB&?isX7YKt2TPCl-&Jx(&L0-E zdQau}S^wc}7u1ymw^rvi|GwwyiKFA^XOtAR`|d73vf%HF!V-QlCu4Z9>(4vZSN$<~ zO#KNRFAWt(9KB#p{=Tv!{0|k!E;%gaPtk8qRQG<-a$aqJ;G&xAcERj6}7HkNf(ePpCal=)SlM8QH`cZj~|HGoDtg-p$My(n@ zdhhW&e^m2%AgA{8mN)wb4kjdS&3?J$DgXIpd-D!WyEgvSsieWTccs^#8$7Y@w!`}%Z4I4>+7!x{zovX z;epQ6hD)OEOD!lk*LRD5xM(UXDf!f>wDI83SUAgPw?d^fC;CFW{>P{Wai@7cRwZeG+Vg9aVPiMa6J$I;U69-=A}K+3^*vrG2^4smtOHJ$T|kW!t57_XjSky14Pf-JN4u zaaSzPEqdJlnQvR+Pctq}{6l!^$foXr9VJ!w1h&@Q-5xu*ZO+P+_w!bkA5)Q7`h3od z3;r4N-hmhTPH8={_K-ka^+!#Y^}IKcH237pUl+$z{J{5Q!Stf!gv>b)?~m?!py8I_ zCxMT)M|FHXv^C=7g{Lho@fB6fFFDGAJ z&B3Dsuhc!;zH;!?@V1m6<=2yXS+WOX;25tyjl-!$pWO=aSsIrW_1*r#P1Cy`!C$w#@-B~#mENOgq_sQdL z#GR0_pmQfJ?YJj6Ub(vNH1DY#XNJ#R@KAn=@3V?ErL{R9 z&pef*sPE4$&(>_Je6H%ori*%F4?HtBF6)$%*DId$^%WE^j!l>dE7*TWXHR`w;N-xS z+go-`4CO}lEX*wE@I6@ZpCu11S(|cp^w{`gd-Ha_Q2pD=i>h0ilY3hx?@9PF`^wV( zig@3rOM~7Barg9NgT1?6-+p!AFM)gOZ}6VVkr2Ht?YY9o{ijs?vUo*yO!7$NEn_h~ zuQzS0T3>m5%?aLl|XG_RGwie=}U)6}jWY z;I_&`>VDFG;^2+p$1eC+{yDz>in~h3az6K-%CYjmbnkz*++6dA%A2cx-gH(^(t)?; z9+GuONmE6&Z$aS+i{~c{hNbSmw6m&yTOc>^z3pG`+BbAmWJ2nx1-bqYDmIpEU2;Rp ziP7JTU$J+n`MTWVvxS7t9x9*$f)*16}Yrnjp8qcX4N7>{!wPi;(kA?Jm%+~wz$ zZ_K|p?Pu{>Q<3}5_mCT?%niQW5Z}FVG(Pr=MGK3vE4uyH6wS%{AnB2a10!p8pWHYc zyr*(r?Psm`3_KOKCi$ei|1Mv+{LZqKc_pd+-t$cE?ce1+mE-BkEy0l;@9jQ&yf1EJ z#)ZX?R$SvhXW5@KRwo`Ae#3BG*GR)f!HB?Fb(gnaHu#(Hf(7gIJImKq#FlQzxp~3w zV=g$bqHnV0nA$4?S5^C(_x7wh@bKJcGOsMY%YSuw@6uJ?eD2M#bN8?4TC(HLs_?4% z`lQYm_n#bj>cWq`vGtd}`-`HpS0z_PrA(~t{oj_>+GSPEwL4ob?mPZq`@HXEO%%12 zr?2Lz_C+?fAGlujY@94|jhy(hzl0>fiF4O3y8uE_f|tZPKd9uCc9q zUv7D;Zm{~4`m5Vt9LSwMF7d;xGm6T~-Y#lc60tBYZpqYjgQs=w*s-|wth&WJvwEt> zE{Z8mdoKS^B}bI}I{(5&4-oL!PW&ylKJ6#@`Nfe%k+~UZU(J1d&fv&xJ-b`J zsQ=7+g64JI(ZlP)e=$ER`@aisD(cG5$QVf)i3%PV8tClEY&xyJ%{#Dot$+Pwbo{aD z4S7#4`$xgk*<%Z`<1Y+L^^SfIZH?LSNyC%1#YQQ-^6ukT;o zIk)BQ=GM03da8$u=CscHSNiEW7v*(lZ%sSgyW(`%!G-&4_msEavGWJ5Rb5x?%bvU} z=H-;XXP%yWM9v+Hf0uk>T5en@9y+{ZDkZKi zr6MCHyFFt;YHh;x-mf2>KU&gX(7kxqmR;|9zh?2}#4jU{Pkc3Pedg_%$ERm4Z!doS*-6yM?#GylG5Iwf!3u~9#t>K!?wzrXv9j*mLq zdTtpioIEk=>cqIT@Qm9QXQaN9I1zLHoZn6?81j3+Q1f`_f!#NFzt*&5&UfP1q{J<{ zeDO1BDan6~FNyr)!4F3s?%%)r$6a4`$M^Y$?>^Wab#G#6>WzzTO7Bg{NVqNfy|90J zzsL3A-VeGz@BXUi$f2z9S0a8Hvu1kN^y;t;)9b>Xojxh7%DeA)dV0XS3%O}}nfI6F zJ^LccTiGz;{Z`+A_bYw-yeDFed3Sh6hVA#Z?Dtk_yOk#P8{OQ4&YB!n!{`oZ-5G9o zSL?oQR+89V>$)%B-I1;P!MS&u9^8A*{rtLfoRt~27R6Q<=)P^c*IT-{H(YngvqnRA zZ*%83nyfvm7VPeE+iLfT>;8QG%B&>nE^)iVo$ste;a9%h${)MOUe?=7{Y<<3GjG8~ zceZm6x;!}GJ&R<-!`9X_v4REd+yl>j_qvaqd&%{SjP{F?Y9Xzx{lG7zvj&G%6|BJ# zB0@-hfS2%9Otm(IUuGpT<_G^}J@UZ`+tkveaPK@Te7HZKs2L9Guefk;zE+!Sl$8TF48hmnfP9`dNIn-b3t?MMPd!82hx<&=wGU7i$b`-d8@0j$ z(}?e~2%LFtfS!ZGiW#ARL{^S!)wuXg=E`rk(ghpTT3f+HEYNy9@G#4IwxPA}wXoebK*O1;T>R?v`LdMEGUiP>wl%VY>! z>VWh~Vkn0UIq9QD%|;Mz5e2>D5x!utWdNB<1(0uwrLBlDK3NZ{fh+_yeUhEFPRm&+ zAN8B!{Fbfh#|rwe>Ja?(d>2-OQ@7PSW@g}XC=MJ#3%2k zKfJhUl#&#B%Ud1y6LmdFb*qm5XE9HONBqYA(R ztLpLt)>12Y!jPWkVQjXD!wHyVIcQlT`xrNF1`=R~ z`K-O#PaTpUY{5ff;E=JZ0ncPHTFDoCUWtut!bdS9&zxLmDLi3iCi51mlH`*C(x_Il zQI=wX#RiY5gJPM+Xk!D`(MMQP%?uZiN1N<{Ns=H8(IpEQX+od&1%56jL_`+BZnv(} z@&QK6a$_BNV;O*NhPc@xKbVQiavRU(4|!tj)E2$S3USHg$$oUo0@eu2PdKkFiZ2-I z6TBg&hBjGZ72lZPs(_O~Cdxz0XIaIHVlhC!@g3*!fi>cO%7}a*4~QEQWrO~-Hj~J~C0IlpObWGzNWlhTB*cx!?3E0|j5d-X{FK%9d=$qk zcqpXQCX`=7+Kv#Wy=MwCPlvf;5B#I@OCwrB^T!Zlo9sXX5!5Q?%z4Qx^$@RApXD?S+-;;k%T-7<(9PsBNRO|;ccFMZQ;-6zAmZMuaEly#xY?lxAPF!HW&9~97_KG1b;CHADeP-lh zc1~>I0Po^7Y+^PiOB4b1BR4>W`GQ#@OZlvElw7eEvcf915J8^&(KlueW{vQdm*56E z)Lq4Q(qkxN02aYq>ERiFtdf_0S7nHi>aeU3>xhAHmL<|=BZ_g^d03+BC8s%c(4+mT8;h23FRYMtd`=L zW52usM_G+mV2SNy%A=KT`p_omEVo_*Jq(xiRTaxVP1T>6&E>on{;bB$^3m18?vK+Fyj(jo>_HkTe zwM(_i;z6#1zHBi47EP?Ro@mxLA}t%y4x8jJHi0vE7$!F363aOLRHVVk^n#o5$i_9! zYN{4tuQ)&^qu;cVO=_)N6Wj1#H8R|*dv-2X6^y+$*HTN_Vm*nKSwplGDT}a01|Hfs z=}{lEQuy#v3vJg6kEM^cV2pmSl|JU$;7ULBM6$*bA|&2&UWBuLcnW*T5Mk-qZT6yF zR4ztlkF%v}hN)u+odstmWH{p?+yWhwqyIJ=R-8hu zB#&g1eVu*NQ-YA;iJXzD zmT*aJ>7^FhrjeA5CPAw>Dx#$zoWk`Mr&AW+AeyA`y3uIhL71h$jKM z=o2eqDA{nw)d7pGwS(E%rCdiht%Rcf7(2mE^2lCANBgNnjsu3MqHP&xBdfl;ARrr5wKx+g?;U1T9&FaT zq=Kh?R&T3Z_(esK4qB^+`p9xaM0-p-`;j8c)ykyG5@Qga3ol-ZP$=@|sW1w)b2gtr z!sMDPs{lG9U>b}kpkr)S4}2jee3v{vX)kzKr7+GD6(L|}U5pdd8gke8M$W-4kkfwU z4ZI+W$#z*UFTs%LNsqQ0&ic%ek=UXaxH89azD#*lOHZX|u1I}BHt~0HRexDVjo=BU zRBg$$5uf&0jyGl%eUS~;Ruz~t%XTseHYg6p8RZ?kC(e*nEp~~+W|i6!FWF-CUbx%5 zO#T^q@Jv`yEzoP<8R5k;$;Wq9IK0Fj?cv;$=>-oMfF$=0O8Txp<26OjW)bcPN}sW=}@1^NMy@Su>&M5ugP*+>o-bN39KSx zv+6BgO1I%1T7l##byT^p@r<0L#xpaKclZHY@s?Sm8@Z9hSfEHrql*pP03~@Mt2u{b zvm@h$Wj|5j8(C#nUx=yXlPA(8FAR60LN?jCFJUGrj6ce1wbzvlU2CIy(}LKO$rd?e z+pJldLv>3oqk!d*LpoHN`01`TNrqx$8Dsd135o$c(b*Usi_lugOB>izn6^ zDGW!iys-JZ?rGuKlr}4pbm4-p97s|A?S2V}{9o8PG|7|*uLthf3=w8?vz{(h%}PXS5JBkiO^`-qCYGpf#h7P<2Xrb~+KkruA4iyK5$+R;ngToj3z^f^q$N@7H& zOQtxbGp3x!BnK4@%WG9m?9dkKkSdivnWy&5Qnf?A@hpRiMk>N$~L~K1=;Fwhnp5%w;e8- z4PYUiYDphgBWPt=CK-HI4CD`2)cLJ4tAg+suhfGhY{du0*~m;yq@ASV52K;>=_Slz zDc|*u#nxW_ORnaUuo8UfMLdls4pZqW?IdcZ`V#@ zgT2@nItHkh{3H(eq?m)LY8x3MkK{cl@~&RO1gUTwoRBBYaP`|~;+_BaA`GpMBw8J@ zJP1WhdaP1lPv~A_9#xLt7IO1VVIzO+TqW~R^+%HaTP-qlL-oN=L!UZecsrcokG(5i zU@ZRHohBAF@Ubiu!sd-IvD#ww10BXr0||?c@L-n1%mDkDA2QRS z%5Vh`ubBU6bb@gXQGCNkrw`@{A+q}09+216Y^zLg2d|aCjB1t>(k<4|%FSlo9bbw# zEkM@rQy*TkUXf$&XH`kI3vD-6QN8)4ALMiIt@w{Ul1=oeuHqe0xBDZcixG(!7;eN5 z%$-NrVDXIg*iA;kckFeHmTd4LgG0YrwwbS{T@j!(Q z1loL1m63BL%mEC0wZv~~24g$(eAq`M;f#=Sm4Q8ax6y#viAHSA%&Fc2LmKP|1s;L4 zupsNDm*@hXyaQEhuUy3n*(IG0N$sH>Y{yrf0dN&btYN+;52Q`L@R@9rRHxmY)v&Ws zVifVgUqg~_ZZDAq9W1c+^49DGNf$qHka5Q5UvB0gy_VOERA@8nsl=goDxufymvn8z zXKhCx|K*kWjvlt)gK4KfHN~PMU*(k$1x1?+S~Z+q{X?sqVkZ4yB7Sg(43V`_g>i?` z6$UGBuu?u?f$>LP!VmI;`HA$XG7uSZ7v|fTW;G?0KhjLqbhQm1g@{E<83wwt+gS#~ zsc43lu}9Xr2$CI0mY-s#FaaB5lN+(nZkjB1YE2&Cr{$9(BV9rTJ7AzjBl}M7qtnI- zNr5NE0z(?>$XjfbMHV5B=^}nj>?kR4B+?kG)u9OR9;iJ z$Tut^Lh6lu#7a?P?xN!`zKJDp&zLIRmb;3HtaH^(J{fM3Kn>(B31(FICOceiTHI~C zQ5#)#V5F7>)Mlb$@0`&9H*GWS^V@IkYJsG&&yd$QXPrLDBirgwh2e&du~)2@eKw|B z4sn(PHoy|FG32NwXafbICF$5@c3KN~N(6+d`G=)8=73$OJlMw;W3N0RO5`3=HF+j3} z6))-F+@rD?&%}I11tjrBR)qRvNYF+zak84(v&lExsWWaiYY{eV%~RQg6|I6V7H3Mf|ipLh`pqF=9YXF}~1G?_`O6 z*9_9+d%I>vWM)+4KS+>E+G0GliU>Yrm?1@`t15G+pnQQP#x(GdG*{P{p>U3v>uan~ zkyKax!I69b$IyJ2X1Hf|%U}Ij54^W6VnFCoo~wfRLSL$%!;_;IVr}pEtXigdtfH={ zh_j4E8i(y@MLx(QvPQ@lOB7>S2*<6jVXUp-CAq|Bwm%@PEL02q2cytx;3@DB!5V1; z8*wJI8e*Zi!4)yfCbhRL;xZ$dlui!5}Q`WCgWc zQH6cRMIvnVm}rwr?)*E9QFXNASn@|G;)85;BbMwztG1#KdtF^s56eBtCq`5U$0N0& zE=!8AbIcU3l0qN(ZoHK)%UvQMY=r?#p^DpaKDrHO$5J6>5r(V8Mv~;W_RA`rwc%=- zu8rE5ZTjg6qS9fn(8^#@lqE2O_Ocy+@g7UjFCVcPTPz=KtJ=?)wmm!ui|p!peiG@`Ge=owPl%lxj8Qpx0#Tuv*fB}tj)H?79nj>l{~#(xGt$&V;rjt7U=V!4{4aVFtM%iF!${?2~^E0W70R8}qUM+dP#X z*^ZAOW#{767O&8Tq#3m{7CN-e4(YJb*&T5SY0hd%n&UK|Ot-UyHoS*=g~!rAo7IK@ zH4iLcg{y+{#n4rMNr$WCtZb3>@&vyv1NC1u-m0vyRa?H>c?|O$)(~rCNgkQXm?Eyo zVzYtK0~S(&de{JV# z68w-o@DJb3D`UPeGTz8Pu(Yv=IR+Jm{)Vo&ftT>wssNmn=k`h3XXD`+G=r1yH8kBg zC@&Q&^MtlkMbjytiJT>=9LXEvwVYO%H2fVWr(X0mkltOGsr-|!MY z3?a)s>an7y2opn#nW85DkgPbUhLA6kLoYni9%m2sf(`sclEsg@!Zue2%~vGR+w|CY zWZ&hL^obL^^bLHO_YhTSg55%ybJZ|L@8+S!ME@;j^hcj!qFJeQ$}Z=PKG|GI_gUDP z8X_e;)q^RFW-wb^` zRy@htP}>b}wNZ`{18t?P;>#bs)ef|+KF9)M#)unw#S71^hT8neagyl34zSW^G=Qx` z&@ztfb`@JX94`9IEM5Ker*~=wb{ika7yKcjhNX?xun#Fnl?28E&7U;qApVRcm4hSh(qph%5>y)8f&Fr5z*fN2Mmu3wEzGB literal 62760 zcmZsj1$Y$K`taA?-8ad`6B68=;!xaO3$zq>cc(4x6nFO)cPBzz*WKN}narJk!oA=2 z+3d`b*Uy7l9bjWq9}Lvlk{X5C{Yk`0uBy2*lQrNCXPOKun#qa8kqr zw#VWqL_ChHd&lBvL^O^;gyUG?jRE?Qz+AW&xCE|Izz8((318KZfqKDvXa+Pg3d{g| zKwVIK{S_n~8VAW~s14KrOM`TS5`G*Fw!?bShzMX=4A2B#VE{Wp`9Hou3*hnfSD+p+ z3XsP7s~_is+97FB)L_qlv=Z_F90%Eosj~`<8E9F(^*?%2ALE8Gg7O3A4zv$`!&cPC z5!41n7tSU$7R)9b3(zZYT*DXSZ^Jl%*@vPCRRtsumJUm(kHr5$*NVaNOVsHPj8t1@C{X0q73O8ax8F9LhT+A6k?3+56ElINy+7NE+x5JPXmlMtBU| z2i`+7>iqz(V9%j71X}@qLX}uwFYr}E??K(*I5-lZ4$xQV9*#Af2lx}R30fodUereo z)&$vCZxiIhkM`GVZWu>+B)BHP9SM#CTu0#8hS!i~U}iv%AdO%f=qo6pp%!oy_=4=M zUqRr!`te|I{VYgoL#v?Zg7$%yz&WgMg;p~dOSpDmd%@^}bKrX@g5Vw49{bNu4(0{2 z5LLHMpfv_%2lOZu$0LGuYY6nPejS3>VW2$-^oQ!64A}pS1;>Aj{Dk<3etGrr7nUgYlh_yxPhyE3akyciBr(qaSY;FQg?DQas_E9 zX)TdYAmSFHmm&s4hlXnW4(}^Zk_X}W#dE~_z+V(P7~6nqgo_jYC9NVaClkoKNH2-Q z3H@+4(5Dd#qLV}K{Z-ym9>4pmyO(F3_m=--=ydEZY7DL^(L_2;-c6R1&yk)HyA!0i zndlw}Nfa5H?cePE+4ItU+TGc6*c(P4ZiC2=oDF(>Y;Tpj zzq_;B>|X9=2A+m7hz=MxekUnJo=0g$IS<5Y9dROlF@}qr7c~Wod|N#P_aN7J*9iAa z@4G-o6oI;k`-OCm@|Jp?%BNP69mE{`ee}opv#{U)%(KhY%PDe7T{ApM0cWH;Y5;yJ z*+x~-?$cgSi^xdg7_0=jJCYda;o0O2+h5vcPKD<}U_xvOCP<{vWQ^&IU+K%K3rV|h zMx-g?^`CS9=`h%eZ3`R@H!|2SuESm+pP^4f$-D%8Y%6jfs`lMgVSv-Yt* zGR{$_5L;np$7TokZm!*LDYqDHt=!Xt6^Lboue5!vHSE5uL-f<+OSme;%}{{{;~-gE zS@v1`I3@o1vEOjZsm)jhwt{_^Q9?1``KYhqL*6A0fz@a>T4p&CecPi~uq0|DRt9G? zTgq%rO(%3im4-KZJ2*aDR+=wcCOF3W@Ucm_8fqcyFy}3s&iqPQgYSjB6B_JkW#?Hs zm<^U}=N12lcqhUf`dfAlXD$0@#(i=OyDwfEIOXzK$D4bZpIZxEvET|+0jUr3D5se7 zm4%@@iKXb==jw2S6>7BW!y@OW~Eycd3irI4RNX|B9Hgyl4gG>*dcQ3NdGJi7F zT8=m`2f84wL?z=NP8aU)>}&KRr0JOL(c``|j(L`2rtxN*4dW$8qgVuO1A965DCZ$F zi~2MECgNm}>vmdc<|8JfCFGnGOh$bs9b|I3YW!5gF)b;ZvTcHjC+=ak06v%<{5T9W#rzlQgD_Se7{_r2T7=0M;pr93>$-YW)VAe-QdFEKl48!%a zjm_*$0(Vem${EgVK{o-8+mZeS|64rln``$Q`|9O}e=VOqiSg&eeXQsF3c&{6GUiND zThxQ#ZRZ5@E&VwC3sau+W_UbK$T-W33a;^gWt%8I%(Mv4lVM$E!0S63TiZtksOUaa zEcd)%HlRUHUy0|&_xQN>6GoDrWiVLUd!NT=l2X}ef*_zFmuVsHMd||{#}iY3eRDm* zyxw&^asV%24(4|ip5PYZ&QHwS3pV(=9X-9hZY=Fn4JLUWRb7;38)*Z8YwF zG|gLOePB4H>uq>%{p$M*nM|3^c_bhTF}$9Piv%HJng6Q2+Njq3uGgBGp6#)5#1$+r ze~PdrU(T|U=A*6$*EoshX8QZOMy7Sn>)|`NEer*JgRq$(j~%8wz!<`d+*2%%^<#DK z4Sno}017jn7UV7xY6ZF68rr`&N3^x~g>{~xyY9PwuJte95#(6PXO2|3K-iZzim`!k zEw1rV?VXIj>oWBO%io@P@j;})>;r;3!uI?^W(sKra#LW41816{Tcx{ddgfXkJws^C z>L5Ugm;yIzANe!tTX2N)s_C?jr8{c;$@w(Q!qb>9`E1c?L7e@KBEwL_3tbb<)w=iE zSB7ePc4#X$kDkOAiqeEyj)6KH+cUDqoos2PKdq%1p4n~$_?WY_UwIhOI^kt*gtiqo zCA!vAZTVBbOgmnG()yeKE9xB8#l?t%!U;SheHFe(te1CzwLm{cTcx{Y>F>LTd_)<` zH3-{^g1p;|IfPd6Bp=fzGmO>F(v_Q=dbc6&lW%bz3#WG5l%D0_`?tH{RBWhIg*$j|<@c7$=I_L+8!DcwCW_J%l--AVXHv_zn0c}O3S zs{=?!U*mG^T2vu((Fl>4=cM6q$0PIIFU+%a+caw3XKQYt5?x7)^D0D} zMca6L=#O#Tqlum_mIu0nn(aD)b*MjrBGNYT?uf375d0R5#rQzO;bA|8N8_81C%D^7`M#D-?4^64zg|lsxN$kxo5Eh7^ z2-7$!@+{Pgz!Upv!*q>Iv)nMkSs3}3KxcmuR*Mb7Wt^Ur7bsQ`<9KcOMdMSqH)tF~ zBfSXES(k-YF<VtZ`V@!A=9>E$djEQ%M=5XgxUFgOkpJTl7pyr^uoqngiJY>h+Vm1>BB@aXo zxJA?%n4O`YoZF2THFMN%U9x?6Xf|#b6DJf&>>?JgJ?%Zl6FTPnV0@?`% z^cA?%5uUril%$QT_GnF(oBl#{7kU+6EO{?J&Tq@G;7HNM?lf~JEkfN#yV;WA?~Q&& z`-6{=1jRM{LyQUd?NOzBfq9&!D{Ac!i?BI9$mDn59(vSv5lQ%!Kz!kUGkE-oN6tk|besRwD7|_*(B>OR}y;CDjZy&GXDejHFi?2v-3gv7*sSjelZ?1Kv&Z4@b zUS}NP{wdy_+@0G;{Ex&ZT+H50dXCWgK3L!COsXwvk+IlyHTEy5mLnGDOB#y`*jREi zNt;NNTOpeLytsrwo3IJZZS5?P!o zkzev&bedx!C!lfyR@)3cLmgB_^-Z0&2%Y$zeNPmYSVb7_Ov+x=@j!R`4SkZjTy;!; z#W6jyi!g+}PDGZbipOyCC`dFnc*1VfcThi6b=BuMK8JAxC98voAe}9K$?Zekh+Y+p z+M5^#sduWXbeHXI!aML=SPD^=^o%%*_lD|2Yl71pQ-J@yfZW{=0ix}v>Mi^Q&h#;6x)uV z58I2m7nt8$(t%H@?os1av$bcep1=fbHDi>pophVzI{#04CH6{K>zrUb zrLI+SwH)h|Kp|!sqmi(?v`|73AQ-K2+(;|eIpaC?9px*{c}tCd0EWx(2zpARl5v7r zj9a*+k?k(Eu}FPQxmc5K8R{=aAEciXw34=#-WOCcWOzr!;A(C1sIMyH~q>=i7pDT4qWugQkra@h{;|}%#K%kZkf+$+9~g-1{j0xl?XG1 z&1Xux$tpx!*#Y9ocpq=f{7}#* zHPukznjDXi*YY%y&9XJ(SsW{A4I;^R!ot!fDK$!?zOSn~wuzj|E0mm-6^g4kBgiVm z3ZKc+59q(9+@SyB92O&xb=

k1~#=6ZZpoEYj?2Z(XiURIXP_^=+JwqJNNfa?eUC zWn(2Lxos&QkUji6toOA!%D&1sy0?yYQ97v^ccsK5+bxN5k5k&C?)!CCv$m;Hs2r>7 z>9`izMtsldE(yqrCBO4z)T_V>=xa;TwNqBrTD5oVX%RMYIVVRVl{b+pdAq1A^!mVl z+bCTx<;~h{+HCv2@KJ({LzQI8mq<&ui%cbhKt9_SIQ~`Z|S|UyFw!! zYxUceRW+y7mo2(LDy}0dLp)u6Pu5J>it!KD7rNnir~g~|sAhz^rDam!0JfGH6wQ*` zWPb{;F%ocN!*nOf@Je~ICaQXBuJI>fe`DSO`cvg0p^SMLmlyuU`IDhYIj`o3D%IS> zzZb(|?ia0<{~{kR+QVew+eN-Ow;MFd?lsL+mrVD4M9fvjSkYhdtMbnx5^FX7R7BzW zXfP@>Yf6=grflCK^gu>4(SEsBo*|yi(&D|5$*zb2t)ka#RX#HQ>rF)~=_cV?IW1wm zSj8Gem=rB?^)Y6uEY(ewX~snFKGYogE#V>g_yn|M7Q2v85zTb3Gd5Q}u6|y7!f?Pt zK@sR{h5O}~5~fN_?9RmYu`TWo#@?#y)iY~T4N>=g@^)#b+4XUb4@qRMMcb~o)nyyZ%vFzA9Ate(zwG@V!EqJs}|K{>a@=F zaSC+;F#lEJ2$_Png**^3$}2H{RdK4nRNvN)agySPDXBoeC-Ii-63;7pTkRoqW{ zE6!xD#$rN#8&~_S>Uc$q+7i>B0UNF*r@!okq9#cs+0OJ~e+x~uPu6;>=2pC}X=(EK zC*j_)NwSlQY{dZyi!}$Q4}G!U(^9HCR!pk7X`JBC#|~syNzW_RDDct)tSa2_aAOBj zC$HvL1go=*FMM6FD%MfyeZ@<~A}N(U5&tQC(9uKJqS{=(r~0g+r|%!kJl1&WBZW_4 zk*;Rv;afx^j=j2Z)nCe6R`U#(y}1}POC)`v7@XW+7G?J#oQ{leR_P{G|6TsNYJ;BX zy^cQ5G)P`6_9uUnZR9*5@S-oB68);`1?A(a$ofs5By=kCk>rb_GP#}H&uK(l6;-%q z>rYnqEhkqk*9F}BP*)hgNWLnPQ_jlga*q)$(G9Mr`lr>c%THB0wR7EMR6B-H@=!4< zg_RKCrjo|Q>@KpQwpvi$wQ`=e(6to#j$R>trr49RC}B45FzIV-xO;+uQG+bkR~R&{ zT~5RZ`f>3?#mAJ|geo4E{8Rj?`;wt)jlS%7#SG0Y=X69Rt-tu30-xF=aXfz$`9eI= z!#0ktDK6_+;Z$chYvQA5QPEaKV(Rt85%-?F>b5*RAw%puiohx5I0b7i#95L zOO+)J7R;h-N4Pz=jsMi#D*L80rLIYOAjqSr5Tm^)ldk4M*@W_i zs;&0!u@HqVnxS}^T9njWIEXqL`Hy$BDWUd48Kpd?jMyGW*Hbri zXr5+O928vxR#?CX{5_=hud*g(higY#<&g;Kx9$nZa#VvFe|) zk`jD%E0ZC>CoJM&lA>wdQXfc5nU}E7g0-{1@(gH58gm5E9wRkkyje64U8?!d%$>9f;@%h#|w;C~ADv`<%4 z%eBSA%7^-2efO}(If;o~({H6ckbBsFrLA?0G={+Vo&T8*VMZ6UlRI)hsUWS-iXaiFUB(Cfdi!0p!0-ACq{Q+n+cnn(3_2 z>??0l+^YO1?JIW&^d^>FJ~7>welJnZyF$Da-RNX%FP77b^UA(!hP%I_a#;`LL(-dP z$dlIdxFljs?Hr~3R*ov(Q#MRfP8z`fMtTuD4eM+il&0Q-DsL0<9hGwi8cP;{}ltVVC06dg(3EV-Q4EK8Qk7B8c9MY+5N!-vX6 zMI(!s)%<3~L<5xGlAUS8v&N*(5m(Z#qWb&NjX_|3W-+}cXjvcGPVtH7rftl6lA0%O zNe`h;_?8%xsumYniXK#NvM?h}Dd)t))6QmL(^^UXr1wMneBX^@f&SY?{i?Z^6XDzB z5#r`)_p^RZ+avMN@1TeHlT3T7<`u0ja##IrP6_`+CW@tLwOPm0!jgfEIQo_!_zMcq z->vA6suc6xP$lWQD32{XVO*);K+Q zmH0)tFSSGVv-E|sSIp5^f8dn)a`oAw`QJsAON`+_XW|;+iqwJG#Eh4+1lA>NkD$j~ zP<^UsoS(f3t3Zfj^J)f*P1s)@j`Y*Yr{i7pRh{cOIe)#DWgi>nEfwqSjb@ctEQ?*U-(b? zb$yn98=fP0m~tRnk=ZHXG&=!*FVxjqT@xs}QP{V9p#F|eg1^B(m~trlm&|hs4)$Wa zJM@RuQsXXKRY)s$>ZbeN;#Tulrd-b6l&MPS#CeAw8m_eVsTCD3D!g5`SZDMOz=e4^ zDaW$EW)4cc#mOQ(40pC2sclz0qHsi6Ks(x7j6K3LC!ffsW<5xhb9WNBkwZ31?bzZ* zg~+n4TBm0ywlVK!^6~7RS?r`$Tn%AGgk*26Tvd!Oyj{xD&hjWRpSiy$pUU2uH7iNW z9Yp*VnQY&!yjAQh7*e`lV|P!-jOI26SMMW;T zP%x)txO%^fg3jQ~1LjZ9E>+Cte^!G_QSw_s`{L`$w~lVeO{`%_BeQcFbxlzVno*9&(_9vH zY0031a8WPict;^Z!r~-#%^uQdU&?UdMoJ*Q!_`MaEgeyCt0=#=%sw9Rkoh_hsNY8R zl$$~$We`H`x~%C@I7attB3iA> zwmDwIXp|VqDsHqdb&2RKHICTkPSc(%?O0&_exhctjTK+PP$d|$tc^-jzlgllUPzUD ztG2wfTfwdGJ!{0ab1^RCuY|8ztelLrH1S~CIb?T_N-HbtRO%a>*?&f%r^lnkR^N455T)jcljS5R8`qH3C@I5LEmE$@`IGiOHnX~|7G z27SrfQBNtKT5z^-P*u5kK%|uVQO3_YmUBOyC6zH|qS?Ni`o87U3Z@s@D<_+a!qcf! zWX{a%Ii!sF(yfd)=y^V*VQ=}0g51JAl>yV#u$e-VJ;;2MGc4nqRLu}$zWK%)%F5Ri zUls6l|(EXJm&?k|UDrOjK@Wrb=dKreVMMCmWAeyeL>xps46%JR8g+ zpOp|Y8M#w4JIK4U=3|os*Nn9l?+XSLyeKa*v<-eE^^_E3D01&+?v)>7y}_;ykW9&y z%7Rt}3(Kb)-UlX4PjdM+z2DyQij4KKJZkku=ET9!|%F%`q0UPlz@v@9gxhYxw z5`JdS!8H#)HC?V07wYq$l>MRi`nM5Vi@RiW%$=5XB;geB9b{dQWcE}xDtwc_ysVji zt)D@x7O^wB=ibf=Bt+SCd`a*(bEm2vg(vfq%3kYO{yT(uqOa-Qa%-}BCXVDx#b<}! zn%7p%C|sMLUpiWM#@CEM5$#Rylbe)%D)AEMK7L(@Z+TO-uJD)q?WIWFPrefT72){w zfw?2IBZ+7(mQWa4U?EnYD;$#FvGjoG zh=%YsE2BpCosr+SWVmLfCx&GR2Bu}?rZwuNSi+k}>=02~XVmokZqADqJJdqY8SE{- zEUkU+ghqQ5w|EzcDloZ zJNREnSE38;pK2LJ&-2QPI;h&X&Y>T2@23>zNOPDeLj)OQe5}--r0fRt?=8w#-fA-9tn-2;_RUIC)%-HRo|^vdBgG9)IO@seUWIl-K9` zo0>)TG6aMDRFR!S%O$3577eFnBGO%b)fa*J?C;}h;?hs zriI_CnpoGzZZfoqYZ{%--JU*EBB6VbNRM2rEy>IC6s)OwZ^6e}GqxuVY4jksEd8P6 zH+px}XwPD8lhUfZuLb<7(U#rOxAf%1q()zJGcp*`cl6_^S01HyQE52uuYwnq8gp87 z82wIyHM=}_R>l%(8Y7BI@cyKGUy971QZTA=k@-yI3$1s;?QB=>hnYFU*%#oN2z8XEXd|v*ed`-*@mJGYX zf7kH1{8j$C{1)ZU4R3>m&1b7xe=jI#TmD3^@w16tMcDKwje2H= z7)!0nKAZ6X=y&1fhH5O^YqE3KGcTqcm(kcW z@t4A3+W?iKtf2Vs%BhC0zBV`?SDBQVflnJM3$XU!>5&`uuE1}3$4Z)2Z8seEy~Ad6 zkx3oWho_cFzp(zs{~qb#5NmK1i%K6?Ju&R^b;ge1AQMNV9ZT6Ly}){ae-W`e5L!g# z$g*zLDC0C=A!ZAkp72|$A(<&%&iaZMMgMe`Y7Ld?<=?7%8hiU@U=zysx}i>oGdL81x*5LUJG}nm9oGl1U)^6jQm=jZbS2 zSG81*HL-npC_R0K_(ft$;uXG+nL?o{m47i zRe~~EwXCVIguy0^ij#fwEt@r|$~^T^v)$VpNunO-8>KwiKZ4DSAbvyqfG^v+P)k+) zp$VGlJ}hzuhqESGCuUvxt1R5#?kRS0tG)*~<6O4R~{W zga5Rxi~h6PqJ3}4@vT6#A!E4-;xl3d|1A9*o{tFlN7#ji%bN4LY-=}PKg1PMJ5Fm+ zm1s7v2mLL+Ct`Ad?!XuiXh-W$SjYL02nJ~ayQ?rPECNm>T);0!d=Gqg)Ed|5I0lw& zsqagC9q|}zl%Tn=3HJ_dC;kYcd2oyKxoM{UxnY-WkMCmKLwL=c${#D(#Ti7Kfq#S8 z7i{L*W*%nPZERxy?%NulPhcagaW=vpXCT~hO5NKNCnJ1uBElah0H0tR72%DOfP1hF ze1m6=F~E6B=?6QhW_vz*|WORyap3tq$LE5U2<+$MM)_5c5Z z=YnT2p;NY?ocfch4bRHLCq|)DrqKCPNbi5khEB0IJl|V?@*1=nnhT#zg{+4>{O=R2 z@EK|7lqxh5v=#CTk^tHZoqL6>g>8bhz&?UDG?Wip{o|S2hB9EepvBNhSIB~fmcyrJ zAqoMS@!zB2Sb<~fC4lSJm}0Z6m>`wlu78+F6=E74fr%K zbn+HF)ec1v9`(PU;5@iOV2R)vXNWq8G$1;ld_XaPEPzj6*H;-7H7MuMNJt}WF?d&R z>5mfOY=Et>WT>hjyI>jcEJ#+pJy3uB%!c|KYOR;nP!i-bv_jx4fLX0S@ePlz&lO}R zm>GCY)c4hUQ-1|n1X~Ez44hFo-|%RtHb4zfPT|Nw)ect<*b2X(xWa1*q;Y_UHjqE~ zyf)HFj3yudU8=`zboluQ{qhD0=|bo8%74w1jYp3W8uB8 zenmm^;CO@W&@9;2h8}^ofL1`3f}a0q6*vcqI2ZxQK4=8Ew%|1kQ!e01f3O#<9Z(D8 zBb>>GFR`XP(ql?Cbl?}`Jx zY4~a=A8LnZz&8H)#BNykklnCNpgw5V09O(mDX0yOA9M%pDDY3XCc(M&yB{1Qcvpi~ zGZX>PvW8iQ{ei}Scd%t(-rZA2z6rf53M}RL-di@HlZvl5*D3c9+ z`0pBl_GQpgP#-9#{;K}j56Xg~1ImZ4p|OzXa2se1d|iJ9RV+-~)=K~-f)xPng3!9G z*9fhT`nrMqY8YqGJ1BP0UJ6oOKdxJ-AB;|YR-hdM$_%8f{^2o{#PF8B^+_6ZXk~u)*Ea;w9~?Qfb~OJ`LVUW4b%%+2Wf%) zf#MFupgv3R)&I_c>H>-YXc<_$@EcmE|Ggq1PaFP(Y=V6O?S-xz)-Tus$%mo>j%k<$ zC|Y1l;p%|u0j`Sw-nSqfpbT)8g?}LbV95=A1LeRkcu#HUX?+bqqagX9Wazsjw5!%j zhHbCUK9nc09w9%$Z?It1fOpXME$EpAMg_J8)bii!7}NpFg7v|cgP)MU^`tN?qv5wu zIP!4)L;GPvDbO_(A*edQ2*7m$>IXf8Ua-ATOyG>bHB&ziZmE}7-vZ8sb5(CUq`$uE z>Yogd<*)_ynS^9V>#_npCm>tl%7%Ui__61I{f4~2J~wQa93TB9-_GZ^W)gKSI2ZlEZrgpMA@{D$j1udw>ic?l%N` zM8k*!02#cGc%2j{y&+8_6%);bG5BWK4XFL`KO&!lyZuJ57GQjbc)oe$zA=H7;kI!y zdIye1JXc2sSCWPRWUzpM$1OuILJR`P;A(&je(6c@pgq(6Lk91QZ9!$?ki>sUE6A(K zDDqy?Q{pH>Z`^J4--yKk8T`>-?mgr2xC`AqJR7~&05bS&>=tS)t}#FcpCRugCy~zr zEO8e?5^e^%7eX9Ggy#C!dk1@-y3e^g){(*E!c*fL(JA=1#F6A$fDG;q%s5JP;tygk zqo&105n=EgKn6S98{DVd(>*D^n!ult7l;p-W`yme7i0`T2Ir6$ks1SJ@I}mD$mTIs zh#fF^XL^piC%d1!i#$%BFeCxU;MTaV#74k)kPJqWIi#J03Apv>_K3Xykij+XzV1$L zD?kP_05TYf=z?+L{~!g)b16*$GPn|m)CBxW3=O#~st%U>HhDzRYkTC8v|tUhxm)I*Z<11)z!x-bV^*mdZYngqzh^gej?dfM+UzF z$Y2C<99D$f6;S|W@J46M{?wl2Oz>O@jE$|p1c>B1GI%yX1}`G*z?qPSh|hl>AcIY| zBHI#&t&R-V0%Y(x`V?jg^C;a%nN0kGxf;(2we)rY$Y6)nY*RTe`WyfmysC~2Ca|tC zT-27NL)dnR^`Wf)kinB}`|HTyTFfPY41UJiUq=Q{BeucJh%F4z-4eUoQUQ>`Z2&U3 z60x+73|`Cb!#YeqNxp_FL);C01IS>qwWVdhwYO8|pBtNlTSjgEA2PTMAcJ|R{P02V zVu$btGMGrsWuG07 z7k~^tQbz`3=?-E!Iy*WKAcK!ttftrI-u5=$)MyFzB|rwt05W(dGlO~%Pe*2j&bk-d zelveIRa=fZF9Bq*jaUtk!Ckq(v2Oxo@J!4WfDHZ{AcK#aCYa4Of|nQ#VX?GT>{Z-@ zoX5-*>R|j;#F-%5?EuK&!zQc6>zoplqrQ>$Gx^*x+_vmqbQSRr^v=i~Z;9P){?&BG zyb~aUcL8K@V|p@Y4|f2E$!tSuj{7;@DNy7j0%Y(5(@E=aclYqmn9l$ie4g8*jtm}4 zIES1KFuO8afr(>kZE5Yi9mq$1BULaD0A%nxfDA4t($Fu$r#(+>M$;qXLVyfD?|*_I zkhU@Zt;TS>PN}M+T2(zvrFheP@5B&Wb1IS>fOC7GnmeZ)*Y5cLgd#n(} zhs}uf_pP#Dsw0Ct*{Z!8;&X|2n7esf_=~vtj5#DT${6bH7Fzom=NSOp*7-50L{Fqn z7i1>}9WS!=Z6BKh_GF}j$ zBF6z_@ESA9P^2GZ9^?v!cj6F?VZ8lT03jWSR!aApd6 z3een6^so3i@qllh-D~WpR~Y`WeD%lyGI&4h8NX7nj<=jSi_`}7Pws`Y+T{puQ>qp-{WEy1x z=a~R4B=UMN&J)ClMgH6N3Zqu{tKMj)dUnUg0A#R_KS|h{FJ)Os3s6^rTb)RAYyBNv zW7Asajqpv}c1ALPqp&eR28SsBVASEI?n##C`q8>ChVFKA0F4<N+V82}l~5uFjl05VvDA%_>cCYY;qpR~^mN_%E#8}=JLh0he_2&*_|>M(4N$e->^ zOB4MCEeRllZwGjFWH3&&MtFnkr|razkFNJrSoY`_YbOE!PB+j01$CC{;UYv);Ygl^ zz5?GX*4I1RTB;wetzS_qKA-&uTs49*j*W1l0h zK{XEUb(}Pw(6-VxF|Ky5iwFpf>d4?UA&E1F(jTo4X4R3wB<%^qX~*Vp6Z}%=;713u|57&Fys;G@bOn*#d!QnBDX_{HSQKNXE0$sJMfXCGIExA%j0! za{@I0864wPi8hFK@cshG;GR*5r@Q60?uce*9T^-&k!YI%GWZU_UUy>5$A_bDJZCLh z-C|9WZk1)TuOn&^wI%O1KnDN9f5s33WNtTDRU#wwirF{}|nq-2L^K6fra2Db>=9g~fRH2c+^^jqxJAsg-{v$;?pxi5OmEuhW>$l&hI z9mY#_WN=~~89anZ1o}ZTxE<{S#uqwTM+Og9-`731eF4Z|3nL%=-$;nZ@-EP_u#>_F z7uQs(8K7RO>uehYkiiQWs{~|82k}iFj(!$f8XoN$YSL@ks}%ry{v@Equo;s9vUnjt z2G63ia2+D|Tz>&%umB)~M_XG3hGOp0lLRS}GXNQ!2av&M05bSHKn6!t`?Lmt3@$)- zrC0E!k~iYh{N@ZN4ijDGRsdu$KpbheSkmjr;N5(*#4lFz_c12nH%C?Oh35GH8BEd= zEJu9zP`zk<`5`f0(n&yPmf?-j-kyKV`?OlsJAe$P0%Y)4s*|S{H z!ASrayuo|hk^+#w63t-KT+c6vVH5)Ip!k4fw(ufA2EPZ$V4<};Kn~ZatBkeos`$76 zkioZvQnrZHyN(QA1(3n_05W)#yGy(mxhr>|_?{#TROp`o8Lakw2FPHGYNJ|gEOgz7 z{X!*Q>gno;%CB$hG)7p&w*VO&m6%0%?lj5~)PX=h z`&E5{x>|Kef7>xJvWGB$y-`GzrU7JdJ_U_t1pfxe;I`_=s&@LO02z!WXj!d+{@=wf z05W(BdU-HzZ))hT-mR+CU9-0iZwJWWBvFR+yf}sTf$B!RvR3i{yC?{ooVaJs5g zn`GM^^kVxl{}eWt_LOu2$lwXsuHkLYOygm7TsdER(i#Yi!73Regzcr905W(ty#gSE zbxW5#A03d@~OIu3+5tK5L@y3Y5)yx!BUs5*FTmi`7 zcIZa*SwR0(fD9hSJdPg)kipAMbj@1j7j+ABhVL5cE-g*a4j_Y93tuy(gzD%+cb+Lp zGhVq;eb;o%D@V1b)$&_NUrWCWo7IuQ$p9JLOEVZCgIfY*@L8k8({L zuPWE+zX4=0o~-6Rl)RD&CGEJM$fJ=KUq|Z_fE-?@lK67|ThwHmZ7lmg{AfDFC?kij1~KS?68 zLdjg7gnA9tB+%QIqHCp8)|#~U?5V&CSjuTE5y@LhD|mlVY3MD1eYTOho&XuVRolqE zA0UGhI3!6U`66jYK7#fK+6<7v+jWDKYiqN$S8ddA0^t=~DQ+u&CcVO63Xs7=f;|8- zcq~8$ztSYzHiS;%$FXmUN65ocyg*Bvj(HcnZYSucD4W;L0Lb8IFbR)fuM-cJca%*B z$l&jo^bp$7UcX3*1<2rZ>%8Dq+);oG9xq=ms}Z~b$lyOigB+XnTb1QCr`6{zmVg4+ zm6a;~Ren>}yp9YGgl;+B>Q5^l)QnO$w@eNk##S+-qUmy{Y^U%hLyDUeCOPSb*UFhM{(c6)15sGh06IgM^(w@p8h=;2J@h3wS0zrf@mL;jPDY8 z=lsK#`3?Ce5g8zZ&qPvO9}E^{T1{CU8GH~ufYDsEN3N5n ziGTkO85}d9RE(O<%72ZIycy_PfDB$IrzUI=D_J86Q=)|c8JwjuRX0&)7$x3As9)*# zgnQ(_Bt*n>*?Dzj@Frtp)${77wI>Y+Jv0=az6l_MFCLJ!FK^NSPhWDGgMov*|nDdG8hYx!8qYQIXf{;`iawo#E<_3 zkiolD3+u>WD?kPpQVRw9$l%OsK~1)<*10K8qD~eZkiSeEDNE&TAP+%|@Jh|!RLttU>YLipPICMZ zB~7qL?oGTSyUg>GA0RG!Cz|7RWN>@!8^@Md4nPL4l}i9Jn8%+>NkB%tX8|(UQY8b( z;GY08_%T2RuaZwoS|Q)XFQsfkj`bPLS?bqS-vBcBg?%eP22TOV;EhSOaxcFx)c}ye zX_f)%$5nf)3eaF42a>NmCxk;SCfyb<#J zr0oe`1*rhH_?!Qe<&64z6{QNH?rq~nsHCmDHga>)SAY!OPGbRNFwI&Bki+GbhXFG9 zX!tI%D^Dz^0%Y)UfDB%QP6~{$vNZRq&Q<2BhFSUH&O|L2B~Mh$NW_Rn(2D>z|B|&0 zKn}00)GA9Y=R$>qecWPM8-NU+D0)I~jZp$*@IrtLZdtidIou)-4JC8{$l$SxNa7a} zz{X>G1t;4cXd4?c>|l18^pawQf+XF?s=$p7XF0Gs zX*I7RTAdD%!9QV@tm6O~{9Lg>%3x0b$lybc?f@BVE#F;z-q79m7&C`8NqSEaQRt-` z+3)dfBVNa0-KgrX<*ln(b!0GtC6oT67?#{u76ZuOzayia<+`cW$I9PUt=F^b$Y7)7 zh2n7X7uiY=8_NNs9sGZ&K>z#0ox^ zA_mCdNygnZZ_DiEbJV*X1LH>OO@JJpohnHhDws~$g>ZRp8Xwi%E<0ZCQ&AmnVoRtq zL`xM%QWqyZ6?~>>5W~C}lcwfU*~Ib%s_pil05X^-nxc4@`YoxYa4>ZQ@`-mOKn7nd zBbUdOA=|U)YRWfZKZPN+h2n(p1@$8`(f7dAyY^@u8NATeElQ(I5oRldX?qoXQP%&f z>^2nbf!&5+H|FR@5NS7L56@b1r%f}TSZwi6hQ<*1e`2Iks*i($PiHkM7D_9 zrhD(EX}Z#p?&NpQJw4vr;^+H+{quU!-22>Tobx{CdG1Yk-g!I612b5zUkA+J^;tee zWz|1)Y;~~rwhw(YvNH5@pOelfEZdAT^*;#A-~)v(RQq?B6sOeXA=4r!huL;9gRklH z3pZtz05iCt>U8^Q#Ts=%$QED*F9K%p607fC z<@&ZLwN_y*%c>R@?aq2Pb9lim{U>c7*&p`W6mlVQV)%5w*Ig!Be+10nLtV_^$Mn{1 z!S+MFUJ6MSn89@}zX3BiQD6qkGFJjKctz_0yKK(^A-R#w;RF5WyC&EyYTa4wU3?`g zJ;VNAX0TOAYvjm?v;O(6M{I7jW>?QE{v+!^#;19wb<0`~%l$m_g59FNh;R=Wu2R@W zw~eUTTbz;gYDWLOUb>Rz2>EXw7lXs1aw6UgIHp=)`%c?eHMzy5S^YEq%)MCgdh=P^ zr5@XYdq+hH%;0;rrEOI;o+b5J9vKUO8C+xRZ)@W*F8J}NS0kqdu6G*@d(NM!oez7) z%hSDbuV|MTFWG$V9v(a->H;u>4ZsZkx&3tQcO}8ux6_a24AAB^#o8pe=LbyzW^i24 zB6kb2}z5gSQ4{y1!|+xnot`ou%Th$WyLR5Ry+29~x27lkV$)GCxKHDKZH7mLFxrTo%<5j-~PK@dk z9T2j~Q>Ab+6&Plg{grK#{#n-4(yaPtEq{0295^Rxee}8zo#zt8t0wsUo3hgEoU}e! z^(C`_8T`EKW56sv8C@JQ&g*Z*P1D|bi*omz2fz$|y(Gmj)uP0uDqwKb-_ehS-twC4 zFw87(m{dL{=dZNAna1K#290dF%khB3DEpWbp}o}?98Q>DX*gBBCg)t*(99LZf7ZKi@*%tmh(xPWu{m0(mI{_gfb{V9W^6n6flFAIld_KXdI%w z4b0#lGY%JJ0y8*9dDq_*`E|_Mu-CkE9IwgVZQQMG$=RB=E~9tRxY|6^Y3G%|4E`-f z5uWNj#wiY%!Ij!aa<`|&XZ&8Msd)~V!BPI{k(DtE!-x4?aQe_ygUXtKnySe4LvszH8cF@p0~|wBzXo1;y11J1054@Aq9~zgYhWci%Uh zM*uUpyyDN?Wofh0mlv$4Hg&W)j`G_TISrV>uleS|J#suSgPWm!Y`P;bgEw_-cWeY^ z@K%8t+*jEyFoVPMUQcUCJDlyia>XU93KGy8l^V2Ddl=R@ssFR+?*CaNci~PXRM{flqpbJZ@0b zul_Esm#k;DSnA{R-%Be>&B`5G`JmNWq4fDZA};RjD9?b!u4VUcH5_>i{#N$Idw~jAsbcM6id!?8& zKPYQ#oM_YE^ZT$vzzp^bdCNm*x2i)~`$*CHbcd85GTqA-HrAVu0yJ-L>L=8DDDxGa8Eh>-SjRaSI528<@dE!g9U* z9QK<64e(pMFKXNa|&s5@!*w5Bpf1<}luzV)(Y?ddBO(4F108cHK~mZK_EjFT^Fr4-MC;dpjO9hu7Bv zGkCJ5BW*;{yt;p60V=PM$Kzgz{~&y}_uGyp^S1gir7oGzYtE*n7pB+Fm0fbZ85|$C zKE5&hq<5|3Sedr|+tSgQV>PSNUM*Z$TLaACmBC(dr{kZBaP}GObYAuZFoV~^^|&=&&89WY{!^jMtt zc*uWij4|PKbWFft*EQDh&6g`&at>)yHEB5>mFom%@I1J#jeZ}P!GBtBY_`zN$@x}b z27jY-Z#(GF=@%1YPKX0up20Q3=5K)+d{whW6O=Pdcc?Ya;h3L1#x?Q9m?s0DRIRn~ zZ@Hk$%K2OKwB~kpdc|X{Ns1AE>CwT7mtuAYexbSp%wYSBgUbS|RKd2XEz2rj&27>sH0o@Nis>y?_MiH$iGCt65}3gg+?Lw*X}wS^X zY4VM%9om-W_w66?9Txp~;$ndre9QLjR;9i!H!WAhHPx&S}AnP75gU{Nn^!X!dL}Fg-XkZ2}kw>-N)o;vOn6gP=1|Ku_v2*hI zG-^U(05F43xc@BQ+~xty;CE6MChq`d@GxVR{Dk*QQR5S*#&rTSSZ=3jTT!LRKMKs? zA;1jIZ(1my;hg}?;ZNd*hD`8y-cH^AbJg_xODThZ89bzHNmG+;9WaB(CjJ?>H{_tl zNxP-+bAjLH>rx_;f67QMZExISyH@=bFoSF2v?0!(9`}hQ7cIz7?TP{2;z2G~7$2$c7cNso=}hqNFY9{v`t&zO}yUxgx@x@N4|D zVcWe{D~5M=)=VwTP0dV7Og~?I4w%7Dd3s0GCg|hOh3UPL6~{Z**4`^rrCm$PNqf9_ zQ2lYM%fJjyP4G+b2%n&icd#*K3C!T%lJ)>I_?#iqYPiSz@T&<=05kXmFoRz)4XJwv zn89a(89c9OgyB2O9AE|?O<0}qW4MoZnZs>h24@vr0_LxKT4!N$-9XD1-S38dlyD$H z8L`y+X-6Nku`aT>7Ovk+-Ca1R?tw+M+sd$46MjpW6LHV`fa9y?8NdvV17`21so{ln zwa;6;;}#G$J)th)mxxH8YR3%oZw6!W%jqYRo=&|{u(~!==IZvFzznvFeBWoXQ=%-m zep1QdbYLa}Ggt<{eejuTcBpeg@5JXKD}2s6?E+@-DS;XMAjO>jSxqi5gKa{qxq`Q9}v%tu`Jh1`f=m1q@p%y))!AB!Okb4up{vv*NSUtk8; zo1$IE3C!Rpqb&R`Ie%pFUBhp{4E_t4!AW_2syCW`a>);VH~w(q_NYaE{>pj_D_{nf zLi+~ICSV5Jng+VOAN+j$g~a@*pZs1|4hLrNnzCM*1xXJy>bx0MJ3H?y1A?RD?$5DZIb6`X67!_ zAMbeJJU-~Gzzoju-|8~gYNx;q?v*@DqtB@TX7B*#)WA35U`tBO-~a>swt>tTpnWrI zWU_;1N6v=I-R-|SEe;$nFoO>VjCNH3GkB@?WY(zU%H+X0iIv0K7dfc|gMbr#sj~VlT8T@+k_Hf`FRiMrK=8F}Vvu`DDPVSriTg9!`G>7xR40h@DRIGi_CDl7N`OW@1 zOJD|1OfJouS@Cr1IA8|LWBq&WihU`_$t?w#!5`{o<(QHOCT{^|@V%C7MY7+ozzn`0 zdp+n?x4yPtwiN2l0yB74^6Oc9v@3ua>;=r=-HB^s_Xn*Ln87E389X4xUXzmbt-uWK z?<R@*Ixnc;2-XJUf?fe;u)F^{Rg&DR^D{$QSygUWc542Ux(UYl z*6AwW!1dvMff+njl_{Us8Cie6JU`D8n89mwPZ+DMYz1cU^3V{!cvZdp5-@}JYV-1S zIktsA>J-LxRtc{DzzqH|Vn86OlrIsG8NA$Y zyH}I4OkQi=*nAv*=k_mP2H&kp1ZMCf4hoL|U{`!?Go?q$6RL+9 zN6CIrJnBBsC*S8~&u5f3<>O?tTioi*6!k)PJC#Qn5?dr#9XAo_V#M6EJ_BWd>jd*P2~rb#UAO%-|;Y*C>?Z zb%m@2_Scg-iY+jM<#2BQ@4iT8;~|HW_Nd~%Phyea&QjbXin~T}pDew2_bu6rGd^FCQ(3-DfBLPxAYduyfkVn0}m>=WP77I6f$5l6!^ zwvKk0(~frDUryKQuUMat#C@{lGuKAW1@SIvmoJS&Jrlo!_V*$mKs=ZFLP%hLT-x&; z{UhcO+Rb~W*@Eux9Anx;k1a9#XlF2DDKav+AI42Yl8(q?Vxgdq9Br;gsI#ZU+Y_^u z`(Sn<;YikT7Nh0lJw+MIrCGw*a~&!2Xf^G2C#D7G33sh%nWr z(ddk^!z(;{c?7Oaag+YV_|Z8X@iE0h?34WCx)f!aN9+w{0-+@SCFIc`(LdTljw=J= zfpO?QQ`jcnInL*31C`;(J+JaPjU*y~5jl`?;i_;eg!>K*Z6%*cM~kGv2OqWqz#pkIsvam6@)sc#&eULtBDf*cR3g*9+2 zW|$B=i$|v&{uC{7<)@Z}ffN@mp_#*(Px(Z51I|je5A7AlqCNY>%n%V|%sGn)AI>h0 z8NYKbu;)F!CC*uQB|dITao{twK;)*P72=hLkIBoP{;|zm&gYB?bj@$TqIa>n+yd`Hk$FZ|FPu4eMda{F z`H)d=JK6@I_uzO+t_BOUI`k=$?(&ab%9AxXLm{Qg6scz9OA%Vjhb#i~8oZ zid%8y7&);%W5W?Z>=*|w<48&Gtb*%3{`B;l&%1lmJ##sJ4`YFGN9$+?iFZNjt$W{e zhCNHOg=e|wEoBy?K<{J$S;cdJ5TLaKGnCgNvWeWrG0_&bkk({zEJn8bXwpcC`AQs1 zyuZ-=;q`;sQcs8wj)*o$kHp!;wb>)eT|$Ka^4#J$<2lDbj1!L29bxjB>e447Vlk2} zWDolzU3b_PX%vQNdWarzUb3$uhW~Yh-tZ?Z{4Exm znPODLsNt9#HI5gdgKfmm6wK%D{+XRik&nqMdw0aqW6@jmgr3Ny`PkDsvJ?AZi$$E+ zULJ+U<)=!UbFxED%Ijy#xX`)DdKp9FV0Fl zrZTLH*J#z?-c4QK{NQZD$kEd(ts^)J-Cb$M@l3++WaxVt%;}_#p|S^7KiSp@yzK3Y z5x~oKw!b0pvMHFiPNM|JiofbkTB}Z9jXR;*i594gn6w6~EgLvkL~r z=Fvb!vjoNF_nMvsUiR^Z#~Z(FYHOa|{*rl{Rfzl#`{x}B9DZ{c4!rCyL9uy1C^o-j z4(RY`Jz+F78XC4VeBH1J{3vMowBxGmAL}@~j}=!PY#c8Mip{<3hs(dW{s?&4rEM-P zl}#@G(vyzHU&3Bb#4b=c-$R=6v6+0C}yZ1or@HXmtiHo7zw zf@1UI4YdvHnv^ZSx0}qvt?T3;D_TIYxwqrDf@1R=`J2`%i&dT4)=cBpMte|fo>l)G z@UriKVzbHOlti4_vXb5XO(K6e#&brOs-Z|LiC70#OHBM_3 z9{?{~*U{K~qT!=DJ@B&M0L5lm>o{|n%`t~>l`pzRx$alCI?l3Bu|96@-!5Vq=p zRi6pG?B%A{1jS~ZTd>z{`FIc-eF7W*Mzb3v9ET(pCF_m#qX|_6GUWf@1Tdpx8V}Us)AnINhb# ze8a88eS_OXmrMWhvg@i})Qc-alr4*CO?`l@EbTu^NOQ&4Q41-$H0$`bpXRv&kq zYs&0WZ2qV=(D;ezEqOm>h``Gx#pY+s!$7fFAt*K<78IMuf?{*C>V(G#k1tel&U5V! zTP$naFYvOxD*IJEZaCADYVnsM-}N((Mvq%=pDEMrRaV!4mwm0KvhtE{jXoX}o3F`i z6+3{JJ;`$@C^ipNi~?Tvp2pq4%ig4$pdVh>(mdGWLq)y7%kJ-fS9#Wcn`LR+mkp=? z z=54^szUwx~xsUuw^U&sYP;7p*q7@XI4WQU;wA&4e%|1Z2L&au0D@~iFpxCU{o~o>^ zQ@6cu{e<&l9z)gddFkBhL9uzJDGn5y^MIGFt+-ZgZ1R*XRTQee@tUE|^!Ux?mi@1w z*nGgSS>LJ6(T>$msc!*Zwx!D?P;7n_c-iM1_15ooJm0vyI;~=cc8P9KZ9FJ8S2&(< zU#uRjw(uAVip|T+M&lc`2Hga0P{kF2m;IalUf^YCtKain?YdMk!17A#S;JgGvH1ol zHviB*2Nau+dYXWj{hFH&6q{cJ#pc-RwV>EMRN!SRL9yA|<2&^dwZg++xk2t`-feWR z{YvMob*!icUiR;TVsoh4tbW(C4=6T&Vo}x7RQIc(*leqRul|RQ-Ex1|abCkgv03KU z+fimcvfWf44T{Zo%2(@b>rS_RY5lXawP&pN8|stp6P#z;{@5Aalv{nV;&}Pcilpkh zz={raobP@a6q~I)p9WqwDmI^~%>!Qc%i2odW$!Z06cn3Z^d9Kt2a3(hEq`cz*I=jb zt^K7uuJT>OX+g0$T;OFVyR{0u>~#%uftNj_{6WRY+DcGto&}1{E4{VqJ|4Nwzu8uH z4sNixz&z{|el9^$>+`x(#az{@^uE;A}>hv{A}kJH*#9RXhU6vafh z{lLo}=Jk(jlwy&^yP()?1&YlZ%1>7Q4vNi3>>g7+s|cRKUy5aj$Wj z47}`5f!jVnd$i1{;wtd6)z*idmwR@4f8c%8!&14$c7ErE#yml>d6xDlC^jbnFWcZ@ z<s@UlIDmz@lX%_khzTlQ`J zsP3rlK-oaxWv@4E>F}^i1YULxC^p-;&vASP6q{oWZk6}T?8**Td|kJ_Jwd+C^(A$c z&nn<$t3a{&Feo;EQfX7JFAb~6tUcH^$o4N#Y?k@n^&a4H#p$y36yRkS0WbTf(jT>v zpxC?x6r1k>@7hV=Wxrrk+g@5f5_s9iOP>T@b~7k8BQLwdXL1)WdvC|OhF|rsfMRno zux4XhimY-#u{qagi_Zt3*!-*QXy9d!sJhU_%bwh9vUG6X;`tLOHk&;Mx~!46f@1UW zs^ao>z{`HK>I35-P;Bn+dCo`YGtcX$i-+BZra{2V_SY^dO$A=|&8Br0e>-0HIOyZ< zdmj{=zqboDXEklCo~B(}`ia2H{z=wb!plyyKW`pw%&XoDyzFshMP0mXo%?gZ%bu+6 z1H9~!vP|PMpxC^&)U|A3<=uvbW|QKe`!JtnzL(VBsro7|%VGt^=Gmobr469ioM+mv znC_kcyzExsWgixJ*;zH-5?;2uqSnn>P;6f7rgHewqP=-w?YN3}OW!H&E%35;*l%$w z_depg#`}ib`wpI#@3wqUyQ^Y#>1g0(UpG7oyzG%~sldzr)jQTb&*4?eYb`fwzphv! z@Uovbq;_1kb8&m%o#(6beiL}vw=9*dwsqGkUIAYAKu~O+)G^BLhU!~EvH2(WBaUHK zue3f_*Iu!vw6LT?y9pGVGv$j^Z+f@+P6A$bo8wO4WnZXE)O}uhy<|TqHcxM#Bk;0i zemi}Z3yRHs+v@A)=)M46_F(P%wRvsTwm-V|0!8MZe17rBaGGcRS=*C_4|T^&SCkma zLu;RFd)4-NR~v6%;AJa4hdAH0Zfg75a7*`V>2tu#{n=U zz8*Bm%iK#cL9uy2%T()gf@1SG|MIdu8qx$__MS3r)fv#c9jq+$^7gv}yzKriwYG9l zY!1^81>W|UvaP_&9%FUI`4doVZUe>Uy)JVFUiKUM#Bn%YMr*6j-rg_G8Rjo6Z6+J0E!2 z&sMHzoG*I;6q^tDe(JZ}`#BYWMr zQc!G0UiP$x;pS0YyzDmbH{5n9Zpey(mwmguruf|we_e9@S<^3y5_ca#u{p)f+M%Dt zq~=FzY=D=&sN`D3Za43#c?_<{m5c+$=Ixzd*(=?1 zeOd)xw$35gGO9Q4 zfnxK-k{!kQ<=@v1YTw?)%l_6kOyFf}ftNiWuFoyrSRMz8%|=jc?f_nOmG3T4Y+hzP z1r(bPRV*kOR~%k`tR_(4W&h^g$3GYpn~yn{SU+g<1YY)rl4#&%ht})_#b)MZ5B5?j zXWEQxf6oxF+gBnhMqYMv%S_;9PxBrKip}47?N+ASoM|rw#pXjLrA4YTb@hua8tb<} zv3Zn#vY)HkqUS6vn~9hGV_EN_r;Ba~yli(x zg2y|4hJcSjvH1%@vAL|evaDZ`SJ9mE5q0M~w%Oko6q_^sUiAzF#pcA8v7p#Is3^a% z3V7M!pxC?yc-g*z{rvMhKXy`DZEg9iW_tO^qH~4YfS0|u{R_KbcYD8tz*YW3yi87? zTIoRZe0%xCqRpV#e5%H|-Ouj6Tek0$fxq~l^?JkE0E*4X%brm*6%?BzYc{kUlTUT~ z33%5ge_P;XFS5P^yli{zOGW()7nYu>Zf%W}cYtE^6i{qlte&b&v5svs*FLFTQ6v`> zn^(4eXZw}vIpAfl56DyBQjWFR(Kfa2UEpQs6&wV`W?f4d@Uo+P7X|(p(A#^U%P%&q zpxFF}_F&PKf+-~ft7f+xu*n5p_F_S?`J_uIC^kD8oPn49Wr3{Zsy?qd%4UJA_YFHiv3Y1g|KgLC zm%4b_I|I)Oip?A4hdQF_?*T76svxr{s4~I0$Ld*^K0dnxOF^;uRh3rW&@sQh6L{IK z1zU=)053bnsz9mt-Vo>-w9&U*HQ8=)=il|?by1+$+#3{|13|HQner?sD$fbB_FLkX zZkOKKtKn_kqs5o=e+R{81i3b&zxV)I?yGsUO!UjtsY2^5>ZbDjXa z>*AnfzvtaA!Y>z;HrQ3ZA}BT=*KQYh**3u2b_tI0zvtdh;cgz?IIr^K;^q1G3SzYN z_1k1wPJgTS2TlOR=3yS+C{~-l0LA7nK(TotC^pXnUiJ#7L!j8aJ-EaFGVro9%!bBH z*zFjb?*+W^#&{Gwh~qR3yLKd9_p-DXp7$4svcfk7cYfnI`Qv$^T8-l3#Y;AIc5lLIgN zE8tzv1I6YKys{j(T1*GU=ID}}d78YZN^b%$d%nX$ul0f3Lh?Ydd6ZL?#W%*7sw~%dilF;@uwn;#Ptn>&IZ_&n7XdH(LdYM!k-*EgZEFO@=D+g}<@5ty z_NOgXwvV`v2VQnTh}v%#@UoWyFMEDjQ{HQ!*xVNso9A`$vJ-)q?FNd?uIo<3o#pYw78$#{^Y#=+#0I$-{kf{KA@wuZXfWnld}8gyH$MO6lZOtQu|*I zod}A}X17sxM?0n&a?9uCe+7!o?`fTzezN+?wchWS(1T%r0x$blP;9F z+iG_-`dG!eUhqo_%>l*c7WY{DO`SpY%e0^8Pt3LkUUpN%9?R1%^ZYcSLE+y5FZ*-* zy3Tj%uLCc80PwQw%U)}6vV6ov4T{av!rg+JJ)(h^U0d%|k)Q9A{bcTnvZVUAEq+&) z_+AO!1B%VBgJN^CX>!B73Pph&6r0ORpQx{vO;>*5`%CB_;iW-2z{?(PzS3~ABCen^ z>j3bwe>A)#D|Q~`dot8UP;B1jaLgRqsM5_UNX;6OQ&W;?$T81!?(lgJ6q`?hVza%Y zTJ|~cvUe2R2gT-~lWbfS2tLip`6GmtEWVqwZqCMc`#O7Axy+nbtZj^jROe zC*rM;%j&;@mp!G)wz8<;RMxEQ)m@6s?mi1cZ$)TA`g%`sdd}i{)0E2a!XsI(*=G#W&qq!Pv++$(I$6yT z6q`3?6=lX0=2rdCeoC-p*Ql(38Mw0QCE#6e$b72chW^vGkL?cuFZ;*H3Bb!<<}%*;vldWFFWQ?mG&3## z3B48YvWI!i3CWE7Jp6`Vjmz)WO)Y(@i;6C0$ud{u{|JiBpMzp^e^6|041dCZE+{rH zYTZ$-F20tPlHru!w^9#^%_}`^LYgB-MV$A~1zz^;*39aK#dov5%=kF(2i@zy%MS3& z3igWH8=($(67*VQ+J@E~DAr`XmN6hNL02XyHlGWQj!KKz7;sdz&~_v6vh#{dvIb<_ z2gT;sn$O$5?6Ez#7x1!cLCfo&ZCP7ojk=^R%PV7k?&B3z#z$>s9#ew*N3DpQ42sQz z<->rNy`aP~TPyIg7XvT*fO|;r@TfD9zXsO29R^4_gLxJ>?P^t*^ifJ zHV(3$0gBCYqc%h}2R-JIYiI2Er*3=cs_duJcV_=yHW7H)jRG(GcGL@?*gR2CY|bp* zo1K_GD0@iR-G;$dTU3U?*-!5g~C*9m=3&bdHN^7%g(7EZ+X*oQ{YQc>!RNZ(R#iNyzEEokAh-zR$Bk8nv%Ko z$-v8gJaCf0%N`GEhqq0i)m#0`%hnjCTNM1O*nHP(g2Pj0TTpBspK~wm%gp-X(S{V+ z>n7DL)Q?{{juhy8e z4S3nTiYCd_AigT86yu*^B(IID=0P(0LA9Gj2ngj)J!)GaE|j&0mbI|;Uj#` zIeiR@%_A%3=Wa;LNgoTm?Cj3_pxAsZ(na89`#RTKoNn49C^jQ6yQF#n@Upl29giFs ztBz3nu5cb@S<|GixSP8|;AJnbmUT8eJ_Ee$DX}j`EcPvSK5jVyc-h9>)oD@bE(OM_ z%^ll7vHA7LZLwD(l70IqJ1oC4uGNKtqH;a(vfr;#f@1S=-)WIw$J#~q2VVAAt0quv zo(+o4XVWHtV)K{nmw=Z&AoAzfae`tq^0I>~KY{jdry24N>m%BSgJN?)WM1sy$Y#Gu zE*8Mcey#FO-meLC>A zr-EX$rS9XFBD*;6NfFcHo{o+Ud=*xJuUnt2aw<3pip|S&4pjuTylI!E?hA^|??k^D zm<)=|HLV|44Fz6ydx}TSz=~g*J?u6D=Vx8qz32i^`@0H?&HAdH1@J|Yz{`$l{y^@j z{v~{0TnO;854(i|FMC9FUO{Wx89}l6qS0G^!RwRogK-ODz6Dq8@=8X6q}2Jp6F6+9#!ZMyzC2EYe2Diysg{J~GYV?J(fR}BNwGDXLlWlr?{s6q~(Xs9! z>w%ZOvcna4*>9#hr+lC3RkpBEYkePh+26#yANzVpp~qz4Wlyi&TXZtLSu-AZ*)wjK^ZTIK+}Neq+y@k!trfdE9qRnLc-g;}C>q|i z+U@>L=yh1>+ImqBtG7os!chZ*?pD{#QzT}n-yzF~%2g3TQ z-*jj)y=9nIvNhu^&CYa{z{{>y?e5}be+0bj31*GqyOL{NyzD!`%igMbHe_a8Z2ZXZ zQguJaFU=A4btM_V%WhA5s%U=Q-?AW;Ysgb^6XQPv#pZV%JIwIEjipN9WuH&e6z0^< zkX>=T9vm09F}^PR2XDROIM6FSRyrngv}SGElENjmdh|@%mOa#T|=!9(oFWcMl zaO2~^%bubcEby|w1d-oAL9uyeLILoym7v&cZhXIdJt#JVW;rM}kMB$d#pX4!ZwS2X z>B?^{hcw+Qzn(P)6q|47&8^Dmc-A>F@bTCW6Xr#I<97yl*(aMG2gT+E8qo9wUiPyc z8mGSn#pa)*l>Wgkiv(Ww`0ST7OH%Zp*qjc$>_q{UG3OH;qnG<{2VV9HqZxSFYc#!6 zcICEJE@-b5c-i+7CJVgm0P8=EbAgw=Sz`r?&F=s&dx_&=ftP(MIt~<@*8(s5mkL$R zK~1_w1B%Va%YMOs0Vq0GMsEdP_FvZPo2`L$h`j7=IY)q(eaNB3uXjvaLOie{>;6@2 z{!R0)COBs(@Ujyfj(}n_@v?`h-mnR7Iit%5#pa2c>)F{AkF;ulmz^3No_HZBq%l`FWb|;r2WHcML}Sy zQ}VMJ&z5d(Y`1Y$kB^K^jEH|GbhYO)`ym}=)r$&7f?_lL2u+Ds<2Rt#>>oKa@%i}u zp{G4h+pp<}tl3d8J+%UO*+)t)2#U=&BYYCykFO1l5EPp~uW1Fv=CY)XpxE5M;iUCt z&s7noge$`Xg4?JkE!w|I5p^Hs#a?3yRIV1;yqttC1e_!hcN|nlLx~l=@?brKZQ~wiInm zJ1;0U6EE96{7AwYP;B<~);ioU?W@ZKUiJ@3D&S?KV)J}KvH4(vW5hD=k&gc6hPtR? zL)uADZ2k;**?$2qdqvn%;APK>fP1CmGV^r9CPA_Jlhnw<8c=LTUiJ(@u{i>G*_q~R zhNdpX=9>j8YV&0Zx4WU?3GRtjk-K~*IQ5c+)sGbvn_YpIZK|0o+pU@rsshF47b45y z8E2>L-TH4!iqcOdrKYsy?*_$YHAbD>NJip{|pKP2r<`6}PHW|{dY z@Unl8e*<{g$9<7K?vZ)(-Yowt=C!IANQ0x#QHP;9={SWvb$vnA;>;ALy|VZh5iA54ugVea^PT(x{o|CN*qk4*3ly7=fnu|Rpx8{j>^~j%1^f}~(d&Y3yRGH zftNiJc-cRxHUTf&PiLKzm;7vUan|gLQLPgl1_`|Ek7DP5Vso<1@Rl9$%M~5Tk0-yI zRjIwxlCSvF?`DioucX*3L9YQX`-_$mP;4Hkc`8Lny%@a{W~Z&tL&fhF-QIm zip}kTlR&ZA)EZs2BR@Lro0MO(3$&j%&$650-2uGpZ=!z=oa|<>UEY=tzl}q@Y=d?V z@Ul0mjiA`v8uen}K(}ytQrl7BWd~*q1YWiiC^k=!pZ019vx%J*WdqM}BjlsnpRQS1 z`bmBrc-f{>3s7txDJV9lT5ol86?oZhU5d@0057{HH#j>zf2eLOC^l|qJrDGCkyL=mu+pe0(jZ`fS3J@?@d=5yV0inhG^Z@!ULe#JVQ`y zu23%Wy#0@=Eg-L9zK>NnP2Us(9l@ zSw9Cy4?o{;d`+Gwl=tQSGDGu=wH^X5JF)snMcdg<+O4@IWdq1Vj3ETyWyE1W)DAwRTqqswp zU%Y=5cWlxz*23D-7w`1sU7F%vTWm!&Y0oO}?dHDlD%wT$*i!MG_m%RFVXV&<&@t@^ z8*Aj{G4cu@>!6rZ^Cqyfc?=<(OehKJFPe z_FB4!nc_&haBSp58r|bvq8OAgGN7*GtogzWGE9S?4 z+rYRHTLx`=*eBkBEFD2akUV6ps1D@?p+KB6vl;C~uPK5QEgqGkjaO-JGw;f#z0%kQ zUt%-}Tgpl7gHWLT@myQX4ayZ77wZz^0g)DYBBCx=&paR;c{UM0gzJ#+J!2zAScU)RuEb zJQI%y=kC7a=$vtQB-X~G?$ERM*HViplPMRO>p`=CIjI-{ z_JEJ2JRI&5)mDi&*mJ&jpC61pj?8m_@>BFeny+F6(09&k=`mx$81jt7$g?jrFNujm zp3%y|)=)N3u8_YyOQbakLC$z-OnDAq4eE#MvNaSTMwDkRA|dsZS1y`~*oJe3`o+IA zQ`md)oMK97u+?mj*jlVF&1KFq@}9jRtY{@=+^DCAXDg4uGYl<9gm|sS>*CzO@&4Nq zo(C91swJ(-J`#>JOBhqEMr=$UT&GB~XE-YQ zMdR^|ChbF-C3L4EJMsF%J&_d;|K;@t`=?R)h%nFjRx*5}t`8^vo3z9jwO)ay$roZY{3p;@yYzH7WNoi?Ih<1rSrd|6)HBCFx(W zl#ZD3hQF92&O?e5#vA+Peki7NMq~2ILL*>J{zs9c>(aV(%yXUMNEy#-0wag#*c*=W5@}y#3%93w zJfmn%OOe5u)H5pR8I3@3lg3iSTG}VuBaSW3UJ*IQntsVaTrv5GbBI=1dYTk%dpMVP zg_1^8jI)Rk%?qrJc1q_1)yDX7ZJLW1E%bn1Xcy(C*jkLEG#XS3$HO*Mr{~>{`y|{c zGwB#(Mk@i2#yQ5>k8QC`Tt&nq?oWK@SYaFKF=L7=2Yy2C&c5ZkGzzUt^m!U1BK9Tr z!hJKEgm=$77~_t$IbTIwc?R%GC}s`$E6pbnd(ON6odFyj9uZq`WC&$xTWJZ_lD48V z!h@`%Y~Xc*?qnz7D9qnGch`B zJAGFqMUCPs_Q&25280`0)iV-|4`(&DLrbYWJjrt`rCfgBgRU+w-7pn40(Y)VjXPT{nztoEHNh3AEPRLV&pnlqx-QK zTk=Zegj;uwi?*?^j0fA-J!c3VaZG z@g^H_cGG8A%1!j1dlZjoOtG9+37Qoc1)3e0r-UA_eS`~_^^6Cfa}MG;uPgX;$F|{f z1+TN+?@gSUSSsch^@sjZmWdIi61I*#7QJJO_+5-X{*I1f4|e+tDdus z?ZLJ*B9)L=II{GJEfQnPcH%qDTAt_F57wiYBc9!3jJDF4_%1!>F=-U)fidX$F4hr` zD61)=G(*IEL)66iK}Tfgf8`osF1^ATj4`KNCfo50E$E&{(##MeCq@=6rJk@ozi6eV zXIjFS^OwHcCLahjw1M!#bMc7I=$tG=FK`{^@8Y;Ujw<4hwn=)4!XBhXz@nF;# zJ4T#G;>@Ne47L}?$DYUoX^zqrMv3gCxxv_52+xJoKaWaQkZ05{|CK%|Fs9@`c}Cel z{m|OY9CyV_Gr9c5EwQBweYcbBcXnEHP3&kEtyo z$n~*T%1s`T^Pg+*JE4PrF+;F_u1V1s?-IObV;)K)B4$+2Be5^KLg&1WN>}FYKA~-# zI~Xl79^4Pspch$!cB2RU?VfbSq%7wsan4gu)HCHj%|y-)?w63H*+W)g=HXRF0sG+W z7r*1AQl9Blm)p}iiuV6^ed+TgjwITSy>fi$o#zurobD+2bk812?UeRRh*4fjW6PPq z*buI`R?>P&2+^G9tR_$JPKM7EA{y+eh*wVxXf(o?Mv{Jx=h$%;BW8>N+x-9M9s7~y zkcc6c_k1y?{KUpl#Cqrv#humzii2n;U1f9;Gsb}OjK||XaJJItG{TTSKk{tj5vd>Q zi?E~nf)oX6gX2gu6>GB{WWAWfj0yHGJ*Lq_-$X?Cd7HdPKYQNMc{cJ|Pac!MJYUJ4 zp3mhtD(4X6!!~q($1%_=@{?9P&L6aia+TwXef7j$w3>aV-mncticw_D#P^3+49adA zm0~3IL|kX25#m)qj5OIx?K$p<2j@Os`LDUcSc%Wi;GrB|anRNdL%_E*c z90%%`|I)ZTD=FJ~{!ol)J>`t!*~Y7^^}pZqJp8GPZNfN^zi0~{(>u*c?wLN3(v0Jg zrHE19bM7*}=*PpD;(QQG(I4tb%v46@zusv!iStHUhh`*=iZv+H*mtf;wa5!bjOzCM zY}931k2lU=euC%!AHqEC{*Pw@j!brAS8N$uizR3WKSw+q4e=VUB+?ASwWoUx?2pFd zBgU)y3}o!77OiAGV~2h7Kf2HIGZe*~`oMUKQ5CHe=QWmzpNtugo_1kBd<~B{BJ|Cn zn0Xj4Ji~nLc|`b1<4g01y_L43WA=nb7sur9);NA-Eu{lnPDqmtVsz14iYjBrV~|~J zx#$Oa&DS_bID@Gzm*VUe*K*84=@^tnWHH-@?>w&&FN*j7IwJqXznnw3VxX1M-pEh1 zQ|d8~z*ogAAS*Z$YzOs;wY$H>>)bon6eGbS6ISFW-?hYX#rA9ywxfSECdVG1VX+sy z#>gVt92>TsGKW3Kk@)*z!iccqSkX25c2Uf4UN598RQGv{<6_U)+kbr~VGl%K_^5k^ zk+qa3yhhOu1R4*=;TQHm-tv8f)&%OAFrnPxXCLyT`^v$6NLxvBobjg^(HZ*5SRm$< zX>1|mzzA_9xd)DrxNe|b_z8mU!qhvD`+p&aws(&h;?MtZe9>N-n>071IAIOKie@BR zE&4^72U)VGE!4^pGIXDWf9OxI-<&nu+(L*Z7`uGyh_~^e-#cOz< z6#tJK_jsTOJQiUgMTGnidnNmYLI00GJuxC|&{p&VEs%cyO7ZFLvzTj)H0LdULZEvw zA;9Qx?CG8ExAe&bz2sH7X9i+9WieUE^NG>Gk*SQ3M%%=`sCCb0bVdw&#AAvRLWZ`{ zb2!gTab{t3Ly$yt>gHL=!>3_?%U)S z`N{L6r=QaH-J{YoE6@TS9j~!P;tE6g!snbPXumi*+C-W0a2&diPfzlE&!Aoi&+dr) ze`N#Cd(IWk7)FnK!g9(e_KG~k(Ywzrwx6CmDPGcfNZ#`}oKYMJYRza6E+QV>C)MsA z8HyXn7taVq@iPdwW)#UbF>3TYK|M(C>OI?0_7UC>$Fh4=IkMOWUt|?OSMj$}ybdrb z6h+1a+lYvWD_ZxF@QhYV$_PT1yyN=_9_S)UkhtL04 z3C)?F>#S%aw!=}o+ehm=M^Jpu=ov+9)w3?*jAghEq5_Hd2^Z_qXw)M=ud?l!ReVpO zKDs~ob1c!`|MimMM96Y}(RzlsO7m9qfbJCkd#=#vl)aRPjG1@^JrSQBq<0G}=Vxuk zhgLSWm{)Veh>)T-q)dcmY&ZFiqvH`;#A^?h(H!8|iP&OYaZTbGL;mv_uWO=5ct4Zw zXX9*@MjfpsRHdH?&`a!>{KIzg57tR)QCQ_4RajVvJ4 z@R(aL4rm>hi!*{+GS(tuWGkXbFTCgS9!`GRpb^nt@yU;^!&2 zfw3g7DR;#(YJr|G3KUT+=QxNF>>fWHgKAUk(GyXZNbE<7x3rYd*mL7VxQu48SqgYD#46K-5i zb=XcE|9`#68;%LhFS1Cqi!JT>PPX)n6q>i|97Cxc?3Xlu|EnI^%$W0&>3`4q|F(cjMIU*# z5(?-qdCv%lXXGEvMY4hO{{PA~iXeJMD-@2vd5-PaB3d1IU7;9~zl;uAN-u1~8H2r| zjc5z|!hLdy=!4W!F?&R;DbH{e`fi8E;q`*g*>bjmK679xT1&O)-o-r#eGc(VVSgyjWIH4M@OW68 z5a)>TyroqOdqtc0lj*~wQ;uLc`A_E$M-}sj*GRU9GKTPBTyQkH&iP7yVvqFVa;!%0QTKZAML+af~+S7_STzQ(WQs$$)a4Gm3mAtT{8d mjL!)P^i_-rA;q3E)>32`MQkO#HzVc`?@tyZKo(19`u_tTEfbpn diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 4ad2574188..36d69b03bb 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -60,7 +60,7 @@ procs = [ PythonProcess("navmodeld", "selfdrive.modeld.navmodeld", only_onroad), NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), - NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], only_onroad), + PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index e89ce5c72b..f8d8db9cdc 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -46,7 +46,7 @@ PROCS = { "selfdrive.thermald.thermald": 3.87, "selfdrive.locationd.calibrationd": 2.0, "selfdrive.locationd.torqued": 5.0, - "./_soundd": (1.0, 65.0), + "selfdrive.ui.soundd": 5.8, "selfdrive.monitoring.dmonitoringd": 4.0, "./proclogd": 1.54, "system.logmessaged": 0.2, diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 916f1017a3..f0db1f63a7 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -70,12 +70,6 @@ qt_env.Command(assets, [assets_src, translations_assets_src], f"rcc $SOURCES -o qt_env.Depends(assets, Glob('#selfdrive/assets/*', exclude=[assets, assets_src, translations_assets_src, "#selfdrive/assets/assets.o"]) + [lrelease]) asset_obj = qt_env.Object("assets", assets) -# build soundd -qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) -if GetOption('extras'): - qt_env.Program("tests/playsound", "tests/playsound.cc", LIBS=base_libs) - qt_env.Program('tests/test_sound', ['tests/test_runner.cc', 'soundd/sound.cc', 'tests/test_sound.cc'], LIBS=qt_libs) - qt_env.SharedLibrary("qt/python_helpers", ["qt/qt_window.cc"], LIBS=qt_libs) # spinner and text window diff --git a/selfdrive/ui/__init__.py b/selfdrive/ui/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py new file mode 100644 index 0000000000..33fcf0be31 --- /dev/null +++ b/selfdrive/ui/soundd.py @@ -0,0 +1,160 @@ +import math +import time +import numpy as np +import os +import wave + +from typing import Dict, Optional, Tuple + +from cereal import car, messaging +from openpilot.common.basedir import BASEDIR +from openpilot.common.realtime import Ratekeeper +from openpilot.system.hardware import PC +from openpilot.system.swaglog import cloudlog + +SAMPLE_RATE = 48000 +MAX_VOLUME = 1.0 +MIN_VOLUME = 0.1 +CONTROLS_TIMEOUT = 5 # 5 seconds + +AMBIENT_DB = 30 # DB where MIN_VOLUME is applied +DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied + +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + + +sound_list: Dict[int, Tuple[str, Optional[int], float]] = { + # AudibleAlert, file name, play count (none for infinite) + AudibleAlert.engage: ("engage.wav", 1, MAX_VOLUME), + AudibleAlert.disengage: ("disengage.wav", 1, MAX_VOLUME), + AudibleAlert.refuse: ("refuse.wav", 1, MAX_VOLUME), + + AudibleAlert.prompt: ("prompt.wav", 1, MAX_VOLUME), + AudibleAlert.promptRepeat: ("prompt.wav", None, MAX_VOLUME), + AudibleAlert.promptDistracted: ("prompt_distracted.wav", None, MAX_VOLUME), + + AudibleAlert.warningSoft: ("warning_soft.wav", None, MAX_VOLUME), + AudibleAlert.warningImmediate: ("warning_immediate.wav", None, MAX_VOLUME), +} + +def check_controls_timeout_alert(sm): + controls_missing = time.monotonic() - sm.rcv_time['controlsState'] + + if controls_missing > CONTROLS_TIMEOUT: + if sm['controlsState'].enabled and (controls_missing - CONTROLS_TIMEOUT) < 10: + return True + + return False + + +class Soundd: + def __init__(self): + self.load_sounds() + + self.current_alert = AudibleAlert.none + self.current_volume = MIN_VOLUME + self.current_sound_frame = 0 + + self.controls_timeout_alert = False + + if not PC: + os.system("pactl set-sink-volume @DEFAULT_SINK@ 0.9") # set to max volume and control volume within soundd + + def load_sounds(self): + self.loaded_sounds: Dict[int, np.ndarray] = {} + + # Load all sounds + for sound in sound_list: + filename, play_count, volume = sound_list[sound] + + wavefile = wave.open(BASEDIR + "/selfdrive/assets/sounds/" + filename, 'r') + + assert wavefile.getnchannels() == 1 + assert wavefile.getsampwidth() == 2 + assert wavefile.getframerate() == SAMPLE_RATE + + length = wavefile.getnframes() + self.loaded_sounds[sound] = np.frombuffer(wavefile.readframes(length), dtype=np.int16).astype(np.float32) / (2**16/2) + + def get_sound_data(self, frames): # get "frames" worth of data from the current alert sound, looping when required + + ret = np.zeros(frames, dtype=np.float32) + + if self.current_alert != AudibleAlert.none: + num_loops = sound_list[self.current_alert][1] + sound_data = self.loaded_sounds[self.current_alert] + written_frames = 0 + + current_sound_frame = self.current_sound_frame % len(sound_data) + loops = self.current_sound_frame // len(sound_data) + + while written_frames < frames and (num_loops is None or loops < num_loops): + available_frames = sound_data.shape[0] - current_sound_frame + frames_to_write = min(available_frames, frames - written_frames) + ret[written_frames:written_frames+frames_to_write] = sound_data[current_sound_frame:current_sound_frame+frames_to_write] + written_frames += frames_to_write + self.current_sound_frame += frames_to_write + + return ret * self.current_volume + + def callback(self, data_out: np.ndarray, frames: int, time, status) -> None: + if status: + cloudlog.warning(f"soundd stream over/underflow: {status}") + data_out[:frames, 0] = self.get_sound_data(frames) + + def update_alert(self, new_alert): + current_alert_played_once = self.current_alert == AudibleAlert.none or self.current_sound_frame > len(self.loaded_sounds[self.current_alert]) + if self.current_alert != new_alert and (new_alert != AudibleAlert.none or current_alert_played_once): + self.current_alert = new_alert + self.current_sound_frame = 0 + + def get_audible_alert(self, sm): + if sm.updated['controlsState']: + new_alert = sm['controlsState'].alertSound.raw + self.update_alert(new_alert) + elif check_controls_timeout_alert(sm): + self.update_alert(AudibleAlert.warningImmediate) + self.controls_timeout_alert = True + elif self.controls_timeout_alert: + self.update_alert(AudibleAlert.none) + self.controls_timeout_alert = False + + def calculate_volume(self, weighted_db): + volume = ((weighted_db - AMBIENT_DB) / DB_SCALE) * (MAX_VOLUME - MIN_VOLUME) + MIN_VOLUME + return math.pow(10, (np.clip(volume, MIN_VOLUME, MAX_VOLUME) - 1)) + + def soundd_thread(self): + # sounddevice must be imported after forking processes + import sounddevice as sd + + rk = Ratekeeper(20) + + sm = messaging.SubMaster(['controlsState', 'microphone']) + + if PC: + device = None + else: + device = "pulse" # "sdm845-tavil-snd-card: - (hw:0,0)" + + with sd.OutputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") + while True: + sm.update(0) + + if sm.updated['microphone']: + self.current_volume = self.calculate_volume(sm["microphone"].filteredSoundPressureWeightedDb) + + self.get_audible_alert(sm) + + rk.keep_time() + + assert stream.active + + +def main(): + s = Soundd() + s.soundd_thread() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/selfdrive/ui/soundd/.gitignore b/selfdrive/ui/soundd/.gitignore deleted file mode 100644 index c47f949d37..0000000000 --- a/selfdrive/ui/soundd/.gitignore +++ /dev/null @@ -1 +0,0 @@ -_soundd diff --git a/selfdrive/ui/soundd/main.cc b/selfdrive/ui/soundd/main.cc deleted file mode 100644 index c6c7434ca4..0000000000 --- a/selfdrive/ui/soundd/main.cc +++ /dev/null @@ -1,18 +0,0 @@ -#include - -#include - -#include "selfdrive/ui/qt/util.h" -#include "selfdrive/ui/soundd/sound.h" - -int main(int argc, char **argv) { - qInstallMessageHandler(swagLogMessageHandler); - setpriority(PRIO_PROCESS, 0, -20); - - QApplication a(argc, argv); - std::signal(SIGINT, sigTermHandler); - std::signal(SIGTERM, sigTermHandler); - - Sound sound; - return a.exec(); -} diff --git a/selfdrive/ui/soundd/sound.cc b/selfdrive/ui/soundd/sound.cc deleted file mode 100644 index a5884f113f..0000000000 --- a/selfdrive/ui/soundd/sound.cc +++ /dev/null @@ -1,67 +0,0 @@ -#include "selfdrive/ui/soundd/sound.h" - -#include - -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/util.h" - -// TODO: detect when we can't play sounds -// TODO: detect when we can't display the UI - -Sound::Sound(QObject *parent) : sm({"controlsState", "microphone"}) { - qInfo() << "default audio device: " << QAudioDeviceInfo::defaultOutputDevice().deviceName(); - - for (auto &[alert, fn, loops, volume] : sound_list) { - QSoundEffect *s = new QSoundEffect(this); - QObject::connect(s, &QSoundEffect::statusChanged, [=]() { - assert(s->status() != QSoundEffect::Error); - }); - s->setSource(QUrl::fromLocalFile("../../assets/sounds/" + fn)); - s->setVolume(volume); - sounds[alert] = {s, loops}; - } - - QTimer *timer = new QTimer(this); - QObject::connect(timer, &QTimer::timeout, this, &Sound::update); - timer->start(1000 / UI_FREQ); -} - -void Sound::update() { - sm.update(0); - - // scale volume using ambient noise level - if (sm.updated("microphone")) { - float volume = util::map_val(sm["microphone"].getMicrophone().getFilteredSoundPressureWeightedDb(), 30.f, 60.f, 0.f, 1.f); - volume = QAudio::convertVolume(volume, QAudio::LogarithmicVolumeScale, QAudio::LinearVolumeScale); - // set volume on changes - if (std::exchange(current_volume, std::nearbyint(volume * 10)) != current_volume) { - Hardware::set_volume(volume); - } - } - - setAlert(Alert::get(sm, 0)); -} - -void Sound::setAlert(const Alert &alert) { - if (!current_alert.equal(alert)) { - current_alert = alert; - // stop sounds - for (auto &[s, loops] : sounds) { - // Only stop repeating sounds - if (s->loopsRemaining() > 1 || s->loopsRemaining() == QSoundEffect::Infinite) { - s->stop(); - } - } - - // play sound - if (alert.sound != AudibleAlert::NONE) { - auto &[s, loops] = sounds[alert.sound]; - s->setLoopCount(loops); - s->play(); - } - } -} diff --git a/selfdrive/ui/soundd/sound.h b/selfdrive/ui/soundd/sound.h deleted file mode 100644 index 4fcb2e1bce..0000000000 --- a/selfdrive/ui/soundd/sound.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include "system/hardware/hw.h" -#include "selfdrive/ui/ui.h" - - -const float MAX_VOLUME = 1.0; - -const std::tuple sound_list[] = { - // AudibleAlert, file name, loop count - {AudibleAlert::ENGAGE, "engage.wav", 0, MAX_VOLUME}, - {AudibleAlert::DISENGAGE, "disengage.wav", 0, MAX_VOLUME}, - {AudibleAlert::REFUSE, "refuse.wav", 0, MAX_VOLUME}, - - {AudibleAlert::PROMPT, "prompt.wav", 0, MAX_VOLUME}, - {AudibleAlert::PROMPT_REPEAT, "prompt.wav", QSoundEffect::Infinite, MAX_VOLUME}, - {AudibleAlert::PROMPT_DISTRACTED, "prompt_distracted.wav", QSoundEffect::Infinite, MAX_VOLUME}, - - {AudibleAlert::WARNING_SOFT, "warning_soft.wav", QSoundEffect::Infinite, MAX_VOLUME}, - {AudibleAlert::WARNING_IMMEDIATE, "warning_immediate.wav", QSoundEffect::Infinite, MAX_VOLUME}, -}; - -class Sound : public QObject { -public: - explicit Sound(QObject *parent = 0); - -protected: - void update(); - void setAlert(const Alert &alert); - - SubMaster sm; - Alert current_alert = {}; - QMap> sounds; - int current_volume = -1; -}; diff --git a/selfdrive/ui/soundd/soundd b/selfdrive/ui/soundd/soundd deleted file mode 100755 index 9b7a32fec9..0000000000 --- a/selfdrive/ui/soundd/soundd +++ /dev/null @@ -1,4 +0,0 @@ -#!/bin/sh -cd "$(dirname "$0")" -export QT_QPA_PLATFORM="offscreen" -exec ./_soundd diff --git a/selfdrive/ui/tests/test_sound.cc b/selfdrive/ui/tests/test_sound.cc deleted file mode 100644 index d9cb5c0a7f..0000000000 --- a/selfdrive/ui/tests/test_sound.cc +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include - -#include "catch2/catch.hpp" -#include "selfdrive/ui/soundd/sound.h" - -class TestSound : public Sound { -public: - TestSound() : Sound() { - for (auto i = sounds.constBegin(); i != sounds.constEnd(); ++i) { - sound_stats[i.key()] = {0, 0}; - QObject::connect(i.value().first, &QSoundEffect::playingChanged, [=, s = i.value().first, a = i.key()]() { - if (s->isPlaying()) { - sound_stats[a].first++; - } else { - sound_stats[a].second++; - } - }); - } - } - - QMap> sound_stats; -}; - -void controls_thread(int loop_cnt) { - PubMaster pm({"controlsState", "deviceState"}); - MessageBuilder deviceStateMsg; - auto deviceState = deviceStateMsg.initEvent().initDeviceState(); - deviceState.setStarted(true); - - const int DT_CTRL = 10; // ms - for (int i = 0; i < loop_cnt; ++i) { - for (auto &[alert, fn, loops, volume] : sound_list) { - printf("testing %s\n", qPrintable(fn)); - for (int j = 0; j < 1000 / DT_CTRL; ++j) { - MessageBuilder msg; - auto cs = msg.initEvent().initControlsState(); - cs.setAlertSound(alert); - cs.setAlertType(fn.toStdString()); - pm.send("controlsState", msg); - pm.send("deviceState", deviceStateMsg); - QThread::msleep(DT_CTRL); - } - } - } - - // send no alert sound - for (int j = 0; j < 1000 / DT_CTRL; ++j) { - MessageBuilder msg; - msg.initEvent().initControlsState(); - pm.send("controlsState", msg); - QThread::msleep(DT_CTRL); - } - - QThread::currentThread()->quit(); -} - -TEST_CASE("test soundd") { - QEventLoop loop; - TestSound test_sound; - const int test_loop_cnt = 2; - - QThread t; - QObject::connect(&t, &QThread::started, [=]() { controls_thread(test_loop_cnt); }); - QObject::connect(&t, &QThread::finished, [&]() { loop.quit(); }); - t.start(); - loop.exec(); - - for (const AudibleAlert alert : test_sound.sound_stats.keys()) { - auto [play, stop] = test_sound.sound_stats[alert]; - REQUIRE(play == test_loop_cnt); - REQUIRE(stop == test_loop_cnt); - } -} diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index 80a261e6d9..94ce26eb47 100755 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -1,75 +1,41 @@ #!/usr/bin/env python3 -import subprocess -import time import unittest -from cereal import log, car -import cereal.messaging as messaging -from openpilot.selfdrive.test.helpers import phone_only, with_processes -# TODO: rewrite for unittest -from openpilot.common.realtime import DT_CTRL -from openpilot.system.hardware import HARDWARE +from cereal import car +from cereal import messaging +from cereal.messaging import SubMaster, PubMaster +from openpilot.selfdrive.ui.soundd import CONTROLS_TIMEOUT, check_controls_timeout_alert -AudibleAlert = car.CarControl.HUDControl.AudibleAlert +import time -SOUNDS = { - # sound: total writes - AudibleAlert.none: 0, - AudibleAlert.engage: 184, - AudibleAlert.disengage: 186, - AudibleAlert.refuse: 194, - AudibleAlert.prompt: 184, - AudibleAlert.promptRepeat: 487, - AudibleAlert.promptDistracted: 508, - AudibleAlert.warningSoft: 471, - AudibleAlert.warningImmediate: 470, -} +AudibleAlert = car.CarControl.HUDControl.AudibleAlert -def get_total_writes(): - audio_flinger = subprocess.check_output('dumpsys media.audio_flinger', shell=True, encoding='utf-8').strip() - write_lines = [l for l in audio_flinger.split('\n') if l.strip().startswith('Total writes')] - return sum(int(l.split(':')[1]) for l in write_lines) class TestSoundd(unittest.TestCase): - def test_sound_card_init(self): - assert HARDWARE.get_sound_card_online() + def test_check_controls_timeout_alert(self): + sm = SubMaster(['controlsState']) + pm = PubMaster(['controlsState']) + + for _ in range(100): + cs = messaging.new_message('controlsState') + cs.controlsState.enabled = True + + pm.send("controlsState", cs) - @phone_only - @with_processes(['soundd']) - def test_alert_sounds(self): - pm = messaging.PubMaster(['deviceState', 'controlsState']) + time.sleep(0.01) - # make sure they're all defined - alert_sounds = {v: k for k, v in car.CarControl.HUDControl.AudibleAlert.schema.enumerants.items()} - diff = set(SOUNDS.keys()).symmetric_difference(alert_sounds.keys()) - assert len(diff) == 0, f"not all sounds defined in test: {diff}" + sm.update(0) - # wait for procs to init - time.sleep(1) + self.assertFalse(check_controls_timeout_alert(sm)) - for sound, expected_writes in SOUNDS.items(): - print(f"testing {alert_sounds[sound]}") - start_writes = get_total_writes() + for _ in range(CONTROLS_TIMEOUT * 110): + sm.update(0) + time.sleep(0.01) - for i in range(int(10 / DT_CTRL)): - msg = messaging.new_message('deviceState') - msg.deviceState.started = True - pm.send('deviceState', msg) + self.assertTrue(check_controls_timeout_alert(sm)) - msg = messaging.new_message('controlsState') - if i < int(6 / DT_CTRL): - msg.controlsState.alertSound = sound - msg.controlsState.alertType = str(sound) - msg.controlsState.alertText1 = "Testing Sounds" - msg.controlsState.alertText2 = f"playing {alert_sounds[sound]}" - msg.controlsState.alertSize = log.ControlsState.AlertSize.mid - pm.send('controlsState', msg) - time.sleep(DT_CTRL) + # TODO: add test with micd for checking that soundd actually outputs sounds - tolerance = expected_writes / 8 - actual_writes = get_total_writes() - start_writes - print(f" expected {expected_writes} writes, got {actual_writes}") - assert abs(expected_writes - actual_writes) <= tolerance, f"{alert_sounds[sound]}: expected {expected_writes} writes, got {actual_writes}" if __name__ == "__main__": unittest.main() diff --git a/system/hardware/base.h b/system/hardware/base.h index 890a743ea0..dc2282a93a 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -29,7 +29,6 @@ public: static void poweroff() {} static void set_brightness(int percent) {} static void set_display_power(bool on) {} - static void set_volume(float volume) {} static bool get_ssh_enabled() { return false; } static void set_ssh_enabled(bool enabled) {} diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h index 189adbbbee..5dea184ca6 100644 --- a/system/hardware/pc/hardware.h +++ b/system/hardware/pc/hardware.h @@ -13,14 +13,6 @@ public: static bool TICI() { return util::getenv("TICI", 0) == 1; } static bool AGNOS() { return util::getenv("TICI", 0) == 1; } - static void set_volume(float volume) { - volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); - - char volume_str[6]; - snprintf(volume_str, sizeof(volume_str), "%.3f", volume); - std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); - } - static void config_cpu_rendering(bool offscreen) { if (offscreen) { setenv("QT_QPA_PLATFORM", "offscreen", 1); diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index 0a00aca5be..f6ea86b002 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -67,14 +67,6 @@ public: bl_power_control.close(); } } - static void set_volume(float volume) { - volume = util::map_val(volume, 0.f, 1.f, MIN_VOLUME, MAX_VOLUME); - - char volume_str[6]; - snprintf(volume_str, sizeof(volume_str), "%.3f", volume); - std::system(("pactl set-sink-volume @DEFAULT_SINK@ " + std::string(volume_str)).c_str()); - } - static std::map get_init_logs() { std::map ret = { From 67d6186bbd5b19269162453a6028cabd6a2fabfc Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 5 Dec 2023 21:24:28 -0600 Subject: [PATCH 46/87] Toyota: LTA unit test (#30613) LTA unit test --- selfdrive/car/toyota/tests/test_toyota.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/selfdrive/car/toyota/tests/test_toyota.py b/selfdrive/car/toyota/tests/test_toyota.py index 2a82ca5ec0..eae59a861f 100755 --- a/selfdrive/car/toyota/tests/test_toyota.py +++ b/selfdrive/car/toyota/tests/test_toyota.py @@ -17,6 +17,10 @@ class TestToyotaInterfaces(unittest.TestCase): self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0) self.assertTrue(len(RADAR_ACC_CAR - TSS2_CAR) == 0) + def test_lta_platforms(self): + # At this time, only RAV4 2023 is expected to use LTA/angle control + self.assertEqual(ANGLE_CONTROL_CAR, {CAR.RAV4_TSS2_2023}) + def test_tss2_dbc(self): # We make some assumptions about TSS2 platforms, # like looking up certain signals only in this DBC From 3b89c5fe29b3a63f34e9782ef71ac868d89c99d5 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 5 Dec 2023 21:17:49 -0800 Subject: [PATCH 47/87] agnos 9 (#30543) * agnos 9 * new build * update * Soundd: use alsa (#30617) * update release notes * agnos does this now * prod manifest --------- Co-authored-by: Justin Newberry --- RELEASES.md | 1 + launch_env.sh | 2 +- selfdrive/test/setup_device_ci.sh | 1 - selfdrive/ui/soundd.py | 6 +--- system/hardware/tici/agnos.json | 48 +++++++++++++++---------------- 5 files changed, 27 insertions(+), 31 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 4c12971c54..6a531791df 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,5 +1,6 @@ Version 0.9.6 (2023-XX-XX) ======================== +* AGNOS 9 Version 0.9.5 (2023-11-17) ======================== diff --git a/launch_env.sh b/launch_env.sh index d07fbe33de..dd8f431c63 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="8.2" + export AGNOS_VERSION="9.1" fi if [ -z "$PASSIVE" ]; then diff --git a/selfdrive/test/setup_device_ci.sh b/selfdrive/test/setup_device_ci.sh index c34e097f73..a180fe0de6 100755 --- a/selfdrive/test/setup_device_ci.sh +++ b/selfdrive/test/setup_device_ci.sh @@ -29,7 +29,6 @@ sudo abctl --set_success # patch sshd config sudo mount -o rw,remount / -echo tici-$(cat /proc/cmdline | sed -e 's/^.*androidboot.serialno=//' -e 's/ .*$//') | sudo tee /etc/hostname sudo sed -i "s,/data/params/d/GithubSshKeys,/usr/comma/setup_keys," /etc/ssh/sshd_config sudo systemctl daemon-reload sudo systemctl restart ssh diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index 33fcf0be31..dac4bb74d8 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -1,7 +1,6 @@ import math import time import numpy as np -import os import wave from typing import Dict, Optional, Tuple @@ -57,9 +56,6 @@ class Soundd: self.controls_timeout_alert = False - if not PC: - os.system("pactl set-sink-volume @DEFAULT_SINK@ 0.9") # set to max volume and control volume within soundd - def load_sounds(self): self.loaded_sounds: Dict[int, np.ndarray] = {} @@ -134,7 +130,7 @@ class Soundd: if PC: device = None else: - device = "pulse" # "sdm845-tavil-snd-card: - (hw:0,0)" + device = "sdm845-tavil-snd-card: - (hw:0,0)" with sd.OutputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 7e8cd480be..080fa2cf8e 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -1,9 +1,9 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279.img.xz", - "hash": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279", - "hash_raw": "8d8d8620de8b2687f3a8fffdb81b2abd1fe2ead5bc831361a1a212e5589ac279", + "url": "https://commadist.azureedge.net/agnosupdate/boot-fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275.img.xz", + "hash": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275", + "hash_raw": "fd30f580375279ff4605034ec13711890a2b227205571a087cdc5226a2710275", "size": 15636480, "sparse": false, "full_check": true, @@ -11,9 +11,9 @@ }, { "name": "abl", - "url": "https://commadist.azureedge.net/agnosupdate/abl-0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d.img.xz", - "hash": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d", - "hash_raw": "0084fcf79fea067632a1c2d9519b6445ad484aa8b09f49f22e6b45b4dccacd2d", + "url": "https://commadist.azureedge.net/agnosupdate/abl-bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a.img.xz", + "hash": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a", + "hash_raw": "bb234733816781b3d09266f91f741436e9bf17e1a7caf468cf7d09ee788cef4a", "size": 274432, "sparse": false, "full_check": true, @@ -21,9 +21,9 @@ }, { "name": "xbl", - "url": "https://commadist.azureedge.net/agnosupdate/xbl-942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa.img.xz", - "hash": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa", - "hash_raw": "942b9b2914d89c2a70fdf27380b59e04b549ac2fd53ecb29d6549d1a9c8daeaa", + "url": "https://commadist.azureedge.net/agnosupdate/xbl-bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5.img.xz", + "hash": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5", + "hash_raw": "bcef195b00a1ab685da601f4072722569773ab161e91c8753ad99ca4217a28f5", "size": 3282672, "sparse": false, "full_check": true, @@ -31,9 +31,9 @@ }, { "name": "xbl_config", - "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf.img.xz", - "hash": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf", - "hash_raw": "6881d94599f65d94c13bcc0bd860184dfba2dfe96ec776d08fb35ac5b5f85bbf", + "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac.img.xz", + "hash": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac", + "hash_raw": "19791056558c16f8dae787531b5e30b3b3db2ded9d666688df45ce1b91a72bac", "size": 98124, "sparse": false, "full_check": true, @@ -41,9 +41,9 @@ }, { "name": "devcfg", - "url": "https://commadist.azureedge.net/agnosupdate/devcfg-9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262.img.xz", - "hash": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262", - "hash_raw": "9bbf168baff6101f4890c5c95c118e30813c2610cfb35b8e19e363f04a32a262", + "url": "https://commadist.azureedge.net/agnosupdate/devcfg-be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27.img.xz", + "hash": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27", + "hash_raw": "be44b73dda5be840b09d5347d536459e31098da3fea97596956c0bdad19bdf27", "size": 40336, "sparse": false, "full_check": true, @@ -51,9 +51,9 @@ }, { "name": "aop", - "url": "https://commadist.azureedge.net/agnosupdate/aop-c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222.img.xz", - "hash": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222", - "hash_raw": "c1d9d712980f6b2a4b12196597f4d1bf3fe4fec6c59edf29ae63ef21f11b8222", + "url": "https://commadist.azureedge.net/agnosupdate/aop-5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f.img.xz", + "hash": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f", + "hash_raw": "5d764611a683d6a738cf06a1dcf8a926d0f47b5117ad40d3054167de6dd8bd0f", "size": 184364, "sparse": false, "full_check": true, @@ -61,16 +61,16 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz", - "hash": "611011f3e3f147bc24f371105a9dd3760ec11ba424c56d4a442a66b098c784c0", - "hash_raw": "e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432", + "url": "https://commadist.azureedge.net/agnosupdate/system-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz", + "hash": "3b6cdf9bd881a5e90b21dd02c6faa923b415e32ecae9bfdc96753d4208fb82fe", + "hash_raw": "e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98", "size": 10737418240, "sparse": true, "full_check": false, "has_ab": true, "alt": { - "hash": "256442a55fcb9e8f72969f003a4db91598dee1136f8dda85b553a557d36b93d8", - "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1fa3018bce9bad01c6967e5e21f1141cf5c8f02d2edfaed51c738f74a32a432.img.xz" + "hash": "2fb81e58f4bc6c4e5e71c8e7ac7553f85082c430627d7a5cc54a6bbc82862500", + "url": "https://commadist.azureedge.net/agnosupdate/system-skip-chunks-e1952bb363688c0f5c0646e39bcdfb45be25b5e2baed37d1ba7801aa1a3a9c98.img.xz" } } -] +] \ No newline at end of file From 3985103974e59efc84a2ffc6e054b0f8daa6adf2 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 5 Dec 2023 21:32:55 -0800 Subject: [PATCH 48/87] update mapsd cpu usage --- selfdrive/test/test_onroad.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index f8d8db9cdc..2d674e4610 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -34,7 +34,7 @@ PROCS = { "./encoderd": 17.0, "./camerad": 14.5, "./locationd": 11.0, - "./mapsd": 1.5, + "./mapsd": (1.0, 10.0), "selfdrive.controls.plannerd": 16.5, "./_ui": 18.0, "selfdrive.locationd.paramsd": 9.0, From db35dcd0b5353d9c009fcf5817e611125a6b0b8b Mon Sep 17 00:00:00 2001 From: Greg Hogan Date: Wed, 6 Dec 2023 09:55:29 -0800 Subject: [PATCH 49/87] replace common.file_helpers.mkdirs_exists_ok with python os.makedirs funtion (#30618) replace common.file_helpers.mkdirs_exists_ok with python os.makedirs function --- common/file_helpers.py | 10 ---------- selfdrive/tombstoned.py | 3 +-- system/camerad/test/get_thumbnails_for_segment.py | 3 +-- tools/lib/auth_config.py | 3 +-- tools/lib/cache.py | 3 +-- tools/lib/url_file.py | 4 ++-- 6 files changed, 6 insertions(+), 20 deletions(-) diff --git a/common/file_helpers.py b/common/file_helpers.py index 227d614d72..a29eafdd9f 100644 --- a/common/file_helpers.py +++ b/common/file_helpers.py @@ -4,16 +4,6 @@ import tempfile from atomicwrites import AtomicWriter -def mkdirs_exists_ok(path): - if path.startswith(('http://', 'https://')): - raise ValueError('URL path') - try: - os.makedirs(path) - except OSError: - if not os.path.isdir(path): - raise - - def rm_not_exists_ok(path): try: os.remove(path) diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 3f2fb28d23..51e20dac49 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -9,7 +9,6 @@ import time import glob from typing import NoReturn -from openpilot.common.file_helpers import mkdirs_exists_ok import openpilot.selfdrive.sentry as sentry from openpilot.system.hardware.hw import Paths from openpilot.system.swaglog import cloudlog @@ -128,7 +127,7 @@ def report_tombstone_apport(fn): new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN] crashlog_dir = os.path.join(Paths.log_root(), "crash") - mkdirs_exists_ok(crashlog_dir) + os.makedirs(crashlog_dir, exist_ok=True) # Files could be on different filesystems, copy, then delete shutil.copy(fn, os.path.join(crashlog_dir, new_fn)) diff --git a/system/camerad/test/get_thumbnails_for_segment.py b/system/camerad/test/get_thumbnails_for_segment.py index 97667ede86..43f543f1bd 100755 --- a/system/camerad/test/get_thumbnails_for_segment.py +++ b/system/camerad/test/get_thumbnails_for_segment.py @@ -4,7 +4,6 @@ import os from tqdm import tqdm -from openpilot.common.file_helpers import mkdirs_exists_ok from openpilot.tools.lib.logreader import LogReader from openpilot.tools.lib.route import Route @@ -17,7 +16,7 @@ if __name__ == "__main__": args = parser.parse_args() out_path = os.path.join("jpegs", f"{args.route.replace('|', '_')}_{args.segment}") - mkdirs_exists_ok(out_path) + os.makedirs(out_path, exist_ok=True) r = Route(args.route) path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment] diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py index e0989f02ea..c4e7b6261b 100644 --- a/tools/lib/auth_config.py +++ b/tools/lib/auth_config.py @@ -1,6 +1,5 @@ import json import os -from openpilot.common.file_helpers import mkdirs_exists_ok from openpilot.system.hardware.hw import Paths @@ -18,7 +17,7 @@ def get_token(): def set_token(token): - mkdirs_exists_ok(Paths.config_root()) + os.makedirs(Paths.config_root(), exist_ok=True) with open(os.path.join(Paths.config_root(), 'auth.json'), 'w') as f: json.dump({'access_token': token}, f) diff --git a/tools/lib/cache.py b/tools/lib/cache.py index fd214f6bb5..d5fa288baa 100644 --- a/tools/lib/cache.py +++ b/tools/lib/cache.py @@ -1,12 +1,11 @@ import os import urllib.parse -from openpilot.common.file_helpers import mkdirs_exists_ok DEFAULT_CACHE_DIR = os.getenv("CACHE_ROOT", os.path.expanduser("~/.commacache")) def cache_path_for_file_path(fn, cache_dir=DEFAULT_CACHE_DIR): dir_ = os.path.join(cache_dir, "local") - mkdirs_exists_ok(dir_) + os.makedirs(dir_, exist_ok=True) fn_parsed = urllib.parse.urlparse(fn) if fn_parsed.scheme == '': cache_fn = os.path.abspath(fn).replace("/", "_") diff --git a/tools/lib/url_file.py b/tools/lib/url_file.py index 315ade514b..a54552d241 100644 --- a/tools/lib/url_file.py +++ b/tools/lib/url_file.py @@ -5,7 +5,7 @@ import pycurl from hashlib import sha256 from io import BytesIO from tenacity import retry, wait_random_exponential, stop_after_attempt -from openpilot.common.file_helpers import mkdirs_exists_ok, atomic_write_in_dir +from openpilot.common.file_helpers import atomic_write_in_dir from openpilot.system.hardware.hw import Paths # Cache chunk size K = 1000 @@ -40,7 +40,7 @@ class URLFile: except AttributeError: self._curl = self._tlocal.curl = pycurl.Curl() if not self._force_download: - mkdirs_exists_ok(Paths.download_cache_root()) + os.makedirs(Paths.download_cache_root(), exist_ok=True) def __enter__(self): return self From 31ab43ce41d6d0f88cb8a9b6df8e79f23b6309fe Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 11:13:22 -0800 Subject: [PATCH 50/87] jenkins: remove pytest tici conf (#30621) * remove pytest conf * remove that too --- Jenkinsfile | 3 ++- release/files_common | 1 - selfdrive/test/pytest-tici.ini | 5 ----- 3 files changed, 2 insertions(+), 7 deletions(-) delete mode 100644 selfdrive/test/pytest-tici.ini diff --git a/Jenkinsfile b/Jenkinsfile index 3dd9c45f14..fbe9cb48a4 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -14,7 +14,8 @@ export GIT_BRANCH=${env.GIT_BRANCH} export GIT_COMMIT=${env.GIT_COMMIT} export AZURE_TOKEN='${env.AZURE_TOKEN}' export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}' -export PYTEST_ADDOPTS="-c selfdrive/test/pytest-tici.ini --rootdir ." +# only use 1 thread for tici tests since most require HIL +export PYTEST_ADDOPTS="-n 1" export GIT_SSH_COMMAND="ssh -i /data/gitkey" diff --git a/release/files_common b/release/files_common index ddc3e31d4d..ff2d8bec28 100644 --- a/release/files_common +++ b/release/files_common @@ -297,7 +297,6 @@ selfdrive/test/helpers.py selfdrive/test/setup_device_ci.sh selfdrive/test/test_onroad.py selfdrive/test/test_time_to_onroad.py -selfdrive/test/pytest-tici.ini selfdrive/ui/.gitignore selfdrive/ui/SConscript diff --git a/selfdrive/test/pytest-tici.ini b/selfdrive/test/pytest-tici.ini deleted file mode 100644 index 98e75d0661..0000000000 --- a/selfdrive/test/pytest-tici.ini +++ /dev/null @@ -1,5 +0,0 @@ -[pytest] -addopts = -Werror --strict-config --strict-markers --durations=10 -markers = - slow: tests that take awhile to run and can be skipped with -m 'not slow' - tici: tests that are only meant to run on the C3/C3X From 5600a8288909e0b6ca95becbdb5746a0a9e35af4 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 14:59:16 -0800 Subject: [PATCH 51/87] Soundd: only update ambient db when not playing an alert (#30620) * move to soundd + only when quiet * not filtered --- selfdrive/ui/soundd.py | 14 +++++++++++--- system/micd.py | 6 ------ 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index dac4bb74d8..a3dc6fedfe 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -1,12 +1,16 @@ import math -import time import numpy as np +import time import wave from typing import Dict, Optional, Tuple from cereal import car, messaging from openpilot.common.basedir import BASEDIR +from openpilot.common.filter_simple import FirstOrderFilter + +from openpilot.system import micd + from openpilot.common.realtime import Ratekeeper from openpilot.system.hardware import PC from openpilot.system.swaglog import cloudlog @@ -15,6 +19,7 @@ SAMPLE_RATE = 48000 MAX_VOLUME = 1.0 MIN_VOLUME = 0.1 CONTROLS_TIMEOUT = 5 # 5 seconds +FILTER_DT = 1. / (micd.SAMPLE_RATE / micd.FFT_SAMPLES) AMBIENT_DB = 30 # DB where MIN_VOLUME is applied DB_SCALE = 30 # AMBIENT_DB + DB_SCALE is where MAX_VOLUME is applied @@ -56,6 +61,8 @@ class Soundd: self.controls_timeout_alert = False + self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) + def load_sounds(self): self.loaded_sounds: Dict[int, np.ndarray] = {} @@ -137,8 +144,9 @@ class Soundd: while True: sm.update(0) - if sm.updated['microphone']: - self.current_volume = self.calculate_volume(sm["microphone"].filteredSoundPressureWeightedDb) + if sm.updated['microphone'] and self.current_alert == AudibleAlert.none: # only update volume filter when not playing alert + self.spl_filter_weighted.update(sm["microphone"].soundPressureWeightedDb) + self.current_volume = self.calculate_volume(float(self.spl_filter_weighted.x)) self.get_audible_alert(sm) diff --git a/system/micd.py b/system/micd.py index 0b7a4dadfb..452c04bc03 100755 --- a/system/micd.py +++ b/system/micd.py @@ -2,7 +2,6 @@ import numpy as np from cereal import messaging -from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import Ratekeeper from openpilot.system.swaglog import cloudlog @@ -10,7 +9,6 @@ RATE = 10 FFT_SAMPLES = 4096 REFERENCE_SPL = 2e-5 # newtons/m^2 SAMPLE_RATE = 44100 -FILTER_DT = 1. / (SAMPLE_RATE / FFT_SAMPLES) def calculate_spl(measurements): @@ -50,15 +48,12 @@ class Mic: self.sound_pressure_weighted = 0 self.sound_pressure_level_weighted = 0 - self.spl_filter_weighted = FirstOrderFilter(0, 2.5, FILTER_DT, initialized=False) - def update(self): msg = messaging.new_message('microphone', valid=True) msg.microphone.soundPressure = float(self.sound_pressure) msg.microphone.soundPressureWeighted = float(self.sound_pressure_weighted) msg.microphone.soundPressureWeightedDb = float(self.sound_pressure_level_weighted) - msg.microphone.filteredSoundPressureWeightedDb = float(self.spl_filter_weighted.x) self.pm.send('microphone', msg) self.rk.keep_time() @@ -79,7 +74,6 @@ class Mic: self.sound_pressure, _ = calculate_spl(measurements) measurements_weighted = apply_a_weighting(measurements) self.sound_pressure_weighted, self.sound_pressure_level_weighted = calculate_spl(measurements_weighted) - self.spl_filter_weighted.update(self.sound_pressure_level_weighted) self.measurements = self.measurements[FFT_SAMPLES:] From 492ba68cfcf71fa084c765c49b4357b769cbe70e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 6 Dec 2023 15:32:46 -0800 Subject: [PATCH 52/87] Update RELEASES.md --- RELEASES.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/RELEASES.md b/RELEASES.md index 6a531791df..28c3da70a2 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,6 +1,7 @@ -Version 0.9.6 (2023-XX-XX) +Version 0.9.6 (2023-12-14) ======================== * AGNOS 9 +* comma body streaming and controls over WebRTC Version 0.9.5 (2023-11-17) ======================== From 9dc8ecf722733d645e0a1492c7aff1e73908ab54 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 15:33:17 -0800 Subject: [PATCH 53/87] ruff: ignore teleoprtc repo (#30627) ignore it --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index 0e55e2ac60..ffbd5d43b7 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -190,6 +190,7 @@ exclude = [ "rednose_repo", "tinygrad_repo", "teleoprtc", + "teleoprtc_repo", "third_party", ] flake8-implicit-str-concat.allow-multiline=false From 3777bf7fb9b6d63f49118d010ac13cee89d13d06 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 15:51:29 -0800 Subject: [PATCH 54/87] jenkins: disable xdist on tici (#30624) to preserve it --- Jenkinsfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Jenkinsfile b/Jenkinsfile index fbe9cb48a4..446435f7fe 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -15,7 +15,7 @@ export GIT_COMMIT=${env.GIT_COMMIT} export AZURE_TOKEN='${env.AZURE_TOKEN}' export MAPBOX_TOKEN='${env.MAPBOX_TOKEN}' # only use 1 thread for tici tests since most require HIL -export PYTEST_ADDOPTS="-n 1" +export PYTEST_ADDOPTS="-n 0" export GIT_SSH_COMMAND="ssh -i /data/gitkey" From f16df8e4b7901f5e38b6f90a8900eef02faab504 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 16:00:59 -0800 Subject: [PATCH 55/87] pytest: group test_models by route instead of car name (#30625) * group by route * dont need those --- Jenkinsfile | 4 ++-- conftest.py | 5 +++-- selfdrive/car/tests/test_models.py | 2 +- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 446435f7fe..ec39882201 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -235,8 +235,8 @@ node { pcStage("car tests") { sh label: "build", script: "selfdrive/manager/build.py" sh label: "test_models.py", script: "INTERNAL_SEG_CNT=250 INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt FILEREADER_CACHE=1 \ - pytest -n auto --dist=loadscope selfdrive/car/tests/test_models.py" - sh label: "test_car_interfaces.py", script: "MAX_EXAMPLES=100 pytest -n auto --dist=load selfdrive/car/tests/test_car_interfaces.py" + pytest selfdrive/car/tests/test_models.py" + sh label: "test_car_interfaces.py", script: "MAX_EXAMPLES=100 pytest selfdrive/car/tests/test_car_interfaces.py" } }, diff --git a/conftest.py b/conftest.py index 9aaf04d798..d1787c24ef 100644 --- a/conftest.py +++ b/conftest.py @@ -45,8 +45,9 @@ def pytest_collection_modifyitems(config, items): item.add_marker(skipper) if "xdist_group_class_property" in item.keywords: - class_property = item.get_closest_marker('xdist_group_class_property').args[0] - item.add_marker(pytest.mark.xdist_group(getattr(item.cls, class_property))) + class_property_name = item.get_closest_marker('xdist_group_class_property').args[0] + class_property_value = getattr(item.cls, class_property_name) + item.add_marker(pytest.mark.xdist_group(class_property_value)) @pytest.hookimpl(trylast=True) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index f26fd363ca..0ae104f9f5 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -382,7 +382,7 @@ class TestCarModelBase(unittest.TestCase): @parameterized_class(('car_model', 'test_route'), get_test_cases()) -@pytest.mark.xdist_group_class_property('car_model') +@pytest.mark.xdist_group_class_property('test_route') class TestCarModel(TestCarModelBase): pass From 36bebb1aa064b4b40580aa48d35b21bd9ebe14c2 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 6 Dec 2023 17:22:32 -0800 Subject: [PATCH 56/87] jenkins: retry build_devel (#30628) * will it work * retry build devel * correct p[ath * try a failure * Revert "try a failure" This reverts commit ba4e6a0a2096b94887fcfbf7d011e34aa2a16c41. --- Jenkinsfile | 2 +- release/build_devel.sh | 1 + scripts/retry.sh | 27 +++++++++++++++++++++++++++ 3 files changed, 29 insertions(+), 1 deletion(-) create mode 100755 scripts/retry.sh diff --git a/Jenkinsfile b/Jenkinsfile index ec39882201..840dd29503 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -158,7 +158,7 @@ node { // tici tests 'onroad tests': { deviceStage("onroad", "tici-needs-can", ["SKIP_COPY=1"], [ - ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR ./build_devel.sh"], + ["build master-ci", "cd $SOURCE_DIR/release && TARGET_DIR=$TEST_DIR $SOURCE_DIR/scripts/retry.sh ./build_devel.sh"], ["build openpilot", "cd selfdrive/manager && ./build.py"], ["check dirty", "release/check-dirty.sh"], ["onroad tests", "pytest selfdrive/test/test_onroad.py -s"], diff --git a/release/build_devel.sh b/release/build_devel.sh index ca04c56f1e..8b6816e423 100755 --- a/release/build_devel.sh +++ b/release/build_devel.sh @@ -22,6 +22,7 @@ pre-commit uninstall || true echo "[-] bringing master-ci and devel in sync T=$SECONDS" cd $TARGET_DIR + git fetch --depth 1 origin master-ci git fetch --depth 1 origin devel diff --git a/scripts/retry.sh b/scripts/retry.sh new file mode 100755 index 0000000000..4861748e55 --- /dev/null +++ b/scripts/retry.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +function fail { + echo $1 >&2 + exit 1 +} + +function retry { + local n=1 + local max=3 # 3 retries before failure + local delay=5 # delay between retries, 5 seconds + while true; do + echo "Running command '$@' with retry, attempt $n/$max" + "$@" && break || { + if [[ $n -lt $max ]]; then + ((n++)) + sleep $delay; + else + fail "The command has failed after $n attempts." + fi + } + done +} + +if [[ "${BASH_SOURCE[0]}" == "${0}" ]]; then + retry "$@" +fi From 35f819c8239b8ac3a000087a0f791b91ccea5e5e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 6 Dec 2023 17:27:51 -0800 Subject: [PATCH 57/87] swaglog.py goes in common/ (#30631) * swaglog.py goes in common/ * all of these go in release * we'll even include the pyx --- {system => common}/swaglog.py | 0 release/files_common | 21 ++----------------- selfdrive/athena/athenad.py | 2 +- selfdrive/athena/manage_athenad.py | 2 +- selfdrive/athena/registration.py | 2 +- selfdrive/boardd/pandad.py | 2 +- selfdrive/car/car_helpers.py | 2 +- selfdrive/car/disable_ecu.py | 2 +- selfdrive/car/ecu_addrs.py | 2 +- selfdrive/car/fw_versions.py | 2 +- selfdrive/car/isotp_parallel_query.py | 2 +- selfdrive/car/vin.py | 2 +- selfdrive/controls/controlsd.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/controls/radard.py | 2 +- selfdrive/locationd/calibrationd.py | 2 +- selfdrive/locationd/helpers.py | 2 +- selfdrive/locationd/models/car_kf.py | 2 +- selfdrive/locationd/paramsd.py | 2 +- selfdrive/locationd/torqued.py | 2 +- selfdrive/manager/build.py | 2 +- selfdrive/manager/manager.py | 2 +- selfdrive/manager/process.py | 2 +- selfdrive/modeld/dmonitoringmodeld.py | 2 +- selfdrive/modeld/modeld.py | 2 +- selfdrive/modeld/navmodeld.py | 2 +- selfdrive/navd/navd.py | 2 +- selfdrive/sentry.py | 2 +- selfdrive/statsd.py | 2 +- selfdrive/thermald/fan_controller.py | 2 +- selfdrive/thermald/power_monitoring.py | 2 +- selfdrive/thermald/thermald.py | 2 +- selfdrive/tombstoned.py | 2 +- selfdrive/ui/soundd.py | 2 +- selfdrive/updated.py | 2 +- system/loggerd/deleter.py | 2 +- system/loggerd/tests/test_uploader.py | 2 +- system/loggerd/uploader.py | 2 +- system/logmessaged.py | 2 +- system/micd.py | 2 +- system/qcomgpsd/qcomgpsd.py | 2 +- system/sensord/pigeond.py | 2 +- system/tests/test_logmessaged.py | 2 +- system/timezoned.py | 2 +- system/version.py | 2 +- 47 files changed, 47 insertions(+), 64 deletions(-) rename {system => common}/swaglog.py (100%) diff --git a/system/swaglog.py b/common/swaglog.py similarity index 100% rename from system/swaglog.py rename to common/swaglog.py diff --git a/release/files_common b/release/files_common index ff2d8bec28..7257fb820d 100644 --- a/release/files_common +++ b/release/files_common @@ -21,24 +21,8 @@ openpilot/** common/.gitignore common/__init__.py -common/conversions.py -common/gpio.py -common/realtime.py -common/timeout.py -common/ffi_wrapper.py -common/file_helpers.py -common/logging_extra.py -common/numpy_fast.py -common/params.py -common/params_pyx.pyx -common/profiler.py -common/basedir.py -common/dict_helpers.py -common/filter_simple.py -common/stat_live.py -common/spinner.py -common/text_window.py -common/time.py +common/*.py +common/*.pyx common/kalman/.gitignore common/kalman/* @@ -76,7 +60,6 @@ selfdrive/statsd.py system/logmessaged.py system/micd.py -system/swaglog.py system/version.py selfdrive/athena/__init__.py diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 423479517c..936797a89e 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -36,7 +36,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import set_core_affinity from openpilot.system.hardware import HARDWARE, PC, AGNOS from openpilot.system.loggerd.xattr_cache import getxattr, setxattr -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_commit, get_origin, get_short_branch, get_version from openpilot.system.hardware.hw import Paths diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py index 877d8aca03..2a4a12e559 100755 --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -5,7 +5,7 @@ from multiprocessing import Process from openpilot.common.params import Params from openpilot.selfdrive.manager.process import launcher -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_version, is_dirty ATHENA_MGR_PID_PARAM = "AthenadPid" diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index 7467e7fa86..c9a4b949ac 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -12,7 +12,7 @@ from openpilot.common.spinner import Spinner from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, PC from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog UNREGISTERED_DONGLE_ID = "UnregisteredDevice" diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 14d272965d..672678778b 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -12,7 +12,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.selfdrive.boardd.set_time import set_time from openpilot.system.hardware import HARDWARE -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog def get_expected_signature(panda: Panda) -> bytes: diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 5c1234258e..fa68313e86 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -10,7 +10,7 @@ from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog import cereal.messaging as messaging from openpilot.selfdrive.car import gen_empty_fingerprint diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py index c11075342c..1f731f0344 100755 --- a/selfdrive/car/disable_ecu.py +++ b/selfdrive/car/disable_ecu.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog EXT_DIAG_REQUEST = b'\x10\x03' EXT_DIAG_RESPONSE = b'\x50\x03' diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index cff1df79a5..13f7926def 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -8,7 +8,7 @@ from panda.python.uds import SERVICE_TYPE from openpilot.selfdrive.car import make_can_msg from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog def make_tester_present_msg(addr, bus, subaddr=None): diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 45c4967cb8..f50c604316 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -12,7 +12,7 @@ from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusTyp from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import FW_VERSIONS from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog Ecu = car.CarParams.Ecu ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa] diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index adbc598ea6..696935fd35 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -3,7 +3,7 @@ from collections import defaultdict from functools import partial import cereal.messaging as messaging -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 28edf157ae..e2709cc842 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -5,7 +5,7 @@ import cereal.messaging as messaging from panda.python.uds import get_rx_addr_for_tx_addr, FUNCTIONAL_ADDRS from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from openpilot.selfdrive.car.fw_query_definitions import StdQueries -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog VIN_UNKNOWN = "0" * 17 VIN_RE = "[A-HJ-NPR-Z0-9]{17}" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6f2ae6aa3a..a180b4c410 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -13,7 +13,7 @@ import cereal.messaging as messaging from cereal.visionipc import VisionIpcClient, VisionStreamType from openpilot.common.conversions import Conversions as CV from panda import ALTERNATIVE_EXPERIENCE -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_short_branch from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp from openpilot.selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a23ba1eaf6..39cfda4e8a 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -4,7 +4,7 @@ import time import numpy as np from cereal import log from openpilot.common.numpy_fast import clip -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from openpilot.selfdrive.modeld.constants import index_function from openpilot.selfdrive.car.interfaces import ACCEL_MIN diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b1eff5302f..13127b4634 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,7 +15,7 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 15dbd4c5e2..46d10ef2f5 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -4,7 +4,7 @@ import numpy as np from cereal import car from openpilot.common.params import Params from openpilot.common.realtime import Priority, config_realtime_process -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner from openpilot.selfdrive.controls.lib.lateral_planner import LateralPlanner diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a2d58cfed2..f75b4b2059 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -9,7 +9,7 @@ from cereal import messaging, log, car from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import Ratekeeper, Priority, config_realtime_process -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.common.kalman.simple_kalman import KF1D diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 654c72528b..961efabf8d 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -18,7 +18,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.params import Params, put_nonblocking from openpilot.common.realtime import set_realtime_priority from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS MAX_VEL_ANGLE_STD = np.radians(0.25) diff --git a/selfdrive/locationd/helpers.py b/selfdrive/locationd/helpers.py index bde21f7879..d57f55d588 100644 --- a/selfdrive/locationd/helpers.py +++ b/selfdrive/locationd/helpers.py @@ -5,7 +5,7 @@ from typing import List, Optional, Tuple, Any from cereal import log from openpilot.common.params import Params -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog class NPQueue: diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index b87c83cac2..9230cb48f0 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -7,7 +7,7 @@ import numpy as np from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.models.constants import ObservationKind -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from rednose.helpers.kalmanfilter import KalmanFilter diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0e779c9e3e..4a598e8d4b 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -12,7 +12,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.numpy_fast import clip from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index ff1fac2c22..d0a03f9b80 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -10,7 +10,7 @@ from cereal import car, log from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, cache_points_onexit diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index 247f3c8fb8..836723e5a8 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -9,7 +9,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.spinner import Spinner from openpilot.common.text_window import TextWindow from openpilot.system.hardware import AGNOS -from openpilot.system.swaglog import cloudlog, add_file_handler +from openpilot.common.swaglog import cloudlog, add_file_handler from openpilot.system.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 7b23445c8b..37947a9a03 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -19,7 +19,7 @@ from openpilot.selfdrive.manager.helpers import unblock_stdout, write_onroad_par from openpilot.selfdrive.manager.process import ensure_running from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID -from openpilot.system.swaglog import cloudlog, add_file_handler +from openpilot.common.swaglog import cloudlog, add_file_handler from openpilot.system.version import is_dirty, get_commit, get_version, get_origin, get_short_branch, \ get_normalized_origin, terms_version, training_version, \ is_tested_branch, is_release_branch diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 82cc8d74a3..523e1fd209 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -15,7 +15,7 @@ import cereal.messaging as messaging import openpilot.selfdrive.sentry as sentry from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog WATCHDOG_FN = "/dev/shm/wd_" ENABLE_WATCHDOG = os.getenv("NO_WATCHDOG") is None diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index aafe78abde..1e25964702 100755 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -11,7 +11,7 @@ from typing import Tuple, Dict from cereal import messaging from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 2b211b22e6..9cc238530a 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -9,7 +9,7 @@ from typing import Dict, Optional from setproctitle import setproctitle from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType, VisionBuf -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.common.numpy_fast import interp diff --git a/selfdrive/modeld/navmodeld.py b/selfdrive/modeld/navmodeld.py index 90b9762800..ed0b597dfe 100755 --- a/selfdrive/modeld/navmodeld.py +++ b/selfdrive/modeld/navmodeld.py @@ -10,7 +10,7 @@ from typing import Tuple, Dict from cereal import messaging from cereal.messaging import PubMaster, SubMaster from cereal.visionipc import VisionIpcClient, VisionStreamType -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params from openpilot.common.realtime import set_realtime_priority from openpilot.selfdrive.modeld.constants import ModelConstants diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 0d71efcc8d..fcaf46aff4 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -15,7 +15,7 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, distance_along_geometry, maxspeed_to_ms, minimum_distance, parse_banner_instructions) -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py index b8bc8eff12..f4bb1fbce8 100644 --- a/selfdrive/sentry.py +++ b/selfdrive/sentry.py @@ -6,7 +6,7 @@ from sentry_sdk.integrations.threading import ThreadingIntegration from openpilot.common.params import Params from openpilot.selfdrive.athena.registration import is_registered_device from openpilot.system.hardware import HARDWARE, PC -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_branch, get_commit, get_origin, get_version, \ is_comma_remote, is_dirty, is_tested_branch diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index e41f41a621..94572b82c7 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -10,7 +10,7 @@ from typing import NoReturn, Union, List, Dict from openpilot.common.params import Params from cereal.messaging import SubMaster from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import HARDWARE from openpilot.common.file_helpers import atomic_write_in_dir from openpilot.system.version import get_normalized_origin, get_short_branch, get_short_version, is_dirty diff --git a/selfdrive/thermald/fan_controller.py b/selfdrive/thermald/fan_controller.py index 64cd4c78ee..19c3292ce2 100755 --- a/selfdrive/thermald/fan_controller.py +++ b/selfdrive/thermald/fan_controller.py @@ -3,7 +3,7 @@ from abc import ABC, abstractmethod from openpilot.common.realtime import DT_TRML from openpilot.common.numpy_fast import interp -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.pid import PIDController class BaseFanController(ABC): diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 0b3c73a1c0..0520385473 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -4,7 +4,7 @@ from typing import Optional from openpilot.common.params import Params, put_nonblocking from openpilot.system.hardware import HARDWARE -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.statsd import statlog CAR_VOLTAGE_LOW_PASS_K = 0.011 # LPF gain for 45s tau (dt/tau / (dt/tau + 1)) diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 690618d0b8..8b51aedbdb 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -23,7 +23,7 @@ from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.hardware import HARDWARE, TICI, AGNOS from openpilot.system.loggerd.config import get_available_percent from openpilot.selfdrive.statsd import statlog -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.thermald.power_monitoring import PowerMonitoring from openpilot.selfdrive.thermald.fan_controller import TiciFanController from openpilot.system.version import terms_version, training_version diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 51e20dac49..5a81e93b28 100755 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -11,7 +11,7 @@ from typing import NoReturn import openpilot.selfdrive.sentry as sentry from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_commit MAX_SIZE = 1_000_000 * 100 # allow up to 100M diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index a3dc6fedfe..d64fb0010c 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -13,7 +13,7 @@ from openpilot.system import micd from openpilot.common.realtime import Ratekeeper from openpilot.system.hardware import PC -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog SAMPLE_RATE = 48000 MAX_VOLUME = 1.0 diff --git a/selfdrive/updated.py b/selfdrive/updated.py index fabe28920f..db6a06dc9e 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -18,7 +18,7 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.common.time import system_time_valid from openpilot.system.hardware import AGNOS, HARDWARE -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.controls.lib.alertmanager import set_offroad_alert from openpilot.system.version import is_tested_branch diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py index b060232b6e..868340150a 100755 --- a/system/loggerd/deleter.py +++ b/system/loggerd/deleter.py @@ -4,7 +4,7 @@ import shutil import threading from typing import List from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.loggerd.config import get_available_bytes, get_available_percent from openpilot.system.loggerd.uploader import listdir_by_creation from openpilot.system.loggerd.xattr_cache import getxattr diff --git a/system/loggerd/tests/test_uploader.py b/system/loggerd/tests/test_uploader.py index a7349fc5e4..538d99f66f 100755 --- a/system/loggerd/tests/test_uploader.py +++ b/system/loggerd/tests/test_uploader.py @@ -9,7 +9,7 @@ from pathlib import Path from typing import List, Optional from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.loggerd.uploader import uploader_fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE from openpilot.system.loggerd.tests.loggerd_tests_common import UploaderTestCase diff --git a/system/loggerd/uploader.py b/system/loggerd/uploader.py index b5804b1624..4bc031a632 100755 --- a/system/loggerd/uploader.py +++ b/system/loggerd/uploader.py @@ -19,7 +19,7 @@ from openpilot.common.realtime import set_core_affinity from openpilot.system.hardware import TICI from openpilot.system.hardware.hw import Paths from openpilot.system.loggerd.xattr_cache import getxattr, setxattr -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog NetworkType = log.DeviceState.NetworkType UPLOAD_ATTR_NAME = 'user.upload' diff --git a/system/logmessaged.py b/system/logmessaged.py index 5bd83f4b05..46bf79b0b2 100755 --- a/system/logmessaged.py +++ b/system/logmessaged.py @@ -5,7 +5,7 @@ from typing import NoReturn import cereal.messaging as messaging from openpilot.common.logging_extra import SwagLogFileFormatter from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import get_file_handler +from openpilot.common.swaglog import get_file_handler def main() -> NoReturn: diff --git a/system/micd.py b/system/micd.py index 452c04bc03..88fb08d684 100755 --- a/system/micd.py +++ b/system/micd.py @@ -3,7 +3,7 @@ import numpy as np from cereal import messaging from openpilot.common.realtime import Ratekeeper -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog RATE = 10 FFT_SAMPLES = 4096 diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index 78539266c0..ae58c04a3b 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -17,7 +17,7 @@ from cereal import log import cereal.messaging as messaging from openpilot.common.gpio import gpio_init, gpio_set from openpilot.system.hardware.tici.pins import GPIO -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv from openpilot.system.qcomgpsd.structs import (dict_unpacker, position_report, relist, gps_measurement_report, gps_measurement_report_sv, diff --git a/system/sensord/pigeond.py b/system/sensord/pigeond.py index 6a02f627b9..78b3b07498 100755 --- a/system/sensord/pigeond.py +++ b/system/sensord/pigeond.py @@ -11,7 +11,7 @@ from typing import List, Optional, Tuple from cereal import messaging from openpilot.common.params import Params -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.hardware import TICI from openpilot.common.gpio import gpio_init, gpio_set from openpilot.system.hardware.tici.pins import GPIO diff --git a/system/tests/test_logmessaged.py b/system/tests/test_logmessaged.py index 5d0e1bfc82..d27dae46ad 100755 --- a/system/tests/test_logmessaged.py +++ b/system/tests/test_logmessaged.py @@ -7,7 +7,7 @@ import unittest import cereal.messaging as messaging from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.system.hardware.hw import Paths -from openpilot.system.swaglog import cloudlog, ipchandler +from openpilot.common.swaglog import cloudlog, ipchandler class TestLogmessaged(unittest.TestCase): diff --git a/system/timezoned.py b/system/timezoned.py index 91424d33b5..546405c9a0 100755 --- a/system/timezoned.py +++ b/system/timezoned.py @@ -10,7 +10,7 @@ from timezonefinder import TimezoneFinder from openpilot.common.params import Params from openpilot.system.hardware import AGNOS -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog from openpilot.system.version import get_version REQUEST_HEADERS = {'User-Agent': "openpilot-" + get_version()} diff --git a/system/version.py b/system/version.py index 82e99fd416..f9fd2bc847 100755 --- a/system/version.py +++ b/system/version.py @@ -5,7 +5,7 @@ from typing import List, Optional from functools import lru_cache from openpilot.common.basedir import BASEDIR -from openpilot.system.swaglog import cloudlog +from openpilot.common.swaglog import cloudlog RELEASE_BRANCHES = ['release3-staging', 'dashcam3-staging', 'release3', 'dashcam3', 'nightly'] TESTED_BRANCHES = RELEASE_BRANCHES + ['devel', 'devel-staging'] From 51fa7b227afbc1134bf4860eaee819cdc64174d2 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 6 Dec 2023 19:55:27 -0800 Subject: [PATCH 58/87] qcomgpsd: move retry logic to a decorator (#30633) * qcomgpsd: move retry logic to a decorator * make that same * fix * fix that --- common/retry.py | 30 +++++++++++++++++++++++++ system/qcomgpsd/qcomgpsd.py | 45 +++++++++---------------------------- 2 files changed, 40 insertions(+), 35 deletions(-) create mode 100644 common/retry.py diff --git a/common/retry.py b/common/retry.py new file mode 100644 index 0000000000..9bd4ac9522 --- /dev/null +++ b/common/retry.py @@ -0,0 +1,30 @@ +import time +import functools + +from openpilot.common.swaglog import cloudlog + + +def retry(attempts=3, delay=1.0, ignore_failure=False): + def decorator(func): + @functools.wraps(func) + def wrapper(*args, **kwargs): + for _ in range(attempts): + try: + return func(*args, **kwargs) + except Exception: + cloudlog.exception(f"{func.__name__} failed, trying again") + time.sleep(delay) + + if ignore_failure: + cloudlog.error(f"{func.__name__} failed after retry") + else: + raise Exception(f"{func.__name__} failed after retry") + return wrapper + return decorator + + +if __name__ == "__main__": + @retry(attempts=10) + def abc(): + raise ValueError("abc failed :(") + abc() diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index ae58c04a3b..007c61552e 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -16,6 +16,7 @@ from struct import unpack_from, calcsize, pack from cereal import log import cereal.messaging as messaging from openpilot.common.gpio import gpio_init, gpio_set +from openpilot.common.retry import retry from openpilot.system.hardware.tici.pins import GPIO from openpilot.common.swaglog import cloudlog from openpilot.system.qcomgpsd.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv @@ -84,27 +85,13 @@ measurementStatusGlonassFields = { "glonassTimeMarkValid": 17 } +@retry(attempts=10, delay=1.0) +def try_setup_logs(diag, logs): + return setup_logs(diag, logs) -def try_setup_logs(diag, log_types): - for _ in range(10): - try: - setup_logs(diag, log_types) - break - except Exception: - cloudlog.exception("setup logs failed, trying again") - time.sleep(1.0) - else: - raise Exception(f"setup logs failed, {log_types=}") - +@retry(attempts=3, delay=1.0) def at_cmd(cmd: str) -> Optional[str]: - for _ in range(3): - try: - return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8') - except subprocess.CalledProcessError: - cloudlog.exception("qcomgps.mmcli_command_failed") - time.sleep(1.0) - raise Exception(f"failed to execute mmcli command {cmd=}") - + return subprocess.check_output(f"mmcli -m any --timeout 30 --command='{cmd}'", shell=True, encoding='utf8') def gps_enabled() -> bool: try: @@ -154,23 +141,11 @@ def downloader_loop(event): except KeyboardInterrupt: pass +@retry(attempts=5, delay=0.2, ignore_failure=True) def inject_assistance(): - for _ in range(5): - try: - cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}" - subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True) - cloudlog.info("successfully loaded assistance data") - return - except subprocess.CalledProcessError as e: - cloudlog.event( - "qcomgps.assistance_loading_failed", - error=True, - cmd=e.cmd, - output=e.output, - returncode=e.returncode - ) - time.sleep(0.2) - cloudlog.error("failed to load assistance after retry") + cmd = f"mmcli -m any --timeout 30 --location-inject-assistance-data={ASSIST_DATA_FILE}" + subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True) + cloudlog.info("successfully loaded assistance data") def setup_quectel(diag: ModemDiag) -> bool: ret = False From 2f2b9c782e2cef0cc972b39bcdc484fe0d8c9dfb Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Wed, 6 Dec 2023 20:56:20 -0800 Subject: [PATCH 59/87] qcomgpsd: retry quectel setup (#30632) --- system/qcomgpsd/qcomgpsd.py | 1 + 1 file changed, 1 insertion(+) diff --git a/system/qcomgpsd/qcomgpsd.py b/system/qcomgpsd/qcomgpsd.py index 007c61552e..0105d4074f 100755 --- a/system/qcomgpsd/qcomgpsd.py +++ b/system/qcomgpsd/qcomgpsd.py @@ -147,6 +147,7 @@ def inject_assistance(): subprocess.check_output(cmd, stderr=subprocess.PIPE, shell=True) cloudlog.info("successfully loaded assistance data") +@retry(attempts=5, delay=1.0) def setup_quectel(diag: ModemDiag) -> bool: ret = False From 8bf78399e220a55855db7bd17872edb29cae7569 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 02:21:14 +0800 Subject: [PATCH 60/87] sensor2_i2c.h: add `#pragma once` (#30636) --- system/camerad/cameras/sensor2_i2c.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/cameras/sensor2_i2c.h index 9170c5183a..6ea10a939b 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/cameras/sensor2_i2c.h @@ -1,3 +1,5 @@ +#pragma once + struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; From 7f07f47302229bcc2694f7c53d1e5ed1236494c8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Thu, 7 Dec 2023 10:53:33 -0800 Subject: [PATCH 61/87] locationd: disable locationd temporary error alert on the body (#30608) * Disable locationd temporary error alert on the body * Disable locationd/paramsd/planner alerts on body with joystick mode * joystick_enabled param not joystick_mode --- selfdrive/controls/controlsd.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a180b4c410..41866efc3d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -93,7 +93,8 @@ class Controls: else: self.CI, self.CP = CI, CI.CP - self.joystick_mode = self.params.get_bool("JoystickDebugMode") or self.CP.notCar + self.joystick_enabled = self.params.get_bool("JoystickDebugMode") + self.joystick_mode = self.joystick_enabled or self.CP.notCar # set alternative experiences from parameters self.disengage_on_accelerator = self.params.get_bool("DisengageOnAccelerator") @@ -370,16 +371,17 @@ class Controls: else: self.logged_comm_issue = None - if not self.sm['lateralPlan'].mpcSolutionValid: - self.events.add(EventName.plannerError) - if not self.sm['liveLocationKalman'].posenetOK: - self.events.add(EventName.posenetInvalid) - if not self.sm['liveLocationKalman'].deviceStable: - self.events.add(EventName.deviceFalling) - if not self.sm['liveLocationKalman'].inputsOK: - self.events.add(EventName.locationdTemporaryError) - if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): - self.events.add(EventName.paramsdTemporaryError) + if not (self.CP.notCar and self.joystick_enabled): + if not self.sm['lateralPlan'].mpcSolutionValid: + self.events.add(EventName.plannerError) + if not self.sm['liveLocationKalman'].posenetOK: + self.events.add(EventName.posenetInvalid) + if not self.sm['liveLocationKalman'].deviceStable: + self.events.add(EventName.deviceFalling) + if not self.sm['liveLocationKalman'].inputsOK: + self.events.add(EventName.locationdTemporaryError) + if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): + self.events.add(EventName.paramsdTemporaryError) # conservative HW alert. if the data or frequency are off, locationd will throw an error if any((self.sm.frame - self.sm.rcv_frame[s])*DT_CTRL > 10. for s in self.sensor_packets): From 1cd7b04e6ffb97018159c1fef930622bb0b77054 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 7 Dec 2023 13:37:18 -0600 Subject: [PATCH 62/87] Ford: add Explorer 2023 FW (#30637) * 49cead668ca2ac46 * docs --- docs/CARS.md | 2 +- selfdrive/car/ford/values.py | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index f0004c5aa0..5113eef50e 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -34,7 +34,7 @@ A supported vehicle is one that just works when you install a comma device. All |comma|body|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|None|| |Ford|Bronco Sport 2021-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|

Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Ford|Escape 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| -|Ford|Explorer 2020-22|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Ford|Explorer 2020-23|Co-Pilot360 Assist+|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Ford|Focus 2018[3](#footnotes)|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Ford|Maverick 2022|LARIAT Luxury|openpilot available[1](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 Ford Q3 connector
- 1 RJ45 cable (7 ft)
- 1 angled mount (8 degrees)
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index ef8708f01d..3f4540da21 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -88,7 +88,7 @@ CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { FordCarInfo("Ford Kuga 2020-22", "Adaptive Cruise Control with Lane Centering"), ], CAR.EXPLORER_MK6: [ - FordCarInfo("Ford Explorer 2020-22"), + FordCarInfo("Ford Explorer 2020-23"), FordCarInfo("Lincoln Aviator 2020-21", "Co-Pilot360 Plus"), ], CAR.F_150_MK14: FordCarInfo("Ford F-150 2023", "Co-Pilot360 Active 2.0"), @@ -185,6 +185,7 @@ FW_VERSIONS = { b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.abs, 0x760, None): [ b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -212,6 +213,7 @@ FW_VERSIONS = { b'MB5A-14C204-RC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB5A-14C204-AZD\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PB5A-14C204-DA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.F_150_MK14: { From ecfe201a57431e5dc1e4def58519f6493d489bab Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 03:48:17 +0800 Subject: [PATCH 63/87] camerad: move AR0231 functions to camera_ar0231.cc (#30635) * move AR0231 functions to camera_ar0231.cc * move to sensors/aro231.cc --- release/files_common | 1 + system/camerad/SConscript | 3 +- system/camerad/cameras/camera_common.h | 3 + system/camerad/cameras/camera_qcom2.cc | 90 ----------------------- system/camerad/cameras/camera_qcom2.h | 3 +- system/camerad/sensors/ar0231.cc | 98 ++++++++++++++++++++++++++ 6 files changed, 105 insertions(+), 93 deletions(-) create mode 100644 system/camerad/sensors/ar0231.cc diff --git a/release/files_common b/release/files_common index 7257fb820d..f8b424683a 100644 --- a/release/files_common +++ b/release/files_common @@ -319,6 +319,7 @@ system/camerad/snapshot/* system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc system/camerad/cameras/sensor2_i2c.h +system/camerad/sensors/ar0231.cc selfdrive/manager/__init__.py selfdrive/manager/build.py diff --git a/system/camerad/SConscript b/system/camerad/SConscript index 83f93344d8..d814ebf1b0 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -2,7 +2,8 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] -camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) +camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', + 'sensors/ar0231.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 23bfc8b7d5..9d9e42a5ed 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -103,3 +103,6 @@ void cameras_close(MultiCameraState *s); void camerad_thread(); int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); + +void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); + diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 8d20ed27c8..13fd60f4d2 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -907,70 +907,6 @@ void cameras_close(MultiCameraState *s) { delete s->pm; } -std::map> CameraState::ar0231_build_register_lut(uint8_t *data) { - // This function builds a lookup table from register address, to a pair of indices in the - // buffer where to read this address. The buffer contains padding bytes, - // as well as markers to indicate the type of the next byte. - // - // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. - // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional - // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. - - int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; - auto get_next_idx = [](int cur_idx) { - return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding - }; - - std::map> registers; - for (int register_row = 0; register_row < 2; register_row++) { - uint8_t *registers_raw = data + ci.frame_stride * register_row; - assert(registers_raw[0] == 0x0a); // Start of line - - int value_tag_count = 0; - int first_val_idx = 0; - uint16_t cur_addr = 0; - - for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { - int val_idx = get_next_idx(i); - - uint8_t tag = registers_raw[i]; - uint16_t val = registers_raw[val_idx]; - - if (tag == 0xAA) { // Register MSB tag - cur_addr = val << 8; - } else if (tag == 0xA5) { // Register LSB tag - cur_addr |= val; - cur_addr -= 2; // Next value tag will increment address again - } else if (tag == 0x5A) { // Value tag - - // First tag - if (value_tag_count % 2 == 0) { - cur_addr += 2; - first_val_idx = val_idx; - } else { - registers[cur_addr] = std::make_pair(first_val_idx + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row); - } - - value_tag_count++; - } - } - } - return registers; -} - -std::map CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list addrs) { - if (ar0231_register_lut.empty()) { - ar0231_register_lut = ar0231_build_register_lut(data); - } - - std::map registers; - for (uint16_t addr : addrs) { - auto offset = ar0231_register_lut[addr]; - registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; - } - return registers; -} - void CameraState::handle_camera_event(void *evdat) { if (!enabled) return; struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; @@ -1180,32 +1116,6 @@ void CameraState::set_camera_exposure(float grey_frac) { } } -static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { - // See AR0231 Developer Guide - page 36 - float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); - float t0 = 55.0 - slope * (float)calib2; - return t0 + slope * (float)data_reg; -} - -static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){ - const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; - uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset; - - if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0){ - LOGE("unexpected register data found"); - return; - } - - auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); - - uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; - framed.setFrameIdSensor(frame_id); - - float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); - float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); - framed.setTemperaturesC({temp_0, temp_1}); -} - static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { c->set_camera_exposure(set_exposure_target(&c->buf, 96, 1832, 2, 242, 1148, 4)); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 1e7cbfc104..87cdd091f6 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -94,7 +94,6 @@ public: CameraBuf buf; MemoryManager mm; -private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); void enqueue_buffer(int i, bool dp); @@ -106,8 +105,8 @@ private: // Register parsing std::map> ar0231_register_lut; - std::map> ar0231_build_register_lut(uint8_t *data); +private: // for debugging Params params; }; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc new file mode 100644 index 0000000000..7d7a454f7e --- /dev/null +++ b/system/camerad/sensors/ar0231.cc @@ -0,0 +1,98 @@ +#include + +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_qcom2.h" + +namespace { + +std::map> ar0231_build_register_lut(CameraState *c, uint8_t *data) { + // This function builds a lookup table from register address, to a pair of indices in the + // buffer where to read this address. The buffer contains padding bytes, + // as well as markers to indicate the type of the next byte. + // + // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. + // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional + // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. + + int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; + auto get_next_idx = [](int cur_idx) { + return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding + }; + + std::map> registers; + for (int register_row = 0; register_row < 2; register_row++) { + uint8_t *registers_raw = data + c->ci.frame_stride * register_row; + assert(registers_raw[0] == 0x0a); // Start of line + + int value_tag_count = 0; + int first_val_idx = 0; + uint16_t cur_addr = 0; + + for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { + int val_idx = get_next_idx(i); + + uint8_t tag = registers_raw[i]; + uint16_t val = registers_raw[val_idx]; + + if (tag == 0xAA) { // Register MSB tag + cur_addr = val << 8; + } else if (tag == 0xA5) { // Register LSB tag + cur_addr |= val; + cur_addr -= 2; // Next value tag will increment address again + } else if (tag == 0x5A) { // Value tag + + // First tag + if (value_tag_count % 2 == 0) { + cur_addr += 2; + first_val_idx = val_idx; + } else { + registers[cur_addr] = std::make_pair(first_val_idx + c->ci.frame_stride * register_row, val_idx + c->ci.frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +std::map ar0231_parse_registers(CameraState *c, uint8_t *data, std::initializer_list addrs) { + if (c->ar0231_register_lut.empty()) { + c->ar0231_register_lut = ar0231_build_register_lut(c, data); + } + + std::map registers; + for (uint16_t addr : addrs) { + auto offset = c->ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + return registers; +} + +float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { + // See AR0231 Developer Guide - page 36 + float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); + float t0 = 55.0 - slope * (float)calib2; + return t0 + slope * (float)data_reg; +} + +} // namespace + +void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { + const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + + if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { + LOGE("unexpected register data found"); + return; + } + + auto registers = ar0231_parse_registers(c, data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); + + uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; + framed.setFrameIdSensor(frame_id); + + float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); + float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); + framed.setTemperaturesC({temp_0, temp_1}); +} From bdf868ddc276e2cf8a644376afe1a9634bdf06fe Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 07:45:49 +0800 Subject: [PATCH 64/87] camerad: split sensor_ic2.h (#30641) * split sensor_ic2.h * move start_reg,stop_reg --- release/files_common | 3 +- system/camerad/cameras/camera_qcom2.cc | 3 +- system/camerad/sensors/ar0231_registers.h | 119 ++++++++++++++++++ .../ox03c10_registers.h} | 117 ----------------- 4 files changed, 123 insertions(+), 119 deletions(-) create mode 100644 system/camerad/sensors/ar0231_registers.h rename system/camerad/{cameras/sensor2_i2c.h => sensors/ox03c10_registers.h} (83%) diff --git a/release/files_common b/release/files_common index f8b424683a..d73c71171a 100644 --- a/release/files_common +++ b/release/files_common @@ -318,8 +318,9 @@ system/camerad/main.cc system/camerad/snapshot/* system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc -system/camerad/cameras/sensor2_i2c.h system/camerad/sensors/ar0231.cc +system/camerad/sensors/ar0231_registers.h +system/camerad/sensors/ox03c10_registers.h selfdrive/manager/__init__.py selfdrive/manager/build.py diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 13fd60f4d2..e6ca0fff94 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -23,7 +23,8 @@ #include "media/cam_sensor_cmn_header.h" #include "media/cam_sync.h" #include "common/swaglog.h" -#include "system/camerad/cameras/sensor2_i2c.h" +#include "system/camerad/sensors/ar0231_registers.h" +#include "system/camerad/sensors/ox03c10_registers.h" // For debugging: // echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h new file mode 100644 index 0000000000..611d08ec8e --- /dev/null +++ b/system/camerad/sensors/ar0231_registers.h @@ -0,0 +1,119 @@ +#pragma once + +struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; + +struct i2c_random_wr_payload init_array_ar0231[] = { + {0x301A, 0x0018}, // RESET_REGISTER + + // CLOCK Settings + // input clock is 19.2 / 2 * 0x37 = 528 MHz + // pixclk is 528 / 6 = 88 MHz + // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms + // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms + {0x302A, 0x0006}, // VT_PIX_CLK_DIV + {0x302C, 0x0001}, // VT_SYS_CLK_DIV + {0x302E, 0x0002}, // PRE_PLL_CLK_DIV + {0x3030, 0x0037}, // PLL_MULTIPLIER + {0x3036, 0x000C}, // OP_PIX_CLK_DIV + {0x3038, 0x0001}, // OP_SYS_CLK_DIV + + // FORMAT + {0x3040, 0xC000}, // READ_MODE + {0x3004, 0x0000}, // X_ADDR_START_ + {0x3008, 0x0787}, // X_ADDR_END_ + {0x3002, 0x0000}, // Y_ADDR_START_ + {0x3006, 0x04B7}, // Y_ADDR_END_ + {0x3032, 0x0000}, // SCALING_MODE + {0x30A2, 0x0001}, // X_ODD_INC_ + {0x30A6, 0x0001}, // Y_ODD_INC_ + {0x3402, 0x0788}, // X_OUTPUT_CONTROL + {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL + {0x3064, 0x1982}, // SMIA_TEST + {0x30BA, 0x11F2}, // DIGITAL_CTRL + + // Enable external trigger and disable GPIO outputs + {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE + {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE + {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2 + + // Readout timing + {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR) + {0x300A, 0x0855}, // FRAME_LENGTH_LINES + {0x3042, 0x0000}, // EXTRA_DELAY + + // Readout Settings + {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI + {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 + {0x3342, 0x1212}, // MIPI_F1_PDT_EDT + {0x3346, 0x1212}, // MIPI_F2_PDT_EDT + {0x334A, 0x1212}, // MIPI_F3_PDT_EDT + {0x334E, 0x1212}, // MIPI_F4_PDT_EDT + {0x3344, 0x0011}, // MIPI_F1_VDT_VC + {0x3348, 0x0111}, // MIPI_F2_VDT_VC + {0x334C, 0x0211}, // MIPI_F3_VDT_VC + {0x3350, 0x0311}, // MIPI_F4_VDT_VC + {0x31B0, 0x0053}, // FRAME_PREAMBLE + {0x31B2, 0x003B}, // LINE_PREAMBLE + {0x301A, 0x001C}, // RESET_REGISTER + + // Noise Corrections + {0x3092, 0x0C24}, // ROW_NOISE_CONTROL + {0x337A, 0x0C80}, // DBLC_SCALE0 + {0x3370, 0x03B1}, // DBLC + {0x3044, 0x0400}, // DARK_CONTROL + + // Enable temperature sensor + {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG + {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG + + // Enable dead pixel correction using + // the 1D line correction scheme + {0x31E0, 0x0003}, + + // HDR Settings + {0x3082, 0x0004}, // OPERATION_MODE_CTRL + {0x3238, 0x0444}, // EXPOSURE_RATIO + + {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN + {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN + {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN + {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN + + // TODO: do these have to be lower than LINE_LENGTH_PCK? + {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ + {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 + + {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? + {0x33DA, 0x0000}, // COMPANDING + {0x318E, 0x0200}, // PRE_HDR_GAIN_EN + + // DLO Settings + {0x3100, 0x4000}, // DLO_CONTROL0 + {0x3280, 0x0CCC}, // T1 G1 + {0x3282, 0x0CCC}, // T1 R + {0x3284, 0x0CCC}, // T1 B + {0x3286, 0x0CCC}, // T1 G2 + {0x3288, 0x0FA0}, // T2 G1 + {0x328A, 0x0FA0}, // T2 R + {0x328C, 0x0FA0}, // T2 B + {0x328E, 0x0FA0}, // T2 G2 + + // Initial Gains + {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ + {0x3366, 0xFF77}, // ANALOG_GAIN (1x) + + {0x3060, 0x3333}, // ANALOG_COLOR_GAIN + + {0x3362, 0x0000}, // DC GAIN + + {0x305A, 0x00F8}, // red gain + {0x3058, 0x0122}, // blue gain + {0x3056, 0x009A}, // g1 gain + {0x305C, 0x009A}, // g2 gain + + {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ + + // Initial Integration Time + {0x3012, 0x0005}, +}; diff --git a/system/camerad/cameras/sensor2_i2c.h b/system/camerad/sensors/ox03c10_registers.h similarity index 83% rename from system/camerad/cameras/sensor2_i2c.h rename to system/camerad/sensors/ox03c10_registers.h index 6ea10a939b..7aed2e8ac0 100644 --- a/system/camerad/cameras/sensor2_i2c.h +++ b/system/camerad/sensors/ox03c10_registers.h @@ -1,7 +1,5 @@ #pragma once -struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; -struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; @@ -761,118 +759,3 @@ struct i2c_random_wr_payload init_array_ox03c10[] = { {0x3548, 0x0F}, {0x3549, 0x00}, {0x35c1, 0x00}, }; - -struct i2c_random_wr_payload init_array_ar0231[] = { - {0x301A, 0x0018}, // RESET_REGISTER - - // CLOCK Settings - // input clock is 19.2 / 2 * 0x37 = 528 MHz - // pixclk is 528 / 6 = 88 MHz - // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms - // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms - {0x302A, 0x0006}, // VT_PIX_CLK_DIV - {0x302C, 0x0001}, // VT_SYS_CLK_DIV - {0x302E, 0x0002}, // PRE_PLL_CLK_DIV - {0x3030, 0x0037}, // PLL_MULTIPLIER - {0x3036, 0x000C}, // OP_PIX_CLK_DIV - {0x3038, 0x0001}, // OP_SYS_CLK_DIV - - // FORMAT - {0x3040, 0xC000}, // READ_MODE - {0x3004, 0x0000}, // X_ADDR_START_ - {0x3008, 0x0787}, // X_ADDR_END_ - {0x3002, 0x0000}, // Y_ADDR_START_ - {0x3006, 0x04B7}, // Y_ADDR_END_ - {0x3032, 0x0000}, // SCALING_MODE - {0x30A2, 0x0001}, // X_ODD_INC_ - {0x30A6, 0x0001}, // Y_ODD_INC_ - {0x3402, 0x0788}, // X_OUTPUT_CONTROL - {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL - {0x3064, 0x1982}, // SMIA_TEST - {0x30BA, 0x11F2}, // DIGITAL_CTRL - - // Enable external trigger and disable GPIO outputs - {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE - {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE - {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2 - - // Readout timing - {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR) - {0x300A, 0x0855}, // FRAME_LENGTH_LINES - {0x3042, 0x0000}, // EXTRA_DELAY - - // Readout Settings - {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI - {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 - {0x3342, 0x1212}, // MIPI_F1_PDT_EDT - {0x3346, 0x1212}, // MIPI_F2_PDT_EDT - {0x334A, 0x1212}, // MIPI_F3_PDT_EDT - {0x334E, 0x1212}, // MIPI_F4_PDT_EDT - {0x3344, 0x0011}, // MIPI_F1_VDT_VC - {0x3348, 0x0111}, // MIPI_F2_VDT_VC - {0x334C, 0x0211}, // MIPI_F3_VDT_VC - {0x3350, 0x0311}, // MIPI_F4_VDT_VC - {0x31B0, 0x0053}, // FRAME_PREAMBLE - {0x31B2, 0x003B}, // LINE_PREAMBLE - {0x301A, 0x001C}, // RESET_REGISTER - - // Noise Corrections - {0x3092, 0x0C24}, // ROW_NOISE_CONTROL - {0x337A, 0x0C80}, // DBLC_SCALE0 - {0x3370, 0x03B1}, // DBLC - {0x3044, 0x0400}, // DARK_CONTROL - - // Enable temperature sensor - {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG - {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG - - // Enable dead pixel correction using - // the 1D line correction scheme - {0x31E0, 0x0003}, - - // HDR Settings - {0x3082, 0x0004}, // OPERATION_MODE_CTRL - {0x3238, 0x0444}, // EXPOSURE_RATIO - - {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN - {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN - {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN - {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN - - // TODO: do these have to be lower than LINE_LENGTH_PCK? - {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ - {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 - - {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? - {0x33DA, 0x0000}, // COMPANDING - {0x318E, 0x0200}, // PRE_HDR_GAIN_EN - - // DLO Settings - {0x3100, 0x4000}, // DLO_CONTROL0 - {0x3280, 0x0CCC}, // T1 G1 - {0x3282, 0x0CCC}, // T1 R - {0x3284, 0x0CCC}, // T1 B - {0x3286, 0x0CCC}, // T1 G2 - {0x3288, 0x0FA0}, // T2 G1 - {0x328A, 0x0FA0}, // T2 R - {0x328C, 0x0FA0}, // T2 B - {0x328E, 0x0FA0}, // T2 G2 - - // Initial Gains - {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ - {0x3366, 0xFF77}, // ANALOG_GAIN (1x) - - {0x3060, 0x3333}, // ANALOG_COLOR_GAIN - - {0x3362, 0x0000}, // DC GAIN - - {0x305A, 0x00F8}, // red gain - {0x3058, 0x0122}, // blue gain - {0x3056, 0x009A}, // g1 gain - {0x305C, 0x009A}, // g2 gain - - {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ - - // Initial Integration Time - {0x3012, 0x0005}, -}; From a70911d6396a41fb006043b41e89281fa7a5d8b1 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 08:06:55 +0800 Subject: [PATCH 65/87] camerad: refactor sensor parameters to struct (#30639) * refactor camerainfo * include --- system/camerad/cameras/camera_common.cc | 6 +- system/camerad/cameras/camera_common.h | 22 +++- system/camerad/cameras/camera_qcom2.cc | 135 ++++++++++++------------ system/camerad/cameras/camera_qcom2.h | 23 +--- system/camerad/sensors/ar0231.cc | 6 +- 5 files changed, 98 insertions(+), 94 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 987ccf23da..703378b104 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -28,7 +28,7 @@ class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; - const CameraInfo *ci = &s->ci; + const CameraInfo *ci = s->ci.get(); snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " @@ -66,7 +66,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, this->yuv_type = init_yuv_type; frame_buf_count = frame_cnt; - const CameraInfo *ci = &s->ci; + const CameraInfo *ci = s->ci.get(); // RAW frame const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); @@ -152,7 +152,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setProcessingTime(frame_data.processing_time); const float ev = c->cur_ev[frame_data.frame_id % 3]; - const float perc = util::map_val(ev, c->min_ev, c->max_ev, 0.0f, 100.0f); + const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); framed.setExposureValPercent(perc); if (c->camera_id == CAMERA_ID_AR0231) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 9d9e42a5ed..8022fa844e 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -16,8 +16,8 @@ #define CAMERA_ID_AR0231 0 #define CAMERA_ID_OX03C10 1 -#define CAMERA_ID_MAX 2 +#define ANALOG_GAIN_MAX_CNT 55 const int YUV_BUFFER_COUNT = 20; enum CameraType { @@ -41,6 +41,26 @@ typedef struct CameraInfo { uint32_t extra_height = 0; int registers_offset = -1; int stats_offset = -1; + + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; + float target_grey_factor; + float min_ev; + float max_ev; } CameraInfo; typedef struct FrameMetadata { diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index e6ca0fff94..5aa95bd63d 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -40,27 +40,6 @@ const size_t AR0231_REGISTERS_HEIGHT = 2; const size_t AR0231_STATS_HEIGHT = 2+8; const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py - -CameraInfo cameras_supported[CAMERA_ID_MAX] = { - [CAMERA_ID_AR0231] = { - .frame_width = FRAME_WIDTH, - .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, - .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, - - .registers_offset = 0, - .frame_offset = AR0231_REGISTERS_HEIGHT, - .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, - }, - [CAMERA_ID_OX03C10] = { - .frame_width = FRAME_WIDTH, - .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, // (0xa80*12//8) - .extra_height = 16, // top 2 + bot 14 - .frame_offset = 2, - }, -}; - const float DC_GAIN_AR0231 = 2.5; const float DC_GAIN_OX03C10 = 7.32; @@ -424,10 +403,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = ci.frame_width, - .height = ci.frame_height + ci.extra_height, - .plane_stride = ci.frame_stride, - .slice_height = ci.frame_height + ci.extra_height, + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, + .plane_stride = ci->frame_stride, + .slice_height = ci->frame_height + ci->extra_height, .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, @@ -517,8 +496,17 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_set_parameters() { - if (camera_id == CAMERA_ID_AR0231) { +struct CameraAR0231 : public CameraInfo { + CameraAR0231() { + frame_width = FRAME_WIDTH, + frame_height = FRAME_HEIGHT, + frame_stride = FRAME_STRIDE, + extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + registers_offset = 0, + frame_offset = AR0231_REGISTERS_HEIGHT, + stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + dc_gain_factor = DC_GAIN_AR0231; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; @@ -532,12 +520,23 @@ void CameraState::camera_set_parameters() { analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; - for (int i=0; i<=analog_gain_max_idx; i++) { + for (int i = 0; i <= analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; } min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_AR0231; - } else if (camera_id == CAMERA_ID_OX03C10) { + } +}; + +struct CameraOx0310 : public CameraInfo { + CameraOx0310() { + frame_width = FRAME_WIDTH, + frame_height = FRAME_HEIGHT, + frame_stride = FRAME_STRIDE, // (0xa80*12//8) + extra_height = 16, // top 2 + bot 14 + frame_offset = 2, + dc_gain_factor = DC_GAIN_OX03C10; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; @@ -555,19 +554,27 @@ void CameraState::camera_set_parameters() { sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; } min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_OX03C10; + } +}; + +void CameraState::camera_set_parameters() { + if (camera_id == CAMERA_ID_AR0231) { + ci = std::make_unique(); + } else if (camera_id == CAMERA_ID_OX03C10) { + ci = std::make_unique(); } else { assert(false); } - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_fraction = 0.3; dc_gain_enabled = false; - dc_gain_weight = dc_gain_min_weight; - gain_idx = analog_gain_rec_idx; + dc_gain_weight = ci->dc_gain_min_weight; + gain_idx = ci->analog_gain_rec_idx; exposure_time = 5; - cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight) * ci->sensor_analog_gains[gain_idx] * exposure_time; } void CameraState::camera_map_bufs(MultiCameraState *s) { @@ -590,10 +597,6 @@ void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int came camera_id = camera_id_; LOGD("camera init %d", camera_num); - assert(camera_id < std::size(cameras_supported)); - ci = cameras_supported[camera_id]; - assert(ci.frame_width != 0); - request_id_last = 0; skipped = true; @@ -680,16 +683,16 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .usage_type = 0x0, .left_start = 0, - .left_stop = ci.frame_width - 1, - .left_width = ci.frame_width, + .left_stop = ci->frame_width - 1, + .left_width = ci->frame_width, .right_start = 0, - .right_stop = ci.frame_width - 1, - .right_width = ci.frame_width, + .right_stop = ci->frame_width - 1, + .right_width = ci->frame_width, .line_start = 0, - .line_stop = ci.frame_height + ci.extra_height - 1, - .height = ci.frame_height + ci.extra_height, + .line_stop = ci->frame_height + ci->extra_height - 1, + .height = ci->frame_height + ci->extra_height, .pixel_clk = 0x0, .batch_size = 0x0, @@ -701,8 +704,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, .format = CAM_FORMAT_MIPI_RAW_12, - .width = ci.frame_width, - .height = ci.frame_height + ci.extra_height, + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -946,7 +949,7 @@ void CameraState::handle_camera_event(void *evdat) { meta_data.frame_id = main_id - idx_offset; meta_data.timestamp_sof = timestamp; exp_lock.lock(); - meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); meta_data.high_conversion_gain = dc_gain_enabled; meta_data.integ_lines = exposure_time; meta_data.measured_grey_fraction = measured_grey_fraction; @@ -972,15 +975,15 @@ void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_i // Cost of ev diff score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; // Cost of absolute gain - float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; // Cost of changing gain score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; } else if (camera_id == CAMERA_ID_OX03C10) { score = std::abs(desired_ev - (exp_t * exp_gain)); - float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; - score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; + float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; + score += ((1 - ci->analog_gain_cost_delta) + ci->analog_gain_cost_delta * (exp_g_idx - ci->analog_gain_min_idx) / (ci->analog_gain_max_idx - ci->analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; } if (score < best_ev_score) { @@ -1008,10 +1011,10 @@ void CameraState::set_camera_exposure(float grey_frac) { const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; // Scale target grey between 0.1 and 0.4 depending on lighting conditions - float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + ci->target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; - float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev); + float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, ci->min_ev, ci->max_ev); float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); @@ -1022,16 +1025,16 @@ void CameraState::set_camera_exposure(float grey_frac) { // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < dc_gain_on_grey) { + if (!enable_dc_gain && target_grey < ci->dc_gain_on_grey) { enable_dc_gain = true; - dc_gain_weight = dc_gain_min_weight; - } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { + dc_gain_weight = ci->dc_gain_min_weight; + } else if (enable_dc_gain && target_grey > ci->dc_gain_off_grey) { enable_dc_gain = false; - dc_gain_weight = dc_gain_max_weight; + dc_gain_weight = ci->dc_gain_max_weight; } - if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} - if (!enable_dc_gain && dc_gain_weight > dc_gain_min_weight) {dc_gain_weight -= 1;} + if (enable_dc_gain && dc_gain_weight < ci->dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > ci->dc_gain_min_weight) {dc_gain_weight -= 1;} std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { @@ -1050,14 +1053,14 @@ void CameraState::set_camera_exposure(float grey_frac) { } else { // Simple brute force optimizer to choose sensor parameters // to reach desired EV - for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { - float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + for (int g = std::max((int)ci->analog_gain_min_idx, gain_idx - 1); g <= std::min((int)ci->analog_gain_max_idx, gain_idx + 1); g++) { + float gain = ci->sensor_analog_gains[g] * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); + int t = std::clamp(int(std::round(desired_ev / gain)), ci->exposure_time_min, ci->exposure_time_max); // Only go below recommended gain when absolutely necessary to not overexpose - if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { + if (g < ci->analog_gain_rec_idx && t > 20 && g < gain_idx) { continue; } @@ -1070,12 +1073,12 @@ void CameraState::set_camera_exposure(float grey_frac) { measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = sensor_analog_gains[new_exp_g]; + analog_gain_frac = ci->sensor_analog_gains[new_exp_g]; gain_idx = new_exp_g; exposure_time = new_exp_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + float gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; exp_lock.unlock(); @@ -1100,7 +1103,7 @@ void CameraState::set_camera_exposure(float grey_frac) { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 87cdd091f6..b1765aa582 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -12,12 +13,11 @@ #include "common/util.h" #define FRAME_BUF_COUNT 4 -#define ANALOG_GAIN_MAX_CNT 55 class CameraState { public: MultiCameraState *multi_cam_state; - CameraInfo ci; + std::unique_ptr ci; bool enabled; std::mutex exp_lock; @@ -28,32 +28,13 @@ public: int gain_idx; float analog_gain_frac; - int exposure_time_min; - int exposure_time_max; - - float dc_gain_factor; - int dc_gain_min_weight; - int dc_gain_max_weight; - float dc_gain_on_grey; - float dc_gain_off_grey; - - float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; - int analog_gain_min_idx; - int analog_gain_max_idx; - int analog_gain_rec_idx; - int analog_gain_cost_delta; - float analog_gain_cost_low; - float analog_gain_cost_high; - float cur_ev[3]; - float min_ev, max_ev; float best_ev_score; int new_exp_g; int new_exp_t; float measured_grey_fraction; float target_grey_fraction; - float target_grey_factor; unique_fd sensor_fd; unique_fd csiphy_fd; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 7d7a454f7e..10034a7c3c 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -21,7 +21,7 @@ std::map> ar0231_build_register_lut(CameraState *c std::map> registers; for (int register_row = 0; register_row < 2; register_row++) { - uint8_t *registers_raw = data + c->ci.frame_stride * register_row; + uint8_t *registers_raw = data + c->ci->frame_stride * register_row; assert(registers_raw[0] == 0x0a); // Start of line int value_tag_count = 0; @@ -46,7 +46,7 @@ std::map> ar0231_build_register_lut(CameraState *c cur_addr += 2; first_val_idx = val_idx; } else { - registers[cur_addr] = std::make_pair(first_val_idx + c->ci.frame_stride * register_row, val_idx + c->ci.frame_stride * register_row); + registers[cur_addr] = std::make_pair(first_val_idx + c->ci->frame_stride * register_row, val_idx + c->ci->frame_stride * register_row); } value_tag_count++; @@ -80,7 +80,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; - uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { LOGE("unexpected register data found"); From 0586f86ad01323bc213c125e8584df277555456d Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Thu, 7 Dec 2023 16:21:16 -0800 Subject: [PATCH 66/87] Revert "camerad: refactor sensor parameters to struct (#30639)" This reverts commit a70911d6396a41fb006043b41e89281fa7a5d8b1. --- system/camerad/cameras/camera_common.cc | 6 +- system/camerad/cameras/camera_common.h | 22 +--- system/camerad/cameras/camera_qcom2.cc | 135 ++++++++++++------------ system/camerad/cameras/camera_qcom2.h | 23 +++- system/camerad/sensors/ar0231.cc | 6 +- 5 files changed, 94 insertions(+), 98 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 703378b104..987ccf23da 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -28,7 +28,7 @@ class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; - const CameraInfo *ci = s->ci.get(); + const CameraInfo *ci = &s->ci; snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " @@ -66,7 +66,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, this->yuv_type = init_yuv_type; frame_buf_count = frame_cnt; - const CameraInfo *ci = s->ci.get(); + const CameraInfo *ci = &s->ci; // RAW frame const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); @@ -152,7 +152,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setProcessingTime(frame_data.processing_time); const float ev = c->cur_ev[frame_data.frame_id % 3]; - const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); + const float perc = util::map_val(ev, c->min_ev, c->max_ev, 0.0f, 100.0f); framed.setExposureValPercent(perc); if (c->camera_id == CAMERA_ID_AR0231) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 8022fa844e..9d9e42a5ed 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -16,8 +16,8 @@ #define CAMERA_ID_AR0231 0 #define CAMERA_ID_OX03C10 1 +#define CAMERA_ID_MAX 2 -#define ANALOG_GAIN_MAX_CNT 55 const int YUV_BUFFER_COUNT = 20; enum CameraType { @@ -41,26 +41,6 @@ typedef struct CameraInfo { uint32_t extra_height = 0; int registers_offset = -1; int stats_offset = -1; - - int exposure_time_min; - int exposure_time_max; - - float dc_gain_factor; - int dc_gain_min_weight; - int dc_gain_max_weight; - float dc_gain_on_grey; - float dc_gain_off_grey; - - float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; - int analog_gain_min_idx; - int analog_gain_max_idx; - int analog_gain_rec_idx; - int analog_gain_cost_delta; - float analog_gain_cost_low; - float analog_gain_cost_high; - float target_grey_factor; - float min_ev; - float max_ev; } CameraInfo; typedef struct FrameMetadata { diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 5aa95bd63d..e6ca0fff94 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -40,6 +40,27 @@ const size_t AR0231_REGISTERS_HEIGHT = 2; const size_t AR0231_STATS_HEIGHT = 2+8; const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py + +CameraInfo cameras_supported[CAMERA_ID_MAX] = { + [CAMERA_ID_AR0231] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, + .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + .registers_offset = 0, + .frame_offset = AR0231_REGISTERS_HEIGHT, + .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + }, + [CAMERA_ID_OX03C10] = { + .frame_width = FRAME_WIDTH, + .frame_height = FRAME_HEIGHT, + .frame_stride = FRAME_STRIDE, // (0xa80*12//8) + .extra_height = 16, // top 2 + bot 14 + .frame_offset = 2, + }, +}; + const float DC_GAIN_AR0231 = 2.5; const float DC_GAIN_OX03C10 = 7.32; @@ -403,10 +424,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = ci->frame_width, - .height = ci->frame_height + ci->extra_height, - .plane_stride = ci->frame_stride, - .slice_height = ci->frame_height + ci->extra_height, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, + .plane_stride = ci.frame_stride, + .slice_height = ci.frame_height + ci.extra_height, .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, @@ -496,17 +517,8 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -struct CameraAR0231 : public CameraInfo { - CameraAR0231() { - frame_width = FRAME_WIDTH, - frame_height = FRAME_HEIGHT, - frame_stride = FRAME_STRIDE, - extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, - - registers_offset = 0, - frame_offset = AR0231_REGISTERS_HEIGHT, - stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, - +void CameraState::camera_set_parameters() { + if (camera_id == CAMERA_ID_AR0231) { dc_gain_factor = DC_GAIN_AR0231; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; @@ -520,23 +532,12 @@ struct CameraAR0231 : public CameraInfo { analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; - for (int i = 0; i <= analog_gain_max_idx; i++) { + for (int i=0; i<=analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; } min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_AR0231; - } -}; - -struct CameraOx0310 : public CameraInfo { - CameraOx0310() { - frame_width = FRAME_WIDTH, - frame_height = FRAME_HEIGHT, - frame_stride = FRAME_STRIDE, // (0xa80*12//8) - extra_height = 16, // top 2 + bot 14 - frame_offset = 2, - + } else if (camera_id == CAMERA_ID_OX03C10) { dc_gain_factor = DC_GAIN_OX03C10; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; @@ -554,27 +555,19 @@ struct CameraOx0310 : public CameraInfo { sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; } min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_OX03C10; - } -}; - -void CameraState::camera_set_parameters() { - if (camera_id == CAMERA_ID_AR0231) { - ci = std::make_unique(); - } else if (camera_id == CAMERA_ID_OX03C10) { - ci = std::make_unique(); } else { assert(false); } + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_fraction = 0.3; dc_gain_enabled = false; - dc_gain_weight = ci->dc_gain_min_weight; - gain_idx = ci->analog_gain_rec_idx; + dc_gain_weight = dc_gain_min_weight; + gain_idx = analog_gain_rec_idx; exposure_time = 5; - cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight) * ci->sensor_analog_gains[gain_idx] * exposure_time; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time; } void CameraState::camera_map_bufs(MultiCameraState *s) { @@ -597,6 +590,10 @@ void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int came camera_id = camera_id_; LOGD("camera init %d", camera_num); + assert(camera_id < std::size(cameras_supported)); + ci = cameras_supported[camera_id]; + assert(ci.frame_width != 0); + request_id_last = 0; skipped = true; @@ -683,16 +680,16 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .usage_type = 0x0, .left_start = 0, - .left_stop = ci->frame_width - 1, - .left_width = ci->frame_width, + .left_stop = ci.frame_width - 1, + .left_width = ci.frame_width, .right_start = 0, - .right_stop = ci->frame_width - 1, - .right_width = ci->frame_width, + .right_stop = ci.frame_width - 1, + .right_width = ci.frame_width, .line_start = 0, - .line_stop = ci->frame_height + ci->extra_height - 1, - .height = ci->frame_height + ci->extra_height, + .line_stop = ci.frame_height + ci.extra_height - 1, + .height = ci.frame_height + ci.extra_height, .pixel_clk = 0x0, .batch_size = 0x0, @@ -704,8 +701,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, .format = CAM_FORMAT_MIPI_RAW_12, - .width = ci->frame_width, - .height = ci->frame_height + ci->extra_height, + .width = ci.frame_width, + .height = ci.frame_height + ci.extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -949,7 +946,7 @@ void CameraState::handle_camera_event(void *evdat) { meta_data.frame_id = main_id - idx_offset; meta_data.timestamp_sof = timestamp; exp_lock.lock(); - meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); meta_data.high_conversion_gain = dc_gain_enabled; meta_data.integ_lines = exposure_time; meta_data.measured_grey_fraction = measured_grey_fraction; @@ -975,15 +972,15 @@ void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_i // Cost of ev diff score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; // Cost of absolute gain - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; // Cost of changing gain score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; } else if (camera_id == CAMERA_ID_OX03C10) { score = std::abs(desired_ev - (exp_t * exp_gain)); - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; - score += ((1 - ci->analog_gain_cost_delta) + ci->analog_gain_cost_delta * (exp_g_idx - ci->analog_gain_min_idx) / (ci->analog_gain_max_idx - ci->analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; } if (score < best_ev_score) { @@ -1011,10 +1008,10 @@ void CameraState::set_camera_exposure(float grey_frac) { const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; // Scale target grey between 0.1 and 0.4 depending on lighting conditions - float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + ci->target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; - float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, ci->min_ev, ci->max_ev); + float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev); float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); @@ -1025,16 +1022,16 @@ void CameraState::set_camera_exposure(float grey_frac) { // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < ci->dc_gain_on_grey) { + if (!enable_dc_gain && target_grey < dc_gain_on_grey) { enable_dc_gain = true; - dc_gain_weight = ci->dc_gain_min_weight; - } else if (enable_dc_gain && target_grey > ci->dc_gain_off_grey) { + dc_gain_weight = dc_gain_min_weight; + } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { enable_dc_gain = false; - dc_gain_weight = ci->dc_gain_max_weight; + dc_gain_weight = dc_gain_max_weight; } - if (enable_dc_gain && dc_gain_weight < ci->dc_gain_max_weight) {dc_gain_weight += 1;} - if (!enable_dc_gain && dc_gain_weight > ci->dc_gain_min_weight) {dc_gain_weight -= 1;} + if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > dc_gain_min_weight) {dc_gain_weight -= 1;} std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { @@ -1053,14 +1050,14 @@ void CameraState::set_camera_exposure(float grey_frac) { } else { // Simple brute force optimizer to choose sensor parameters // to reach desired EV - for (int g = std::max((int)ci->analog_gain_min_idx, gain_idx - 1); g <= std::min((int)ci->analog_gain_max_idx, gain_idx + 1); g++) { - float gain = ci->sensor_analog_gains[g] * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { + float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), ci->exposure_time_min, ci->exposure_time_max); + int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); // Only go below recommended gain when absolutely necessary to not overexpose - if (g < ci->analog_gain_rec_idx && t > 20 && g < gain_idx) { + if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { continue; } @@ -1073,12 +1070,12 @@ void CameraState::set_camera_exposure(float grey_frac) { measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = ci->sensor_analog_gains[new_exp_g]; + analog_gain_frac = sensor_analog_gains[new_exp_g]; gain_idx = new_exp_g; exposure_time = new_exp_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); + float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; exp_lock.unlock(); @@ -1103,7 +1100,7 @@ void CameraState::set_camera_exposure(float grey_frac) { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index b1765aa582..87cdd091f6 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -2,7 +2,6 @@ #include #include -#include #include #include @@ -13,11 +12,12 @@ #include "common/util.h" #define FRAME_BUF_COUNT 4 +#define ANALOG_GAIN_MAX_CNT 55 class CameraState { public: MultiCameraState *multi_cam_state; - std::unique_ptr ci; + CameraInfo ci; bool enabled; std::mutex exp_lock; @@ -28,13 +28,32 @@ public: int gain_idx; float analog_gain_frac; + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; + float cur_ev[3]; + float min_ev, max_ev; float best_ev_score; int new_exp_g; int new_exp_t; float measured_grey_fraction; float target_grey_fraction; + float target_grey_factor; unique_fd sensor_fd; unique_fd csiphy_fd; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 10034a7c3c..7d7a454f7e 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -21,7 +21,7 @@ std::map> ar0231_build_register_lut(CameraState *c std::map> registers; for (int register_row = 0; register_row < 2; register_row++) { - uint8_t *registers_raw = data + c->ci->frame_stride * register_row; + uint8_t *registers_raw = data + c->ci.frame_stride * register_row; assert(registers_raw[0] == 0x0a); // Start of line int value_tag_count = 0; @@ -46,7 +46,7 @@ std::map> ar0231_build_register_lut(CameraState *c cur_addr += 2; first_val_idx = val_idx; } else { - registers[cur_addr] = std::make_pair(first_val_idx + c->ci->frame_stride * register_row, val_idx + c->ci->frame_stride * register_row); + registers[cur_addr] = std::make_pair(first_val_idx + c->ci.frame_stride * register_row, val_idx + c->ci.frame_stride * register_row); } value_tag_count++; @@ -80,7 +80,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; - uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci.registers_offset; if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { LOGE("unexpected register data found"); From 49317e390351f702bfe95a0400fef479d389007f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 7 Dec 2023 18:37:59 -0600 Subject: [PATCH 67/87] Ford: add Explorer 2020 FW (#30638) * 49cead668ca2ac46 * docs * b18d8435f1460528 --- selfdrive/car/ford/values.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index 3f4540da21..301847b7ea 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -193,6 +193,7 @@ FW_VERSIONS = { b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -214,6 +215,7 @@ FW_VERSIONS = { b'NB5A-14C204-AZD\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'NB5A-14C204-HB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PB5A-14C204-DA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'LB5A-14C204-ATS\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], }, CAR.F_150_MK14: { From 9bff8ccd0fa8f29d3fa92e4505d9cdd7a72d0f12 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 12:11:44 +0800 Subject: [PATCH 68/87] camerad: fix use of uninitialized `CameraInfo` in `camera_open()` (#30642) --- system/camerad/cameras/camera_qcom2.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index e6ca0fff94..d5fd643bf1 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -597,8 +597,6 @@ void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int came request_id_last = 0; skipped = true; - camera_set_parameters(); - buf.init(device_id, ctx, this, v, FRAME_BUF_COUNT, yuv_type); camera_map_bufs(s); } @@ -634,6 +632,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num return; } + camera_set_parameters(); + // create session struct cam_req_mgr_session_info session_info = {}; ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); From 51328609d21d3d2c6ca211c2eeafcdc9511feb6f Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Fri, 8 Dec 2023 16:01:16 +0800 Subject: [PATCH 69/87] camerad: refactor sensor parameters to struct (#30644) * refactor camerainfo * include --- system/camerad/cameras/camera_common.cc | 6 +- system/camerad/cameras/camera_common.h | 22 +++- system/camerad/cameras/camera_qcom2.cc | 135 ++++++++++++------------ system/camerad/cameras/camera_qcom2.h | 23 +--- system/camerad/sensors/ar0231.cc | 6 +- 5 files changed, 98 insertions(+), 94 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 987ccf23da..703378b104 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -28,7 +28,7 @@ class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; - const CameraInfo *ci = &s->ci; + const CameraInfo *ci = s->ci.get(); snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " @@ -66,7 +66,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, this->yuv_type = init_yuv_type; frame_buf_count = frame_cnt; - const CameraInfo *ci = &s->ci; + const CameraInfo *ci = s->ci.get(); // RAW frame const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); @@ -152,7 +152,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr framed.setProcessingTime(frame_data.processing_time); const float ev = c->cur_ev[frame_data.frame_id % 3]; - const float perc = util::map_val(ev, c->min_ev, c->max_ev, 0.0f, 100.0f); + const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); framed.setExposureValPercent(perc); if (c->camera_id == CAMERA_ID_AR0231) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 9d9e42a5ed..8022fa844e 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -16,8 +16,8 @@ #define CAMERA_ID_AR0231 0 #define CAMERA_ID_OX03C10 1 -#define CAMERA_ID_MAX 2 +#define ANALOG_GAIN_MAX_CNT 55 const int YUV_BUFFER_COUNT = 20; enum CameraType { @@ -41,6 +41,26 @@ typedef struct CameraInfo { uint32_t extra_height = 0; int registers_offset = -1; int stats_offset = -1; + + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; + float target_grey_factor; + float min_ev; + float max_ev; } CameraInfo; typedef struct FrameMetadata { diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index d5fd643bf1..2e84a8605b 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -40,27 +40,6 @@ const size_t AR0231_REGISTERS_HEIGHT = 2; const size_t AR0231_STATS_HEIGHT = 2+8; const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py - -CameraInfo cameras_supported[CAMERA_ID_MAX] = { - [CAMERA_ID_AR0231] = { - .frame_width = FRAME_WIDTH, - .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, - .extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, - - .registers_offset = 0, - .frame_offset = AR0231_REGISTERS_HEIGHT, - .stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, - }, - [CAMERA_ID_OX03C10] = { - .frame_width = FRAME_WIDTH, - .frame_height = FRAME_HEIGHT, - .frame_stride = FRAME_STRIDE, // (0xa80*12//8) - .extra_height = 16, // top 2 + bot 14 - .frame_offset = 2, - }, -}; - const float DC_GAIN_AR0231 = 2.5; const float DC_GAIN_OX03C10 = 7.32; @@ -424,10 +403,10 @@ void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int b if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; io_cfg[0].planes[0] = (struct cam_plane_cfg){ - .width = ci.frame_width, - .height = ci.frame_height + ci.extra_height, - .plane_stride = ci.frame_stride, - .slice_height = ci.frame_height + ci.extra_height, + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, + .plane_stride = ci->frame_stride, + .slice_height = ci->frame_height + ci->extra_height, .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, @@ -517,8 +496,17 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_set_parameters() { - if (camera_id == CAMERA_ID_AR0231) { +struct CameraAR0231 : public CameraInfo { + CameraAR0231() { + frame_width = FRAME_WIDTH, + frame_height = FRAME_HEIGHT, + frame_stride = FRAME_STRIDE, + extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, + + registers_offset = 0, + frame_offset = AR0231_REGISTERS_HEIGHT, + stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, + dc_gain_factor = DC_GAIN_AR0231; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; @@ -532,12 +520,23 @@ void CameraState::camera_set_parameters() { analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; - for (int i=0; i<=analog_gain_max_idx; i++) { + for (int i = 0; i <= analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; } min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_AR0231; - } else if (camera_id == CAMERA_ID_OX03C10) { + } +}; + +struct CameraOx0310 : public CameraInfo { + CameraOx0310() { + frame_width = FRAME_WIDTH, + frame_height = FRAME_HEIGHT, + frame_stride = FRAME_STRIDE, // (0xa80*12//8) + extra_height = 16, // top 2 + bot 14 + frame_offset = 2, + dc_gain_factor = DC_GAIN_OX03C10; dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; @@ -555,19 +554,27 @@ void CameraState::camera_set_parameters() { sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; } min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_factor = TARGET_GREY_FACTOR_OX03C10; + } +}; + +void CameraState::camera_set_parameters() { + if (camera_id == CAMERA_ID_AR0231) { + ci = std::make_unique(); + } else if (camera_id == CAMERA_ID_OX03C10) { + ci = std::make_unique(); } else { assert(false); } - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; target_grey_fraction = 0.3; dc_gain_enabled = false; - dc_gain_weight = dc_gain_min_weight; - gain_idx = analog_gain_rec_idx; + dc_gain_weight = ci->dc_gain_min_weight; + gain_idx = ci->analog_gain_rec_idx; exposure_time = 5; - cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight) * sensor_analog_gains[gain_idx] * exposure_time; + cur_ev[0] = cur_ev[1] = cur_ev[2] = (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight) * ci->sensor_analog_gains[gain_idx] * exposure_time; } void CameraState::camera_map_bufs(MultiCameraState *s) { @@ -590,10 +597,6 @@ void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int came camera_id = camera_id_; LOGD("camera init %d", camera_num); - assert(camera_id < std::size(cameras_supported)); - ci = cameras_supported[camera_id]; - assert(ci.frame_width != 0); - request_id_last = 0; skipped = true; @@ -680,16 +683,16 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .usage_type = 0x0, .left_start = 0, - .left_stop = ci.frame_width - 1, - .left_width = ci.frame_width, + .left_stop = ci->frame_width - 1, + .left_width = ci->frame_width, .right_start = 0, - .right_stop = ci.frame_width - 1, - .right_width = ci.frame_width, + .right_stop = ci->frame_width - 1, + .right_width = ci->frame_width, .line_start = 0, - .line_stop = ci.frame_height + ci.extra_height - 1, - .height = ci.frame_height + ci.extra_height, + .line_stop = ci->frame_height + ci->extra_height - 1, + .height = ci->frame_height + ci->extra_height, .pixel_clk = 0x0, .batch_size = 0x0, @@ -701,8 +704,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .data[0] = (struct cam_isp_out_port_info){ .res_type = CAM_ISP_IFE_OUT_RES_RDI_0, .format = CAM_FORMAT_MIPI_RAW_12, - .width = ci.frame_width, - .height = ci.frame_height + ci.extra_height, + .width = ci->frame_width, + .height = ci->frame_height + ci->extra_height, .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0, }, }; @@ -946,7 +949,7 @@ void CameraState::handle_camera_event(void *evdat) { meta_data.frame_id = main_id - idx_offset; meta_data.timestamp_sof = timestamp; exp_lock.lock(); - meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + meta_data.gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); meta_data.high_conversion_gain = dc_gain_enabled; meta_data.integ_lines = exposure_time; meta_data.measured_grey_fraction = measured_grey_fraction; @@ -972,15 +975,15 @@ void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_i // Cost of ev diff score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; // Cost of absolute gain - float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; // Cost of changing gain score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; } else if (camera_id == CAMERA_ID_OX03C10) { score = std::abs(desired_ev - (exp_t * exp_gain)); - float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; - score += ((1 - analog_gain_cost_delta) + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; + float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; + score += ((1 - ci->analog_gain_cost_delta) + ci->analog_gain_cost_delta * (exp_g_idx - ci->analog_gain_min_idx) / (ci->analog_gain_max_idx - ci->analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; } if (score < best_ev_score) { @@ -1008,10 +1011,10 @@ void CameraState::set_camera_exposure(float grey_frac) { const float cur_ev_ = cur_ev[buf.cur_frame_data.frame_id % 3]; // Scale target grey between 0.1 and 0.4 depending on lighting conditions - float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); + float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + ci->target_grey_factor*cur_ev_) / log2(6000.0), 0.1, 0.4); float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey; - float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, min_ev, max_ev); + float desired_ev = std::clamp(cur_ev_ * target_grey / grey_frac, ci->min_ev, ci->max_ev); float k = (1.0 - k_ev) / 3.0; desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev); @@ -1022,16 +1025,16 @@ void CameraState::set_camera_exposure(float grey_frac) { // Hysteresis around high conversion gain // We usually want this on since it results in lower noise, but turn off in very bright day scenes bool enable_dc_gain = dc_gain_enabled; - if (!enable_dc_gain && target_grey < dc_gain_on_grey) { + if (!enable_dc_gain && target_grey < ci->dc_gain_on_grey) { enable_dc_gain = true; - dc_gain_weight = dc_gain_min_weight; - } else if (enable_dc_gain && target_grey > dc_gain_off_grey) { + dc_gain_weight = ci->dc_gain_min_weight; + } else if (enable_dc_gain && target_grey > ci->dc_gain_off_grey) { enable_dc_gain = false; - dc_gain_weight = dc_gain_max_weight; + dc_gain_weight = ci->dc_gain_max_weight; } - if (enable_dc_gain && dc_gain_weight < dc_gain_max_weight) {dc_gain_weight += 1;} - if (!enable_dc_gain && dc_gain_weight > dc_gain_min_weight) {dc_gain_weight -= 1;} + if (enable_dc_gain && dc_gain_weight < ci->dc_gain_max_weight) {dc_gain_weight += 1;} + if (!enable_dc_gain && dc_gain_weight > ci->dc_gain_min_weight) {dc_gain_weight -= 1;} std::string gain_bytes, time_bytes; if (env_ctrl_exp_from_params) { @@ -1050,14 +1053,14 @@ void CameraState::set_camera_exposure(float grey_frac) { } else { // Simple brute force optimizer to choose sensor parameters // to reach desired EV - for (int g = std::max((int)analog_gain_min_idx, gain_idx - 1); g <= std::min((int)analog_gain_max_idx, gain_idx + 1); g++) { - float gain = sensor_analog_gains[g] * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + for (int g = std::max((int)ci->analog_gain_min_idx, gain_idx - 1); g <= std::min((int)ci->analog_gain_max_idx, gain_idx + 1); g++) { + float gain = ci->sensor_analog_gains[g] * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); // Compute optimal time for given gain - int t = std::clamp(int(std::round(desired_ev / gain)), exposure_time_min, exposure_time_max); + int t = std::clamp(int(std::round(desired_ev / gain)), ci->exposure_time_min, ci->exposure_time_max); // Only go below recommended gain when absolutely necessary to not overexpose - if (g < analog_gain_rec_idx && t > 20 && g < gain_idx) { + if (g < ci->analog_gain_rec_idx && t > 20 && g < gain_idx) { continue; } @@ -1070,12 +1073,12 @@ void CameraState::set_camera_exposure(float grey_frac) { measured_grey_fraction = grey_frac; target_grey_fraction = target_grey; - analog_gain_frac = sensor_analog_gains[new_exp_g]; + analog_gain_frac = ci->sensor_analog_gains[new_exp_g]; gain_idx = new_exp_g; exposure_time = new_exp_t; dc_gain_enabled = enable_dc_gain; - float gain = analog_gain_frac * (1 + dc_gain_weight * (dc_gain_factor-1) / dc_gain_max_weight); + float gain = analog_gain_frac * (1 + dc_gain_weight * (ci->dc_gain_factor-1) / ci->dc_gain_max_weight); cur_ev[buf.cur_frame_data.frame_id % 3] = exposure_time * gain; exp_lock.unlock(); @@ -1100,7 +1103,7 @@ void CameraState::set_camera_exposure(float grey_frac) { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 87cdd091f6..b1765aa582 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -12,12 +13,11 @@ #include "common/util.h" #define FRAME_BUF_COUNT 4 -#define ANALOG_GAIN_MAX_CNT 55 class CameraState { public: MultiCameraState *multi_cam_state; - CameraInfo ci; + std::unique_ptr ci; bool enabled; std::mutex exp_lock; @@ -28,32 +28,13 @@ public: int gain_idx; float analog_gain_frac; - int exposure_time_min; - int exposure_time_max; - - float dc_gain_factor; - int dc_gain_min_weight; - int dc_gain_max_weight; - float dc_gain_on_grey; - float dc_gain_off_grey; - - float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; - int analog_gain_min_idx; - int analog_gain_max_idx; - int analog_gain_rec_idx; - int analog_gain_cost_delta; - float analog_gain_cost_low; - float analog_gain_cost_high; - float cur_ev[3]; - float min_ev, max_ev; float best_ev_score; int new_exp_g; int new_exp_t; float measured_grey_fraction; float target_grey_fraction; - float target_grey_factor; unique_fd sensor_fd; unique_fd csiphy_fd; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 7d7a454f7e..10034a7c3c 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -21,7 +21,7 @@ std::map> ar0231_build_register_lut(CameraState *c std::map> registers; for (int register_row = 0; register_row < 2; register_row++) { - uint8_t *registers_raw = data + c->ci.frame_stride * register_row; + uint8_t *registers_raw = data + c->ci->frame_stride * register_row; assert(registers_raw[0] == 0x0a); // Start of line int value_tag_count = 0; @@ -46,7 +46,7 @@ std::map> ar0231_build_register_lut(CameraState *c cur_addr += 2; first_val_idx = val_idx; } else { - registers[cur_addr] = std::make_pair(first_val_idx + c->ci.frame_stride * register_row, val_idx + c->ci.frame_stride * register_row); + registers[cur_addr] = std::make_pair(first_val_idx + c->ci->frame_stride * register_row, val_idx + c->ci->frame_stride * register_row); } value_tag_count++; @@ -80,7 +80,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; - uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { LOGE("unexpected register data found"); From ef262ff9eb5e52a58838479510947552a88ae3bd Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 03:19:10 +0800 Subject: [PATCH 70/87] camerad: move sensor related code to `/sensors` (#30647) --- release/files_common | 2 + system/camerad/SConscript | 2 +- system/camerad/cameras/camera_common.h | 33 ---- system/camerad/cameras/camera_qcom2.cc | 167 +-------------------- system/camerad/cameras/camera_qcom2.h | 3 +- system/camerad/sensors/ar0231.cc | 72 +++++++++ system/camerad/sensors/ar0231_registers.h | 6 +- system/camerad/sensors/ox03c10.cc | 88 +++++++++++ system/camerad/sensors/ox03c10_registers.h | 6 +- system/camerad/sensors/sensor.h | 59 ++++++++ 10 files changed, 238 insertions(+), 200 deletions(-) create mode 100644 system/camerad/sensors/ox03c10.cc create mode 100644 system/camerad/sensors/sensor.h diff --git a/release/files_common b/release/files_common index d73c71171a..637f8bd7d7 100644 --- a/release/files_common +++ b/release/files_common @@ -320,7 +320,9 @@ system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc system/camerad/sensors/ar0231.cc system/camerad/sensors/ar0231_registers.h +system/camerad/sensors/ox03c10.cc system/camerad/sensors/ox03c10_registers.h +system/camerad/sensors/sensor.h selfdrive/manager/__init__.py selfdrive/manager/build.py diff --git a/system/camerad/SConscript b/system/camerad/SConscript index d814ebf1b0..a0056c1f9f 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -3,7 +3,7 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', - 'sensors/ar0231.cc']) + 'sensors/ar0231.cc', 'sensors/ox03c10.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 8022fa844e..eedeaf1b7a 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -17,7 +17,6 @@ #define CAMERA_ID_AR0231 0 #define CAMERA_ID_OX03C10 1 -#define ANALOG_GAIN_MAX_CNT 55 const int YUV_BUFFER_COUNT = 20; enum CameraType { @@ -34,35 +33,6 @@ const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != NULL; const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != NULL; -typedef struct CameraInfo { - uint32_t frame_width, frame_height; - uint32_t frame_stride; - uint32_t frame_offset = 0; - uint32_t extra_height = 0; - int registers_offset = -1; - int stats_offset = -1; - - int exposure_time_min; - int exposure_time_max; - - float dc_gain_factor; - int dc_gain_min_weight; - int dc_gain_max_weight; - float dc_gain_on_grey; - float dc_gain_off_grey; - - float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; - int analog_gain_min_idx; - int analog_gain_max_idx; - int analog_gain_rec_idx; - int analog_gain_cost_delta; - float analog_gain_cost_low; - float analog_gain_cost_high; - float target_grey_factor; - float min_ev; - float max_ev; -} CameraInfo; - typedef struct FrameMetadata { uint32_t frame_id; @@ -123,6 +93,3 @@ void cameras_close(MultiCameraState *s); void camerad_thread(); int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); - -void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); - diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 2e84a8605b..75367a6478 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -19,85 +19,17 @@ #include "media/cam_defs.h" #include "media/cam_isp.h" #include "media/cam_isp_ife.h" -#include "media/cam_sensor.h" #include "media/cam_sensor_cmn_header.h" #include "media/cam_sync.h" #include "common/swaglog.h" -#include "system/camerad/sensors/ar0231_registers.h" -#include "system/camerad/sensors/ox03c10_registers.h" + +const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py // For debugging: // echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl extern ExitHandler do_exit; -const size_t FRAME_WIDTH = 1928; -const size_t FRAME_HEIGHT = 1208; -const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) - -const size_t AR0231_REGISTERS_HEIGHT = 2; -// TODO: this extra height is universal and doesn't apply per camera -const size_t AR0231_STATS_HEIGHT = 2+8; - -const int MIPI_SETTLE_CNT = 33; // Calculated by camera_freqs.py -const float DC_GAIN_AR0231 = 2.5; -const float DC_GAIN_OX03C10 = 7.32; - -const float DC_GAIN_ON_GREY_AR0231 = 0.2; -const float DC_GAIN_OFF_GREY_AR0231 = 0.3; -const float DC_GAIN_ON_GREY_OX03C10 = 0.9; -const float DC_GAIN_OFF_GREY_OX03C10 = 1.0; - -const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; -const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; -const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine -const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; - -const float TARGET_GREY_FACTOR_AR0231 = 1.0; -const float TARGET_GREY_FACTOR_OX03C10 = 0.01; - -const float sensor_analog_gains_AR0231[] = { - 1.0/8.0, 2.0/8.0, 2.0/7.0, 3.0/7.0, // 0, 1, 2, 3 - 3.0/6.0, 4.0/6.0, 4.0/5.0, 5.0/5.0, // 4, 5, 6, 7 - 5.0/4.0, 6.0/4.0, 6.0/3.0, 7.0/3.0, // 8, 9, 10, 11 - 7.0/2.0, 8.0/2.0, 8.0/1.0}; // 12, 13, 14, 15 = bypass - -const float sensor_analog_gains_OX03C10[] = { - 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, - 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, - 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, - 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, - 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; - -const uint32_t ox03c10_analog_gains_reg[] = { - 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, - 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, - 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, - 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, - 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; - -const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x -const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x -const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x -const int ANALOG_GAIN_COST_DELTA_AR0231 = 0; -const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1; -const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0; - -const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; -const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x -const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; -const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; -const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; -const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4; - -const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss -const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms - -const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x -const int EXPOSURE_TIME_MAX_OX03C10 = 2016; -const uint32_t VS_TIME_MIN_OX03C10 = 1; -const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 - int CameraState::clear_req_queue() { struct cam_req_mgr_flush_info req_mgr_flush_request = {0}; req_mgr_flush_request.session_hdl = session_handle; @@ -141,7 +73,7 @@ void CameraState::sensors_poke(int request_id) { } } -void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { +void CameraState::sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) { // LOGD("sensors_i2c: %d", len); uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1; @@ -496,74 +428,11 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -struct CameraAR0231 : public CameraInfo { - CameraAR0231() { - frame_width = FRAME_WIDTH, - frame_height = FRAME_HEIGHT, - frame_stride = FRAME_STRIDE, - extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT, - - registers_offset = 0, - frame_offset = AR0231_REGISTERS_HEIGHT, - stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT, - - dc_gain_factor = DC_GAIN_AR0231; - dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; - dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; - dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231; - dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231; - exposure_time_min = EXPOSURE_TIME_MIN_AR0231; - exposure_time_max = EXPOSURE_TIME_MAX_AR0231; - analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; - analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; - analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; - analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; - analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; - analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; - for (int i = 0; i <= analog_gain_max_idx; i++) { - sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; - } - min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; - target_grey_factor = TARGET_GREY_FACTOR_AR0231; - } -}; - -struct CameraOx0310 : public CameraInfo { - CameraOx0310() { - frame_width = FRAME_WIDTH, - frame_height = FRAME_HEIGHT, - frame_stride = FRAME_STRIDE, // (0xa80*12//8) - extra_height = 16, // top 2 + bot 14 - frame_offset = 2, - - dc_gain_factor = DC_GAIN_OX03C10; - dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; - dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; - dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10; - dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10; - exposure_time_min = EXPOSURE_TIME_MIN_OX03C10; - exposure_time_max = EXPOSURE_TIME_MAX_OX03C10; - analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; - analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; - analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; - analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10; - analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10; - analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10; - for (int i=0; i<=analog_gain_max_idx; i++) { - sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; - } - min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; - max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; - target_grey_factor = TARGET_GREY_FACTOR_OX03C10; - } -}; - void CameraState::camera_set_parameters() { if (camera_id == CAMERA_ID_AR0231) { ci = std::make_unique(); } else if (camera_id == CAMERA_ID_OX03C10) { - ci = std::make_unique(); + ci = std::make_unique(); } else { assert(false); } @@ -1092,31 +961,11 @@ void CameraState::set_camera_exposure(float grey_frac) { // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); if (camera_id == CAMERA_ID_AR0231) { - uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; - struct i2c_random_wr_payload exp_reg_array[] = { - {0x3366, analog_gain_reg}, - {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, - {0x3012, (uint16_t)exposure_time}, - }; - sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); + auto exp_reg_array = ar0231_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); + sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); } else if (camera_id == CAMERA_ID_OX03C10) { - // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD - uint32_t hcg_time = exposure_time; - uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); - uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); - - uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; - - struct i2c_random_wr_payload exp_reg_array[] = { - {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, - {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, - {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, - {0x35c2, vs_time&0xFF}, - - {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, - }; - sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); + auto exp_reg_array = ox03c10_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); + sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); } } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index b1765aa582..dc924c882a 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -9,6 +9,7 @@ #include "system/camerad/cameras/camera_common.h" #include "system/camerad/cameras/camera_util.h" +#include "system/camerad/sensors/sensor.h" #include "common/params.h" #include "common/util.h" @@ -82,7 +83,7 @@ public: int sensors_init(); void sensors_poke(int request_id); - void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); + void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); // Register parsing std::map> ar0231_register_lut; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 10034a7c3c..0ddb5b63d1 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -2,9 +2,40 @@ #include "system/camerad/cameras/camera_common.h" #include "system/camerad/cameras/camera_qcom2.h" +#include "system/camerad/sensors/sensor.h" namespace { +const size_t AR0231_REGISTERS_HEIGHT = 2; +// TODO: this extra height is universal and doesn't apply per camera +const size_t AR0231_STATS_HEIGHT = 2 + 8; + +const float DC_GAIN_AR0231 = 2.5; + +const float DC_GAIN_ON_GREY_AR0231 = 0.2; +const float DC_GAIN_OFF_GREY_AR0231 = 0.3; + +const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; +const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; + +const float TARGET_GREY_FACTOR_AR0231 = 1.0; + +const float sensor_analog_gains_AR0231[] = { + 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3 + 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7 + 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11 + 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass + +const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x +const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x +const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x +const int ANALOG_GAIN_COST_DELTA_AR0231 = 0; +const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1; +const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0; + +const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss +const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms + std::map> ar0231_build_register_lut(CameraState *c, uint8_t *data) { // This function builds a lookup table from register address, to a pair of indices in the // buffer where to read this address. The buffer contains padding bytes, @@ -78,6 +109,37 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace +CameraAR0231::CameraAR0231() { + frame_width = FRAME_WIDTH; + frame_height = FRAME_HEIGHT; + frame_stride = FRAME_STRIDE; + extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT; + + registers_offset = 0; + frame_offset = AR0231_REGISTERS_HEIGHT; + stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT; + + dc_gain_factor = DC_GAIN_AR0231; + dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; + dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231; + dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231; + exposure_time_min = EXPOSURE_TIME_MIN_AR0231; + exposure_time_max = EXPOSURE_TIME_MAX_AR0231; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; + analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; + analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; + analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; + } + min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = TARGET_GREY_FACTOR_AR0231; +} + void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; @@ -96,3 +158,13 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); framed.setTemperaturesC({temp_0, temp_1}); } + + +std::vector ar0231_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { + uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; + return { + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; +} diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h index 611d08ec8e..6c4c251e8e 100644 --- a/system/camerad/sensors/ar0231_registers.h +++ b/system/camerad/sensors/ar0231_registers.h @@ -1,9 +1,9 @@ #pragma once -struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; -struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; +const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; -struct i2c_random_wr_payload init_array_ar0231[] = { +const struct i2c_random_wr_payload init_array_ar0231[] = { {0x301A, 0x0018}, // RESET_REGISTER // CLOCK Settings diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc new file mode 100644 index 0000000000..c1e1cdf6ec --- /dev/null +++ b/system/camerad/sensors/ox03c10.cc @@ -0,0 +1,88 @@ +#include "system/camerad/sensors/sensor.h" + +namespace { + +const float DC_GAIN_OX03C10 = 7.32; + +const float DC_GAIN_ON_GREY_OX03C10 = 0.9; +const float DC_GAIN_OFF_GREY_OX03C10 = 1.0; + +const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine +const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; + +const float TARGET_GREY_FACTOR_OX03C10 = 0.01; + +const float sensor_analog_gains_OX03C10[] = { + 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, + 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, + 3.125, 3.375, 3.625, 3.875, 4.0, 4.25, 4.5, 4.75, 5.0, 5.25, 5.5, + 5.75, 6.0, 6.25, 6.5, 7.0, 7.5, 8.0, 8.5, 9.0, 9.5, 10.0, + 10.5, 11.0, 11.5, 12.0, 12.5, 13.0, 13.5, 14.0, 14.5, 15.0, 15.5}; + +const uint32_t ox03c10_analog_gains_reg[] = { + 0x100, 0x110, 0x120, 0x130, 0x140, 0x150, 0x160, 0x170, 0x180, 0x190, 0x1B0, + 0x1D0, 0x1F0, 0x200, 0x220, 0x240, 0x260, 0x280, 0x2A0, 0x2C0, 0x2E0, 0x300, + 0x320, 0x360, 0x3A0, 0x3E0, 0x400, 0x440, 0x480, 0x4C0, 0x500, 0x540, 0x580, + 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, + 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; + +const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; +const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x +const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; +const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; +const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; +const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4; + +const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x +const int EXPOSURE_TIME_MAX_OX03C10 = 2016; +const uint32_t VS_TIME_MIN_OX03C10 = 1; +const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 + +} // namespace + +CameraOx03c10::CameraOx03c10() { + frame_width = FRAME_WIDTH; + frame_height = FRAME_HEIGHT; + frame_stride = FRAME_STRIDE; // (0xa80*12//8) + extra_height = 16; // top 2 + bot 14 + frame_offset = 2; + + dc_gain_factor = DC_GAIN_OX03C10; + dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; + dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; + dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10; + dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10; + exposure_time_min = EXPOSURE_TIME_MIN_OX03C10; + exposure_time_max = EXPOSURE_TIME_MAX_OX03C10; + analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; + analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; + analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; + analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10; + analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10; + analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; + } + min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = TARGET_GREY_FACTOR_OX03C10; +} + +std::vector ox03c10_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { + // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD + uint32_t hcg_time = exposure_time; + uint32_t lcg_time = hcg_time; + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); + + uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; + + return { + {0x3501, hcg_time>>8}, {0x3502, hcg_time&0xFF}, + {0x3581, lcg_time>>8}, {0x3582, lcg_time&0xFF}, + {0x3541, spd_time>>8}, {0x3542, spd_time&0xFF}, + {0x35c2, vs_time&0xFF}, + + {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, + }; +} diff --git a/system/camerad/sensors/ox03c10_registers.h b/system/camerad/sensors/ox03c10_registers.h index 7aed2e8ac0..575a2cb934 100644 --- a/system/camerad/sensors/ox03c10_registers.h +++ b/system/camerad/sensors/ox03c10_registers.h @@ -1,9 +1,9 @@ #pragma once -struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; -struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; +const struct i2c_random_wr_payload start_reg_array_ox03c10[] = {{0x100, 1}}; +const struct i2c_random_wr_payload stop_reg_array_ox03c10[] = {{0x100, 0}}; -struct i2c_random_wr_payload init_array_ox03c10[] = { +const struct i2c_random_wr_payload init_array_ox03c10[] = { {0x103, 1}, {0x107, 1}, diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h new file mode 100644 index 0000000000..60fbdea81b --- /dev/null +++ b/system/camerad/sensors/sensor.h @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include "media/cam_sensor.h" +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/sensors/ar0231_registers.h" +#include "system/camerad/sensors/ox03c10_registers.h" + +#define ANALOG_GAIN_MAX_CNT 55 +const size_t FRAME_WIDTH = 1928; +const size_t FRAME_HEIGHT = 1208; +const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) + +class CameraInfo { +public: + CameraInfo() = default; + + uint32_t frame_width, frame_height; + uint32_t frame_stride; + uint32_t frame_offset = 0; + uint32_t extra_height = 0; + int registers_offset = -1; + int stats_offset = -1; + + int exposure_time_min; + int exposure_time_max; + + float dc_gain_factor; + int dc_gain_min_weight; + int dc_gain_max_weight; + float dc_gain_on_grey; + float dc_gain_off_grey; + + float sensor_analog_gains[ANALOG_GAIN_MAX_CNT]; + int analog_gain_min_idx; + int analog_gain_max_idx; + int analog_gain_rec_idx; + int analog_gain_cost_delta; + float analog_gain_cost_low; + float analog_gain_cost_high; + float target_grey_factor; + float min_ev; + float max_ev; +}; + +class CameraAR0231 : public CameraInfo { +public: + CameraAR0231(); +}; + +class CameraOx03c10 : public CameraInfo { +public: + CameraOx03c10(); +}; + +void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); +std::vector ox03c10_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); +std::vector ar0231_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); From 2590cf8615ca3dc86cb7bacae83bfb8f95bf8d80 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Dec 2023 11:39:30 -0800 Subject: [PATCH 71/87] update plannerd cpu usage --- selfdrive/test/test_onroad.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 2d674e4610..b4121529d1 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -35,7 +35,7 @@ PROCS = { "./camerad": 14.5, "./locationd": 11.0, "./mapsd": (1.0, 10.0), - "selfdrive.controls.plannerd": 16.5, + "selfdrive.controls.plannerd": 11.0, "./_ui": 18.0, "selfdrive.locationd.paramsd": 9.0, "./sensord": 7.0, From e757d9bae7bcd068d26224344e0227c12a5171cc Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Dec 2023 11:39:40 -0800 Subject: [PATCH 72/87] camerad: renames (#30649) * sensorinfo * drop the camera --- system/camerad/cameras/camera_common.cc | 4 ++-- system/camerad/cameras/camera_qcom2.cc | 4 ++-- system/camerad/cameras/camera_qcom2.h | 2 +- system/camerad/sensors/ar0231.cc | 4 ++-- system/camerad/sensors/ox03c10.cc | 4 ++-- system/camerad/sensors/sensor.h | 16 ++++++++-------- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index 703378b104..a53b0d412a 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -28,7 +28,7 @@ class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s, int buf_width, int uv_offset) { char args[4096]; - const CameraInfo *ci = s->ci.get(); + const SensorInfo *ci = s->ci.get(); snprintf(args, sizeof(args), "-cl-fast-relaxed-math -cl-denorms-are-zero " "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d -DFRAME_OFFSET=%d " @@ -66,7 +66,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s, this->yuv_type = init_yuv_type; frame_buf_count = frame_cnt; - const CameraInfo *ci = s->ci.get(); + const SensorInfo *ci = s->ci.get(); // RAW frame const int frame_size = (ci->frame_height + ci->extra_height) * ci->frame_stride; camera_bufs = std::make_unique(frame_buf_count); diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 75367a6478..f2ce058f4f 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -430,9 +430,9 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { void CameraState::camera_set_parameters() { if (camera_id == CAMERA_ID_AR0231) { - ci = std::make_unique(); + ci = std::make_unique(); } else if (camera_id == CAMERA_ID_OX03C10) { - ci = std::make_unique(); + ci = std::make_unique(); } else { assert(false); } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index dc924c882a..65c7abfb36 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -18,7 +18,7 @@ class CameraState { public: MultiCameraState *multi_cam_state; - std::unique_ptr ci; + std::unique_ptr ci; bool enabled; std::mutex exp_lock; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 0ddb5b63d1..b542f27c82 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -109,7 +109,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace -CameraAR0231::CameraAR0231() { +AR0231::AR0231() { frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; @@ -160,7 +160,7 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame } -std::vector ar0231_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; return { {0x3366, analog_gain_reg}, diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index c1e1cdf6ec..b60917b5e8 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -40,7 +40,7 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 } // namespace -CameraOx03c10::CameraOx03c10() { +OX03C10::OX03C10() { frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; // (0xa80*12//8) @@ -68,7 +68,7 @@ CameraOx03c10::CameraOx03c10() { target_grey_factor = TARGET_GREY_FACTOR_OX03C10; } -std::vector ox03c10_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 60fbdea81b..d038d44545 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -12,9 +12,9 @@ const size_t FRAME_WIDTH = 1928; const size_t FRAME_HEIGHT = 1208; const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alignment) -class CameraInfo { +class SensorInfo { public: - CameraInfo() = default; + SensorInfo() = default; uint32_t frame_width, frame_height; uint32_t frame_stride; @@ -44,16 +44,16 @@ public: float max_ev; }; -class CameraAR0231 : public CameraInfo { +class AR0231 : public SensorInfo { public: - CameraAR0231(); + AR0231(); }; -class CameraOx03c10 : public CameraInfo { +class OX03C10 : public SensorInfo { public: - CameraOx03c10(); + OX03C10(); }; void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); -std::vector ox03c10_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); -std::vector ar0231_get_exp_registers(const CameraInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); +std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); +std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); From 8fdcddec8a3798f2f100aec48a7e8047040c2c1d Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 05:01:41 +0800 Subject: [PATCH 73/87] camerad: define the constants directly in ctor (#30651) --- system/camerad/sensors/ar0231.cc | 48 +++++++++---------------------- system/camerad/sensors/ox03c10.cc | 47 +++++++++--------------------- 2 files changed, 28 insertions(+), 67 deletions(-) diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index b542f27c82..01a95252f4 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -10,32 +10,12 @@ const size_t AR0231_REGISTERS_HEIGHT = 2; // TODO: this extra height is universal and doesn't apply per camera const size_t AR0231_STATS_HEIGHT = 2 + 8; -const float DC_GAIN_AR0231 = 2.5; - -const float DC_GAIN_ON_GREY_AR0231 = 0.2; -const float DC_GAIN_OFF_GREY_AR0231 = 0.3; - -const int DC_GAIN_MIN_WEIGHT_AR0231 = 0; -const int DC_GAIN_MAX_WEIGHT_AR0231 = 1; - -const float TARGET_GREY_FACTOR_AR0231 = 1.0; - const float sensor_analog_gains_AR0231[] = { 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass -const int ANALOG_GAIN_MIN_IDX_AR0231 = 0x1; // 0.25x -const int ANALOG_GAIN_REC_IDX_AR0231 = 0x6; // 0.8x -const int ANALOG_GAIN_MAX_IDX_AR0231 = 0xD; // 4.0x -const int ANALOG_GAIN_COST_DELTA_AR0231 = 0; -const float ANALOG_GAIN_COST_LOW_AR0231 = 0.1; -const float ANALOG_GAIN_COST_HIGH_AR0231 = 5.0; - -const int EXPOSURE_TIME_MIN_AR0231 = 2; // with HDR, fastest ss -const int EXPOSURE_TIME_MAX_AR0231 = 0x0855; // with HDR, slowest ss, 40ms - std::map> ar0231_build_register_lut(CameraState *c, uint8_t *data) { // This function builds a lookup table from register address, to a pair of indices in the // buffer where to read this address. The buffer contains padding bytes, @@ -119,25 +99,25 @@ AR0231::AR0231() { frame_offset = AR0231_REGISTERS_HEIGHT; stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT; - dc_gain_factor = DC_GAIN_AR0231; - dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_AR0231; - dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_AR0231; - dc_gain_on_grey = DC_GAIN_ON_GREY_AR0231; - dc_gain_off_grey = DC_GAIN_OFF_GREY_AR0231; - exposure_time_min = EXPOSURE_TIME_MIN_AR0231; - exposure_time_max = EXPOSURE_TIME_MAX_AR0231; - analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_AR0231; - analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_AR0231; - analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_AR0231; - analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_AR0231; - analog_gain_cost_low = ANALOG_GAIN_COST_LOW_AR0231; - analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_AR0231; + dc_gain_factor = 2.5; + dc_gain_min_weight = 0; + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.2; + dc_gain_off_grey = 0.3; + exposure_time_min = 2; // with HDR, fastest ss + exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms + analog_gain_min_idx = 0x1; // 0.25x + analog_gain_rec_idx = 0x6; // 0.8x + analog_gain_max_idx = 0xD; // 4.0x + analog_gain_cost_delta = 0; + analog_gain_cost_low = 0.1; + analog_gain_cost_high = 5.0; for (int i = 0; i <= analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; } min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; - target_grey_factor = TARGET_GREY_FACTOR_AR0231; + target_grey_factor = 1.0; } void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index b60917b5e8..d55c73a436 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -2,16 +2,6 @@ namespace { -const float DC_GAIN_OX03C10 = 7.32; - -const float DC_GAIN_ON_GREY_OX03C10 = 0.9; -const float DC_GAIN_OFF_GREY_OX03C10 = 1.0; - -const int DC_GAIN_MIN_WEIGHT_OX03C10 = 1; // always on is fine -const int DC_GAIN_MAX_WEIGHT_OX03C10 = 1; - -const float TARGET_GREY_FACTOR_OX03C10 = 0.01; - const float sensor_analog_gains_OX03C10[] = { 1.0, 1.0625, 1.125, 1.1875, 1.25, 1.3125, 1.375, 1.4375, 1.5, 1.5625, 1.6875, 1.8125, 1.9375, 2.0, 2.125, 2.25, 2.375, 2.5, 2.625, 2.75, 2.875, 3.0, @@ -26,15 +16,6 @@ const uint32_t ox03c10_analog_gains_reg[] = { 0x5C0, 0x600, 0x640, 0x680, 0x700, 0x780, 0x800, 0x880, 0x900, 0x980, 0xA00, 0xA80, 0xB00, 0xB80, 0xC00, 0xC80, 0xD00, 0xD80, 0xE00, 0xE80, 0xF00, 0xF80}; -const int ANALOG_GAIN_MIN_IDX_OX03C10 = 0x0; -const int ANALOG_GAIN_REC_IDX_OX03C10 = 0x0; // 1x -const int ANALOG_GAIN_MAX_IDX_OX03C10 = 0x36; -const int ANALOG_GAIN_COST_DELTA_OX03C10 = -1; -const float ANALOG_GAIN_COST_LOW_OX03C10 = 0.4; -const float ANALOG_GAIN_COST_HIGH_OX03C10 = 6.4; - -const int EXPOSURE_TIME_MIN_OX03C10 = 2; // 1x -const int EXPOSURE_TIME_MAX_OX03C10 = 2016; const uint32_t VS_TIME_MIN_OX03C10 = 1; const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 @@ -47,25 +28,25 @@ OX03C10::OX03C10() { extra_height = 16; // top 2 + bot 14 frame_offset = 2; - dc_gain_factor = DC_GAIN_OX03C10; - dc_gain_min_weight = DC_GAIN_MIN_WEIGHT_OX03C10; - dc_gain_max_weight = DC_GAIN_MAX_WEIGHT_OX03C10; - dc_gain_on_grey = DC_GAIN_ON_GREY_OX03C10; - dc_gain_off_grey = DC_GAIN_OFF_GREY_OX03C10; - exposure_time_min = EXPOSURE_TIME_MIN_OX03C10; - exposure_time_max = EXPOSURE_TIME_MAX_OX03C10; - analog_gain_min_idx = ANALOG_GAIN_MIN_IDX_OX03C10; - analog_gain_rec_idx = ANALOG_GAIN_REC_IDX_OX03C10; - analog_gain_max_idx = ANALOG_GAIN_MAX_IDX_OX03C10; - analog_gain_cost_delta = ANALOG_GAIN_COST_DELTA_OX03C10; - analog_gain_cost_low = ANALOG_GAIN_COST_LOW_OX03C10; - analog_gain_cost_high = ANALOG_GAIN_COST_HIGH_OX03C10; + dc_gain_factor = 7.32; + dc_gain_min_weight = 1; // always on is fine + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.9; + dc_gain_off_grey = 1.0; + exposure_time_min = 2; // 1x + exposure_time_max = 2016; + analog_gain_min_idx = 0x0; + analog_gain_rec_idx = 0x0; // 1x + analog_gain_max_idx = 0x36; + analog_gain_cost_delta = -1; + analog_gain_cost_low = 0.4; + analog_gain_cost_high = 6.4; for (int i = 0; i <= analog_gain_max_idx; i++) { sensor_analog_gains[i] = sensor_analog_gains_OX03C10[i]; } min_ev = (exposure_time_min + VS_TIME_MIN_OX03C10) * sensor_analog_gains[analog_gain_min_idx]; max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; - target_grey_factor = TARGET_GREY_FACTOR_OX03C10; + target_grey_factor = 0.01; } std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { From 011b1a6e6a1db1047b2c72bc7e9e273a358ab18d Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 05:48:31 +0800 Subject: [PATCH 74/87] ui/ParamControl: do not create ConfirmationDialog on every click (#30496) --- selfdrive/ui/qt/widgets/controls.cc | 25 +++++++++++++++++++++++++ selfdrive/ui/qt/widgets/controls.h | 20 ++------------------ 2 files changed, 27 insertions(+), 18 deletions(-) diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc index 18a79b5983..40dda971f5 100644 --- a/selfdrive/ui/qt/widgets/controls.cc +++ b/selfdrive/ui/qt/widgets/controls.cc @@ -114,3 +114,28 @@ void ElidedLabel::paintEvent(QPaintEvent *event) { opt.initFrom(this); style()->drawItemText(&painter, contentsRect(), alignment(), opt.palette, isEnabled(), elidedText_, foregroundRole()); } + +// ParamControl + +ParamControl::ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent) + : ToggleControl(title, desc, icon, false, parent) { + key = param.toStdString(); + QObject::connect(this, &ParamControl::toggleFlipped, this, &ParamControl::toggleClicked); +} + +void ParamControl::toggleClicked(bool state) { + auto do_confirm = [this]() { + QString content("

" + title_label->text() + "


" + "

" + getDescription() + "

"); + return ConfirmationDialog(content, tr("Enable"), tr("Cancel"), true, this).exec(); + }; + + bool confirmed = store_confirm && params.getBool(key + "Confirmed"); + if (!confirm || confirmed || !state || do_confirm()) { + if (store_confirm && state) params.putBool(key + "Confirmed", true); + params.putBool(key, state); + setIcon(state); + } else { + toggle.togglePosition(); + } +} diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h index 811595726d..e4630c6590 100644 --- a/selfdrive/ui/qt/widgets/controls.h +++ b/selfdrive/ui/qt/widgets/controls.h @@ -144,24 +144,7 @@ class ParamControl : public ToggleControl { Q_OBJECT public: - ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr) : ToggleControl(title, desc, icon, false, parent) { - key = param.toStdString(); - QObject::connect(this, &ParamControl::toggleFlipped, [=](bool state) { - QString content("

" + title + "


" - "

" + getDescription() + "

"); - ConfirmationDialog dialog(content, tr("Enable"), tr("Cancel"), true, this); - - bool confirmed = store_confirm && params.getBool(key + "Confirmed"); - if (!confirm || confirmed || !state || dialog.exec()) { - if (store_confirm && state) params.putBool(key + "Confirmed", true); - params.putBool(key, state); - setIcon(state); - } else { - toggle.togglePosition(); - } - }); - } - + ParamControl(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr); void setConfirmation(bool _confirm, bool _store_confirm) { confirm = _confirm; store_confirm = _store_confirm; @@ -184,6 +167,7 @@ public: } private: + void toggleClicked(bool state); void setIcon(bool state) { if (state && !active_icon_pixmap.isNull()) { icon_label->setPixmap(active_icon_pixmap); From fb2f2d9cb21d8109d14a9d9790fda7de07202910 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 06:27:28 +0800 Subject: [PATCH 75/87] camerad: c++ sensorInfo (#30650) * move remaining sensor parameters to CameraInfo * same order * member functions * fix segfault --- system/camerad/cameras/camera_qcom2.cc | 94 +++++--------------------- system/camerad/cameras/camera_qcom2.h | 2 +- system/camerad/sensors/ar0231.cc | 28 +++++++- system/camerad/sensors/ox03c10.cc | 27 +++++++- system/camerad/sensors/sensor.h | 24 +++++-- 5 files changed, 88 insertions(+), 87 deletions(-) diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index f2ce058f4f..2592721886 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -46,13 +46,7 @@ int CameraState::clear_req_queue() { void CameraState::sensors_start() { if (!enabled) return; LOGD("starting sensor %d", camera_num); - if (camera_id == CAMERA_ID_AR0231) { - sensors_i2c(start_reg_array_ar0231, std::size(start_reg_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_OX03C10) { - sensors_i2c(start_reg_array_ox03c10, std::size(start_reg_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - } else { - assert(false); - } + sensors_i2c(ci->start_reg_array.data(), ci->start_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); } void CameraState::sensors_poke(int request_id) { @@ -112,6 +106,8 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { } int CameraState::sensors_init() { + create_sensor(); + uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; auto pkt = mm.alloc(size, &cam_packet_handle); @@ -127,21 +123,7 @@ int CameraState::sensors_init() { auto probe = (struct cam_cmd_probe *)(i2c_info.get() + 1); probe->camera_id = camera_num; - switch (camera_num) { - case 0: - // port 0 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; // 6C = 0x36*2 - break; - case 1: - // port 1 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x20; - break; - case 2: - // port 2 - i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x6C; - break; - } - + i2c_info->slave_addr = ci->getSlaveAddress(camera_num); // 0(I2C_STANDARD_MODE) = 100khz, 1(I2C_FAST_MODE) = 400khz //i2c_info->i2c_freq_mode = I2C_STANDARD_MODE; i2c_info->i2c_freq_mode = I2C_FAST_MODE; @@ -151,15 +133,8 @@ int CameraState::sensors_init() { probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD; probe->op_code = 3; // don't care? probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE; - if (camera_id == CAMERA_ID_AR0231) { - probe->reg_addr = 0x3000; - probe->expected_data = 0x354; - } else if (camera_id == CAMERA_ID_OX03C10) { - probe->reg_addr = 0x300a; - probe->expected_data = 0x5803; - } else { - assert(false); - } + probe->reg_addr = ci->probe_reg_addr; + probe->expected_data = ci->probe_expected_data; probe->data_mask = 0; //buf_desc[1].size = buf_desc[1].length = 148; @@ -182,7 +157,7 @@ int CameraState::sensors_init() { power->count = 1; power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP; power->power_settings[0].power_seq_type = 0; - power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz + power->power_settings[0].config_val_low = ci->power_config_val_low; power = power_set_wait(power, 1); // reset high @@ -428,7 +403,7 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::camera_set_parameters() { +void CameraState::create_sensor() { if (camera_id == CAMERA_ID_AR0231) { ci = std::make_unique(); } else if (camera_id == CAMERA_ID_OX03C10) { @@ -504,8 +479,6 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num return; } - camera_set_parameters(); - // create session struct cam_req_mgr_session_info session_info = {}; ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); @@ -520,18 +493,8 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num LOGD("acquire sensor dev"); LOG("-- Configuring sensor"); - uint32_t dt; - if (camera_id == CAMERA_ID_AR0231) { - sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead - } else if (camera_id == CAMERA_ID_OX03C10) { - sensors_i2c(init_array_ox03c10, std::size(init_array_ox03c10), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - // one is 0x2a, two are 0x2b - dt = 0x2c; - } else { - assert(false); - } - printf("dt is %x\n", dt); + sensors_i2c(ci->init_reg_array.data(), ci->init_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); + printf("dt is %x\n", ci->in_port_info_dt); // NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c // If you don't do this, the strobe GPIO is an output (even in reset it seems!) @@ -545,7 +508,7 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num .lane_cfg = 0x3210, .vc = 0x0, - .dt = dt, + .dt = ci->in_port_info_dt, .format = CAM_FORMAT_MIPI_RAW_12, .test_pattern = 0x2, // 0x3? @@ -839,22 +802,7 @@ void CameraState::handle_camera_event(void *evdat) { } void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) { - float score = 1e6; - if (camera_id == CAMERA_ID_AR0231) { - // Cost of ev diff - score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; - // Cost of absolute gain - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; - // Cost of changing gain - score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; - } else if (camera_id == CAMERA_ID_OX03C10) { - score = std::abs(desired_ev - (exp_t * exp_gain)); - float m = exp_g_idx > ci->analog_gain_rec_idx ? ci->analog_gain_cost_high : ci->analog_gain_cost_low; - score += std::abs(exp_g_idx - (int)ci->analog_gain_rec_idx) * m; - score += ((1 - ci->analog_gain_cost_delta) + ci->analog_gain_cost_delta * (exp_g_idx - ci->analog_gain_min_idx) / (ci->analog_gain_max_idx - ci->analog_gain_min_idx)) * std::abs(exp_g_idx - gain_idx) * 5.0; - } - + float score = ci->getExposureScore(desired_ev, exp_t, exp_g_idx, exp_gain, gain_idx); if (score < best_ev_score) { new_exp_t = exp_t; new_exp_g = exp_g_idx; @@ -960,13 +908,8 @@ void CameraState::set_camera_exposure(float grey_frac) { } // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof)); - if (camera_id == CAMERA_ID_AR0231) { - auto exp_reg_array = ar0231_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); - sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true); - } else if (camera_id == CAMERA_ID_OX03C10) { - auto exp_reg_array = ox03c10_get_exp_registers(ci.get(), exposure_time, new_exp_g, dc_gain_enabled); - sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false); - } + auto exp_reg_array = ci->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled); + sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, ci->data_word); } static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { @@ -977,9 +920,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) framed.setFrameType(cereal::FrameData::FrameType::FRONT); fill_frame_data(framed, c->buf.cur_frame_data, c); - if (c->camera_id == CAMERA_ID_AR0231) { - ar0231_process_registers(s, c, framed); - } + c->ci->processRegisters(s, c, framed); s->pm->send("driverCameraState", msg); } @@ -994,10 +935,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { } LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); - if (c->camera_id == CAMERA_ID_AR0231) { - ar0231_process_registers(s, c, framed); - } - + c->ci->processRegisters(s, c, framed); s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 65c7abfb36..03c766a2ee 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -49,7 +49,7 @@ public: void sensors_start(); void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); - void camera_set_parameters(); + void create_sensor(); void camera_map_bufs(MultiCameraState *s); void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 01a95252f4..c750533165 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -90,6 +90,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace AR0231::AR0231() { + data_word = true; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; @@ -99,6 +100,13 @@ AR0231::AR0231() { frame_offset = AR0231_REGISTERS_HEIGHT; stats_offset = AR0231_REGISTERS_HEIGHT + FRAME_HEIGHT; + start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231)); + init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231)); + probe_reg_addr = 0x3000; + probe_expected_data = 0x354; + in_port_info_dt = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + power_config_val_low = 19200000; //Hz + dc_gain_factor = 2.5; dc_gain_min_weight = 0; dc_gain_max_weight = 1; @@ -120,7 +128,7 @@ AR0231::AR0231() { target_grey_factor = 1.0; } -void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { +void AR0231::processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; @@ -140,7 +148,7 @@ void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::Frame } -std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +std::vector AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; return { {0x3366, analog_gain_reg}, @@ -148,3 +156,19 @@ std::vector ar0231_get_exp_registers(const SensorI {0x3012, (uint16_t)exposure_time}, }; } + +int AR0231::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x20, 0x30, 0x20}[port]; +} + +float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + // Cost of ev diff + float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; + // Cost of absolute gain + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + // Cost of changing gain + score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; + return score; +} diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index d55c73a436..c30838a7fe 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -22,12 +22,20 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 } // namespace OX03C10::OX03C10() { + data_word = false; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; frame_stride = FRAME_STRIDE; // (0xa80*12//8) extra_height = 16; // top 2 + bot 14 frame_offset = 2; + start_reg_array.assign(std::begin(start_reg_array_ox03c10), std::end(start_reg_array_ox03c10)); + init_reg_array.assign(std::begin(init_array_ox03c10), std::end(init_array_ox03c10)); + probe_reg_addr = 0x300a; + probe_expected_data = 0x5803; + in_port_info_dt = 0x2c; // one is 0x2a, two are 0x2b + power_config_val_low = 24000000; //Hz + dc_gain_factor = 7.32; dc_gain_min_weight = 1; // always on is fine dc_gain_max_weight = 1; @@ -49,11 +57,11 @@ OX03C10::OX03C10() { target_grey_factor = 0.01; } -std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled) { +std::vector OX03C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { // t_HCG&t_LCG + t_VS on LPD, t_SPD on SPD uint32_t hcg_time = exposure_time; uint32_t lcg_time = hcg_time; - uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (ci->exposure_time_max + VS_TIME_MAX_OX03C10) / 3), ci->exposure_time_max + VS_TIME_MAX_OX03C10); + uint32_t spd_time = std::min(std::max((uint32_t)exposure_time, (exposure_time_max + VS_TIME_MAX_OX03C10) / 3), exposure_time_max + VS_TIME_MAX_OX03C10); uint32_t vs_time = std::min(std::max((uint32_t)exposure_time / 40, VS_TIME_MIN_OX03C10), VS_TIME_MAX_OX03C10); uint32_t real_gain = ox03c10_analog_gains_reg[new_exp_g]; @@ -67,3 +75,18 @@ std::vector ox03c10_get_exp_registers(const Sensor {0x3508, real_gain>>8}, {0x3509, real_gain&0xFF}, }; } + +int OX03C10::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x6C, 0x20, 0x6C}[port]; +} + +float OX03C10::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + float score = std::abs(desired_ev - (exp_t * exp_gain)); + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + score += ((1 - analog_gain_cost_delta) + + analog_gain_cost_delta * (exp_g_idx - analog_gain_min_idx) / (analog_gain_max_idx - analog_gain_min_idx)) * + std::abs(exp_g_idx - gain_idx) * 5.0; + return score; +} diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index d038d44545..62cb31c52c 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include "media/cam_sensor.h" @@ -15,6 +16,10 @@ const size_t FRAME_STRIDE = 2896; // for 12 bit output. 1928 * 12 / 8 + 4 (alig class SensorInfo { public: SensorInfo() = default; + virtual std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; } + virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; } + virtual int getSlaveAddress(int port) const { assert(0); } + virtual void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const {} uint32_t frame_width, frame_height; uint32_t frame_stride; @@ -42,18 +47,29 @@ public: float target_grey_factor; float min_ev; float max_ev; + + bool data_word; + uint32_t probe_reg_addr; + uint32_t probe_expected_data; + std::vector start_reg_array; + std::vector init_reg_array; + uint32_t in_port_info_dt; + uint32_t power_config_val_low; }; class AR0231 : public SensorInfo { public: AR0231(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; + void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const override; }; class OX03C10 : public SensorInfo { public: OX03C10(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; }; - -void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); -std::vector ox03c10_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); -std::vector ar0231_get_exp_registers(const SensorInfo *ci, int exposure_time, int new_exp_g, bool dc_gain_enabled); From 80bc5833e7a7df4f9f258b19b38f67209b1d413e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Dec 2023 14:44:39 -0800 Subject: [PATCH 76/87] remove common/xattr.py --- common/xattr.py | 46 ---------------------------------------------- 1 file changed, 46 deletions(-) delete mode 100644 common/xattr.py diff --git a/common/xattr.py b/common/xattr.py deleted file mode 100644 index 26616fd638..0000000000 --- a/common/xattr.py +++ /dev/null @@ -1,46 +0,0 @@ -import os -from cffi import FFI -from typing import Any, List - -# Workaround for the EON/termux build of Python having os.*xattr removed. -ffi = FFI() -ffi.cdef(""" -int setxattr(const char *path, const char *name, const void *value, size_t size, int flags); -ssize_t getxattr(const char *path, const char *name, void *value, size_t size); -ssize_t listxattr(const char *path, char *list, size_t size); -int removexattr(const char *path, const char *name); -""") -libc = ffi.dlopen(None) - -def setxattr(path, name, value, flags=0) -> None: - path = path.encode() - name = name.encode() - if libc.setxattr(path, name, value, len(value), flags) == -1: - raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: setxattr({path}, {name}, {value}, {flags})") - -def getxattr(path, name, size=128): - path = path.encode() - name = name.encode() - value = ffi.new(f"char[{size}]") - l = libc.getxattr(path, name, value, size) - if l == -1: - # errno 61 means attribute hasn't been set - if ffi.errno == 61: - return None - raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: getxattr({path}, {name}, {size})") - return ffi.buffer(value)[:l] - -def listxattr(path, size=128) -> List[Any]: - path = path.encode() - attrs = ffi.new(f"char[{size}]") - l = libc.listxattr(path, attrs, size) - if l == -1: - raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: listxattr({path}, {size})") - # attrs is b'\0' delimited values (so chop off trailing empty item) - return [a.decode() for a in ffi.buffer(attrs)[:l].split(b"\0")[0:-1]] - -def removexattr(path, name) -> None: - path = path.encode() - name = name.encode() - if libc.removexattr(path, name) == -1: - raise OSError(ffi.errno, f"{os.strerror(ffi.errno)}: removexattr({path}, {name})") From 21d5d7d07a278635a1166aef5d5b209e32986382 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 06:57:29 +0800 Subject: [PATCH 77/87] camerad: move ar0231_register_lut to AR0231 (#30652) --- system/camerad/cameras/camera_qcom2.cc | 4 ++-- system/camerad/cameras/camera_qcom2.h | 6 ------ system/camerad/sensors/ar0231.cc | 24 +++++++++--------------- system/camerad/sensors/sensor.h | 9 +++++++-- 4 files changed, 18 insertions(+), 25 deletions(-) diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 2592721886..2462e969c4 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -920,7 +920,7 @@ static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) framed.setFrameType(cereal::FrameData::FrameType::FRONT); fill_frame_data(framed, c->buf.cur_frame_data, c); - c->ci->processRegisters(s, c, framed); + c->ci->processRegisters(c, framed); s->pm->send("driverCameraState", msg); } @@ -935,7 +935,7 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { } LOGT(c->buf.cur_frame_data.frame_id, "%s: Image set", c == &s->road_cam ? "RoadCamera" : "WideRoadCamera"); - c->ci->processRegisters(s, c, framed); + c->ci->processRegisters(c, framed); s->pm->send(c == &s->road_cam ? "roadCameraState" : "wideRoadCameraState", msg); const auto [x, y, w, h] = (c == &s->wide_road_cam) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 03c766a2ee..50a68b0b19 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include #include @@ -54,8 +53,6 @@ public: void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); - std::map ar0231_parse_registers(uint8_t *data, std::initializer_list addrs); - int32_t session_handle; int32_t sensor_dev_handle; int32_t isp_dev_handle; @@ -85,9 +82,6 @@ public: void sensors_poke(int request_id); void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word); - // Register parsing - std::map> ar0231_register_lut; - private: // for debugging Params params; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index c750533165..9a70befdcc 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -67,19 +67,6 @@ std::map> ar0231_build_register_lut(CameraState *c return registers; } -std::map ar0231_parse_registers(CameraState *c, uint8_t *data, std::initializer_list addrs) { - if (c->ar0231_register_lut.empty()) { - c->ar0231_register_lut = ar0231_build_register_lut(c, data); - } - - std::map registers; - for (uint16_t addr : addrs) { - auto offset = c->ar0231_register_lut[addr]; - registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; - } - return registers; -} - float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { // See AR0231 Developer Guide - page 36 float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); @@ -128,7 +115,7 @@ AR0231::AR0231() { target_grey_factor = 1.0; } -void AR0231::processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const { +void AR0231::processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const { const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci->registers_offset; @@ -137,7 +124,14 @@ void AR0231::processRegisters(MultiCameraState *s, CameraState *c, cereal::Frame return; } - auto registers = ar0231_parse_registers(c, data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); + if (ar0231_register_lut.empty()) { + ar0231_register_lut = ar0231_build_register_lut(c, data); + } + std::map registers; + for (uint16_t addr : {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}) { + auto offset = ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; framed.setFrameIdSensor(frame_id); diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 62cb31c52c..1e45778f20 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -2,6 +2,8 @@ #include #include +#include +#include #include #include "media/cam_sensor.h" #include "system/camerad/cameras/camera_common.h" @@ -19,7 +21,7 @@ public: virtual std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { return {}; } virtual float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const {return 0; } virtual int getSlaveAddress(int port) const { assert(0); } - virtual void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const {} + virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {} uint32_t frame_width, frame_height; uint32_t frame_stride; @@ -63,7 +65,10 @@ public: std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; int getSlaveAddress(int port) const override; - void processRegisters(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) const override; + void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const override; + +private: + mutable std::map> ar0231_register_lut; }; class OX03C10 : public SensorInfo { From bf4026ed7e55866e1a27f5aee277fbdbeb4853cd Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 8 Dec 2023 17:31:10 -0600 Subject: [PATCH 78/87] Toyota: enable LTA (#30109) * bump panda * enable lta * try upper/lower delay similar to longcontrol lag comp * add comment about eps torque rate limits * more clear * more clear * Revert "try upper/lower delay similar to longcontrol lag comp" This reverts commit 8e85333ee6e2456fcda076af4c31a9e9babbc897. * bump * no driver torque limiting to test * fix the setme_x3 * bump * bump * enable inactive safety * use vEgoRaw * rename * fix * some comments/organization * bump * docs * no corolla * shorter name * bump * bump * add to releases * bump panda * remove camry * bump * bump * rm * bump * bump * bump again * bump --- RELEASES.md | 2 ++ docs/CARS.md | 4 +++- panda | 2 +- selfdrive/car/toyota/interface.py | 1 - 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 28c3da70a2..07f70cdaff 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -2,6 +2,8 @@ Version 0.9.6 (2023-12-14) ======================== * AGNOS 9 * comma body streaming and controls over WebRTC +* Toyota RAV4 2023 support +* Toyota RAV4 Hybrid 2023 support Version 0.9.5 (2023-11-17) ======================== diff --git a/docs/CARS.md b/docs/CARS.md index 5113eef50e..647b1b15c5 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -4,7 +4,7 @@ A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 269 Supported Cars +# 271 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -237,10 +237,12 @@ A supported vehicle is one that just works when you install a comma device. All |Toyota|RAV4 2017-18|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Toyota|RAV4 2023|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 Hybrid 2016|Toyota Safety Sense P|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 Hybrid 2017-18|All|openpilot available[2](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 Hybrid 2019-21|All|openpilot|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|RAV4 Hybrid 2022|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Toyota|RAV4 Hybrid 2023|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Toyota|Sienna 2018-20|All|openpilot available[2](#footnotes)|19 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 RJ45 cable (7 ft)
- 1 Toyota A connector
- 1 comma 3X
- 1 comma power v2
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 J533 connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|openpilot available[1,12](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|
Parts- 1 J533 connector
- 1 USB-C coupler
- 1 comma 3X
- 1 harness box
- 1 long OBD-C cable
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/panda b/panda index ea78657bef..80782bcbe4 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ea78657bef236c6a74b44549e541caf1d7fee618 +Subproject commit 80782bcbe49705810d0859debeff18d86b2cc56e diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index ea28d18302..ae4cb1f2c5 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -28,7 +28,6 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE if candidate in ANGLE_CONTROL_CAR: - ret.dashcamOnly = True ret.steerControlType = SteerControlType.angle ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA From ea0b8920f578e2b6f348d2e18b5783166731c880 Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 9 Dec 2023 08:15:58 +0800 Subject: [PATCH 79/87] camerad: remove camera_id (#30654) * remove camera_id * use variable --- system/camerad/cameras/camera_common.cc | 9 ++--- system/camerad/cameras/camera_common.h | 3 -- system/camerad/cameras/camera_qcom2.cc | 46 ++++++++++--------------- system/camerad/cameras/camera_qcom2.h | 5 ++- system/camerad/sensors/ar0231.cc | 1 + system/camerad/sensors/ox03c10.cc | 1 + system/camerad/sensors/sensor.h | 1 + 7 files changed, 26 insertions(+), 40 deletions(-) diff --git a/system/camerad/cameras/camera_common.cc b/system/camerad/cameras/camera_common.cc index a53b0d412a..e36766ca2d 100644 --- a/system/camerad/cameras/camera_common.cc +++ b/system/camerad/cameras/camera_common.cc @@ -36,7 +36,7 @@ public: "-DIS_OX=%d -DCAM_NUM=%d%s", ci->frame_width, ci->frame_height, ci->frame_stride, ci->frame_offset, b->rgb_width, b->rgb_height, b->rgb_stride, buf_width, uv_offset, - s->camera_id==CAMERA_ID_OX03C10 ? 1 : 0, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); + ci->image_sensor == cereal::FrameData::ImageSensor::OX03C10, s->camera_num, s->camera_num==1 ? " -DVIGNETTING" : ""); const char *cl_file = "cameras/real_debayer.cl"; cl_program prg_debayer = cl_program_from_file(context, device_id, cl_file, args); krnl_ = CL_CHECK_ERR(clCreateKernel(prg_debayer, "debayer10", &err)); @@ -154,12 +154,7 @@ void fill_frame_data(cereal::FrameData::Builder &framed, const FrameMetadata &fr const float ev = c->cur_ev[frame_data.frame_id % 3]; const float perc = util::map_val(ev, c->ci->min_ev, c->ci->max_ev, 0.0f, 100.0f); framed.setExposureValPercent(perc); - - if (c->camera_id == CAMERA_ID_AR0231) { - framed.setSensor(cereal::FrameData::ImageSensor::AR0231); - } else if (c->camera_id == CAMERA_ID_OX03C10) { - framed.setSensor(cereal::FrameData::ImageSensor::OX03C10); - } + framed.setSensor(c->ci->image_sensor); } kj::Array get_raw_frame_image(const CameraBuf *b) { diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index eedeaf1b7a..e93d357b8d 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -14,9 +14,6 @@ #include "common/swaglog.h" #include "system/hardware/hw.h" -#define CAMERA_ID_AR0231 0 -#define CAMERA_ID_OX03C10 1 - const int YUV_BUFFER_COUNT = 20; enum CameraType { diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 2462e969c4..194f11fe6d 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -106,8 +106,6 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) { } int CameraState::sensors_init() { - create_sensor(); - uint32_t cam_packet_handle = 0; int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2; auto pkt = mm.alloc(size, &cam_packet_handle); @@ -403,15 +401,7 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) { // ******************* camera ******************* -void CameraState::create_sensor() { - if (camera_id == CAMERA_ID_AR0231) { - ci = std::make_unique(); - } else if (camera_id == CAMERA_ID_OX03C10) { - ci = std::make_unique(); - } else { - assert(false); - } - +void CameraState::sensor_set_parameters() { target_grey_fraction = 0.3; dc_gain_enabled = false; @@ -436,9 +426,8 @@ void CameraState::camera_map_bufs(MultiCameraState *s) { enqueue_req_multi(1, FRAME_BUF_COUNT, 0); } -void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, int camera_id_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { +void CameraState::camera_init(MultiCameraState *s, VisionIpcServer * v, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type) { if (!enabled) return; - camera_id = camera_id_; LOGD("camera init %d", camera_num); request_id_last = 0; @@ -462,22 +451,25 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num // init memorymanager for this camera mm.init(multi_cam_state->video0_fd); - // probe the sensor LOGD("-- Probing sensor %d", camera_num); - camera_id = CAMERA_ID_AR0231; - ret = sensors_init(); - if (ret != 0) { - // TODO: use build flag instead? - LOGD("AR0231 init failed, trying OX03C10"); - camera_id = CAMERA_ID_OX03C10; - ret = sensors_init(); - } - LOGD("-- Probing sensor %d done with %d", camera_num, ret); - if (ret != 0) { + + auto init_sensor_lambda = [this](SensorInfo *sensor) { + ci.reset(sensor); + int ret = sensors_init(); + if (ret == 0) { + sensor_set_parameters(); + } + return ret == 0; + }; + + // Try different sensors one by one until it success. + if (!init_sensor_lambda(new AR0231) && + !init_sensor_lambda(new OX03C10)) { LOGE("** sensor %d FAILED bringup, disabling", camera_num); enabled = false; return; } + LOGD("-- Probing sensor %d success", camera_num); // create session struct cam_req_mgr_session_info session_info = {}; @@ -626,9 +618,9 @@ void CameraState::camera_open(MultiCameraState *multi_cam_state_, int camera_num } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - s->driver_cam.camera_init(s, v, s->driver_cam.camera_id, 20, device_id, ctx, VISION_STREAM_DRIVER); - s->road_cam.camera_init(s, v, s->road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_ROAD); - s->wide_road_cam.camera_init(s, v, s->wide_road_cam.camera_id, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); + s->driver_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_DRIVER); + s->road_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_ROAD); + s->wide_road_cam.camera_init(s, v, 20, device_id, ctx, VISION_STREAM_WIDE_ROAD); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); } diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 50a68b0b19..3da5ec207d 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -48,9 +48,9 @@ public: void sensors_start(); void camera_open(MultiCameraState *multi_cam_state, int camera_num, bool enabled); - void create_sensor(); + void sensor_set_parameters(); void camera_map_bufs(MultiCameraState *s); - void camera_init(MultiCameraState *s, VisionIpcServer *v, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); + void camera_init(MultiCameraState *s, VisionIpcServer *v, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType yuv_type); void camera_close(); int32_t session_handle; @@ -68,7 +68,6 @@ public: int frame_id_last; int idx_offset; bool skipped; - int camera_id; CameraBuf buf; MemoryManager mm; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc index 9a70befdcc..9a657982a8 100644 --- a/system/camerad/sensors/ar0231.cc +++ b/system/camerad/sensors/ar0231.cc @@ -77,6 +77,7 @@ float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_r } // namespace AR0231::AR0231() { + image_sensor = cereal::FrameData::ImageSensor::AR0231; data_word = true; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; diff --git a/system/camerad/sensors/ox03c10.cc b/system/camerad/sensors/ox03c10.cc index c30838a7fe..2bad50cff9 100644 --- a/system/camerad/sensors/ox03c10.cc +++ b/system/camerad/sensors/ox03c10.cc @@ -22,6 +22,7 @@ const uint32_t VS_TIME_MAX_OX03C10 = 34; // vs < 35 } // namespace OX03C10::OX03C10() { + image_sensor = cereal::FrameData::ImageSensor::OX03C10; data_word = false; frame_width = FRAME_WIDTH; frame_height = FRAME_HEIGHT; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 1e45778f20..06d3a43a21 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -23,6 +23,7 @@ public: virtual int getSlaveAddress(int port) const { assert(0); } virtual void processRegisters(CameraState *c, cereal::FrameData::Builder &framed) const {} + cereal::FrameData::ImageSensor image_sensor = cereal::FrameData::ImageSensor::UNKNOWN; uint32_t frame_width, frame_height; uint32_t frame_stride; uint32_t frame_offset = 0; From bb9dda976405d62ceb9e366011dbf57c665bfc42 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 8 Dec 2023 16:31:30 -0800 Subject: [PATCH 80/87] simulator: set valid flags (#30656) sim set valid --- tools/sim/lib/camerad.py | 2 +- tools/sim/lib/simulated_sensors.py | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tools/sim/lib/camerad.py b/tools/sim/lib/camerad.py index 4621347787..cc6f0cd664 100644 --- a/tools/sim/lib/camerad.py +++ b/tools/sim/lib/camerad.py @@ -59,7 +59,7 @@ class Camerad: eof = int(frame_id * 0.05 * 1e9) self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) - dat = messaging.new_message(pub_type) + dat = messaging.new_message(pub_type, valid=True) msg = { "frameId": frame_id, "transform": [1.0, 0.0, 0.0, diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index 4520ed35ae..764f99f4d5 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -23,7 +23,7 @@ class SimulatedSensors: def send_imu_message(self, simulator_state: 'SimulatorState'): for _ in range(5): - dat = messaging.new_message('accelerometer') + dat = messaging.new_message('accelerometer', valid=True) dat.accelerometer.sensor = 4 dat.accelerometer.type = 0x10 dat.accelerometer.timestamp = dat.logMonoTime # TODO: use the IMU timestamp @@ -32,7 +32,7 @@ class SimulatedSensors: self.pm.send('accelerometer', dat) # copied these numbers from locationd - dat = messaging.new_message('gyroscope') + dat = messaging.new_message('gyroscope', valid=True) dat.gyroscope.sensor = 5 dat.gyroscope.type = 0x10 dat.gyroscope.timestamp = dat.logMonoTime # TODO: use the IMU timestamp @@ -53,7 +53,7 @@ class SimulatedSensors: ] for _ in range(10): - dat = messaging.new_message('gpsLocationExternal') + dat = messaging.new_message('gpsLocationExternal', valid=True) dat.gpsLocationExternal = { "unixTimestampMillis": int(time.time() * 1000), "flags": 1, # valid fix @@ -94,7 +94,7 @@ class SimulatedSensors: self.pm.send('driverStateV2', dat) # dmonitoringd output - dat = messaging.new_message('driverMonitoringState') + dat = messaging.new_message('driverMonitoringState', valid=True) dat.driverMonitoringState = { "faceDetected": True, "isDistracted": False, From 371c1366d435140a1507b6f2bfdd57820515a373 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Dec 2023 16:34:23 -0800 Subject: [PATCH 81/87] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 4fb8352484..a3a6e4969e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 4fb8352484c4ef074bee775975c0d1d1e8f57a78 +Subproject commit a3a6e4969e58875f7cdfc9e6cc6b1af3ee2392b5 From e909f634f5b30ab2db5626e88a8637a0eda807ba Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 8 Dec 2023 16:44:07 -0800 Subject: [PATCH 82/87] Bump submodules (#30655) bump submodules Co-authored-by: sshane --- teleoprtc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/teleoprtc_repo b/teleoprtc_repo index 8ec4778685..4df237c512 160000 --- a/teleoprtc_repo +++ b/teleoprtc_repo @@ -1 +1 @@ -Subproject commit 8ec477868591eed9a6136a44f16428bc0468b4e9 +Subproject commit 4df237c512622d3914a73f4fc15a3fc89b7f5a97 From bc70c94f75f64c31998b9796f20ec6e310cae347 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 8 Dec 2023 17:59:31 -0800 Subject: [PATCH 83/87] soundd/micd: more robust setup procedure (#30640) * just use the default device :/ * Ruff * this seems like the most reliable way * Ruff * move comments too * wait for devices * fix that * and that * ruff * add logging for avaliable devices --------- Co-authored-by: Comma Device --- selfdrive/ui/soundd.py | 15 ++++++--------- system/micd.py | 19 ++++++++++++++++++- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index d64fb0010c..9d6d24a594 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -10,9 +10,9 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.filter_simple import FirstOrderFilter from openpilot.system import micd +from openpilot.system.hardware import TICI from openpilot.common.realtime import Ratekeeper -from openpilot.system.hardware import PC from openpilot.common.swaglog import cloudlog SAMPLE_RATE = 48000 @@ -130,16 +130,13 @@ class Soundd: # sounddevice must be imported after forking processes import sounddevice as sd - rk = Ratekeeper(20) + if TICI: + micd.wait_for_devices(sd) # wait for alsa to be initialized on device - sm = messaging.SubMaster(['controlsState', 'microphone']) + with sd.OutputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + rk = Ratekeeper(20) + sm = messaging.SubMaster(['controlsState', 'microphone']) - if PC: - device = None - else: - device = "sdm845-tavil-snd-card: - (hw:0,0)" - - with sd.OutputStream(device=device, channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"soundd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") while True: sm.update(0) diff --git a/system/micd.py b/system/micd.py index 88fb08d684..f4085becd4 100755 --- a/system/micd.py +++ b/system/micd.py @@ -3,7 +3,9 @@ import numpy as np from cereal import messaging from openpilot.common.realtime import Ratekeeper +from openpilot.common.retry import retry from openpilot.common.swaglog import cloudlog +from openpilot.system.hardware import TICI RATE = 10 FFT_SAMPLES = 4096 @@ -11,6 +13,18 @@ REFERENCE_SPL = 2e-5 # newtons/m^2 SAMPLE_RATE = 44100 +@retry(attempts=7, delay=3) +def wait_for_devices(sd): + # reload sounddevice to reinitialize portaudio + sd._terminate() + sd._initialize() + + devices = sd.query_devices() + cloudlog.info(f"sounddevice available devices: {list(devices)}") + + assert len(devices) > 0 + + def calculate_spl(measurements): # https://www.engineeringtoolbox.com/sound-pressure-d_711.html sound_pressure = np.sqrt(np.mean(measurements ** 2)) # RMS of amplitudes @@ -81,7 +95,10 @@ class Mic: # sounddevice must be imported after forking processes import sounddevice as sd - with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: + if TICI: + wait_for_devices(sd) # wait for alsa to be initialized on device + + with sd.InputStream(channels=1, samplerate=SAMPLE_RATE, callback=self.callback) as stream: cloudlog.info(f"micd stream started: {stream.samplerate=} {stream.channels=} {stream.dtype=} {stream.device=}") while True: self.update() From 17d71d2829c8f61b68a6b05b4a5796662185ab7f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 8 Dec 2023 23:40:15 -0600 Subject: [PATCH 84/87] test_athena: remove no-op line (#30660) * test * Update selfdrive/athena/tests/test_athenad.py --- selfdrive/athena/tests/test_athenad.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 8829ebfbf1..beffa0d232 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -46,8 +46,6 @@ class TestAthenadMethods(unittest.TestCase): else: os.unlink(p) - dispatcher["listUploadQueue"]() # ensure queue is empty at start - # *** test helpers *** @staticmethod From e78b80c8fe4f1cf6e6350c9f613ba8c9a43e9d57 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 9 Dec 2023 00:15:27 -0600 Subject: [PATCH 85/87] jenkins: run all car tests with one command (#30661) * run all jenkins car tests in one script, less down time? * fix * speed up * u --- Jenkinsfile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 840dd29503..bd894f53e0 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -234,9 +234,8 @@ node { 'car tests': { pcStage("car tests") { sh label: "build", script: "selfdrive/manager/build.py" - sh label: "test_models.py", script: "INTERNAL_SEG_CNT=250 INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt FILEREADER_CACHE=1 \ - pytest selfdrive/car/tests/test_models.py" - sh label: "test_car_interfaces.py", script: "MAX_EXAMPLES=100 pytest selfdrive/car/tests/test_car_interfaces.py" + sh label: "run car tests", script: "cd selfdrive/car/tests && MAX_EXAMPLES=100 INTERNAL_SEG_CNT=250 FILEREADER_CACHE=1 \ + INTERNAL_SEG_LIST=selfdrive/car/tests/test_models_segs.txt pytest test_models.py test_car_interfaces.py" } }, From 90c2aee6c36d6f37bc580804d98613e9918a9e33 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 9 Dec 2023 06:53:15 -0600 Subject: [PATCH 86/87] athenad: use `socket` constant (#30663) Update athenad.py --- selfdrive/athena/athenad.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 936797a89e..bdca04c457 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -41,9 +41,6 @@ from openpilot.system.version import get_commit, get_origin, get_short_branch, g from openpilot.system.hardware.hw import Paths -# TODO: use socket constant when mypy recognizes this as a valid attribute -TCP_USER_TIMEOUT = 18 - ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) LOCAL_PORT_WHITELIST = {8022} @@ -760,7 +757,7 @@ def ws_manage(ws: WebSocket, end_event: threading.Event) -> None: if onroad != onroad_prev: onroad_prev = onroad - sock.setsockopt(socket.IPPROTO_TCP, TCP_USER_TIMEOUT, 16000 if onroad else 0) + sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_USER_TIMEOUT, 16000 if onroad else 0) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPIDLE, 7 if onroad else 30) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPINTVL, 7 if onroad else 10) sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_KEEPCNT, 2 if onroad else 3) From 2afff9a0cba605d5118c6cb33c157017a3999957 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 9 Dec 2023 07:06:30 -0600 Subject: [PATCH 87/87] athena tests: patch instead of juggling function (#30664) patch instead of juggling function --- selfdrive/athena/tests/test_athenad_ping.py | 25 +++++++-------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/selfdrive/athena/tests/test_athenad_ping.py b/selfdrive/athena/tests/test_athenad_ping.py index 2958ec2262..5231b0475f 100755 --- a/selfdrive/athena/tests/test_athenad_ping.py +++ b/selfdrive/athena/tests/test_athenad_ping.py @@ -3,8 +3,8 @@ import subprocess import threading import time import unittest -from typing import Callable, cast, Optional -from unittest.mock import MagicMock +from typing import cast, Optional +from unittest import mock from openpilot.common.params import Params from openpilot.common.timeout import Timeout @@ -27,8 +27,6 @@ class TestAthenadPing(unittest.TestCase): athenad: threading.Thread exit_event: threading.Event - _create_connection: Callable - def _get_ping_time(self) -> Optional[str]: return cast(Optional[str], self.params.get("LastAthenaPingTime", encoding="utf-8")) @@ -38,15 +36,9 @@ class TestAthenadPing(unittest.TestCase): def _received_ping(self) -> bool: return self._get_ping_time() is not None - @classmethod - def setUpClass(cls) -> None: - cls._create_connection = athenad.create_connection - athenad.create_connection = MagicMock(wraps=cls._create_connection) - @classmethod def tearDownClass(cls) -> None: wifi_radio(True) - athenad.create_connection = cls._create_connection def setUp(self) -> None: self.params = Params() @@ -58,19 +50,18 @@ class TestAthenadPing(unittest.TestCase): self.exit_event = threading.Event() self.athenad = threading.Thread(target=athenad.main, args=(self.exit_event,)) - athenad.create_connection.reset_mock() - def tearDown(self) -> None: if self.athenad.is_alive(): self.exit_event.set() self.athenad.join() - def assertTimeout(self, reconnect_time: float) -> None: + @mock.patch('openpilot.selfdrive.athena.athenad.create_connection', autospec=True) + def assertTimeout(self, reconnect_time: float, mock_create_connection: mock.MagicMock) -> None: self.athenad.start() time.sleep(1) - athenad.create_connection.assert_called_once() - athenad.create_connection.reset_mock() + mock_create_connection.assert_called_once() + mock_create_connection.reset_mock() # check normal behaviour with self.subTest("Wi-Fi: receives ping"), Timeout(70, "no ping received"): @@ -78,7 +69,7 @@ class TestAthenadPing(unittest.TestCase): time.sleep(0.1) print("ping received") - athenad.create_connection.assert_not_called() + mock_create_connection.assert_not_called() # websocket should attempt reconnect after short time with self.subTest("LTE: attempt reconnect"): @@ -86,7 +77,7 @@ class TestAthenadPing(unittest.TestCase): print("waiting for reconnect attempt") start_time = time.monotonic() with Timeout(reconnect_time, "no reconnect attempt"): - while not athenad.create_connection.called: + while not mock_create_connection.called: time.sleep(0.1) print(f"reconnect attempt after {time.monotonic() - start_time:.2f}s")