From 11b9e057ae3529023aa67537394fce05681e2d4a Mon Sep 17 00:00:00 2001 From: Martin Lillepuu Date: Sun, 4 Dec 2022 22:12:16 +0000 Subject: [PATCH 01/45] Add support for Subaru Forester 2022 --- .gitmodules | 3 ++- docs/CARS.md | 3 ++- panda | 2 +- release/files_common | 1 + selfdrive/car/docs_definitions.py | 1 + selfdrive/car/subaru/carcontroller.py | 4 +++- selfdrive/car/subaru/carstate.py | 10 +++++++--- selfdrive/car/subaru/interface.py | 4 +++- selfdrive/car/subaru/subarucan.py | 8 ++++++++ selfdrive/car/subaru/values.py | 24 +++++++++++++++++++++++ selfdrive/car/torque_data/substitute.yaml | 1 + 11 files changed, 53 insertions(+), 8 deletions(-) diff --git a/.gitmodules b/.gitmodules index 26f93ef164e782..c735238ec3247d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,7 @@ [submodule "panda"] path = panda - url = ../../commaai/panda.git + url = ../../martinl/panda.git + branch = forester-2022-PR [submodule "opendbc"] path = opendbc url = ../../commaai/opendbc.git diff --git a/docs/CARS.md b/docs/CARS.md index 580e2ad4617d13..802f6d51e5961f 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 three. All supported cars provide a better experience than any stock system. -# 217 Supported Cars +# 218 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Harness| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:| @@ -141,6 +141,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Forester 2019-21|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| +|Subaru|Forester 2022|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru C| |Subaru|Impreza 2017-19|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Impreza 2020-22|EyeSight Driver Assistance|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru A| |Subaru|Legacy 2020-22|All|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|Subaru B| diff --git a/panda b/panda index 4edd1a602131ec..3d201f5eb8df9c 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4edd1a602131ec2f09a604a4bd28e7d00e334458 +Subproject commit 3d201f5eb8df9ce69ad0d10ae0804730ba52db92 diff --git a/release/files_common b/release/files_common index 297a7a808e8752..254b62ac672d65 100644 --- a/release/files_common +++ b/release/files_common @@ -560,6 +560,7 @@ opendbc/nissan_x_trail_2017.dbc opendbc/nissan_leaf_2018.dbc opendbc/subaru_global_2017_generated.dbc +opendbc/subaru_global_2022_generated.dbc opendbc/subaru_outback_2015_generated.dbc opendbc/subaru_outback_2019_generated.dbc opendbc/subaru_forester_2017_generated.dbc diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 7cf44514d622b3..b012ad0b779075 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -36,6 +36,7 @@ class Harness(Enum): toyota = "Toyota" subaru_a = "Subaru A" subaru_b = "Subaru B" + subaru_c = "Subaru C" fca = "FCA" ram = "Ram" vw = "VW" diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index b5429daef221d3..10b2de02b4a42a 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams class CarController: @@ -41,6 +41,8 @@ def update(self, CC, CS): if self.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer)) + elif self.CP.carFingerprint == CAR.FORESTER_2022: + can_sends.append(subarucan.create_steering_control_2(self.packer, apply_steer)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer)) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index ba873c48d784c8..90c40c11d74b15 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -53,9 +53,13 @@ def update(self, cp, cp_cam, cp_body): steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold - cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp - ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 - ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 + if self.car_fingerprint == CAR.FORESTER_2022: + ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 + ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 + else: + cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp + ret.cruiseState.enabled = cp_cruise.vl["CruiseControl"]["Cruise_Activated"] != 0 + ret.cruiseState.available = cp_cruise.vl["CruiseControl"]["Cruise_On"] != 0 ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]["Cruise_Set_Speed"] * CV.KPH_TO_MS if (self.car_fingerprint in PREGLOBAL_CARS and cp.vl["Dash_State2"]["UNITS"] == 1) or \ diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 22468801eca7f9..7c002b0395ac9f 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -23,6 +23,8 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] if candidate in GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 + elif candidate == CAR.FORESTER_2022: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_FORESTER_2022 ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 @@ -60,7 +62,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] - elif candidate == CAR.FORESTER: + elif candidate in (CAR.FORESTER, CAR.FORESTER_2022): ret.mass = 1568. + STD_CARGO_KG ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.5 diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index d83b639a41fb0a..069b6001fa8629 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -11,6 +11,14 @@ def create_steering_control(packer, apply_steer): } return packer.make_can_msg("ES_LKAS", 0, values) +def create_steering_control_2(packer, apply_steer): + values = { + "LKAS_Output": apply_steer, + "LKAS_Request": 1 if apply_steer != 0 else 0, + "SET_3": 3 + } + return packer.make_can_msg("ES_LKAS_2", 0, values) + def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 6ac2637fa2d5cd..a2fa4ce741bdd3 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -36,6 +36,7 @@ class CAR: IMPREZA = "SUBARU IMPREZA LIMITED 2019" IMPREZA_2020 = "SUBARU IMPREZA SPORT 2020" FORESTER = "SUBARU FORESTER 2019" + FORESTER_2022 = "SUBARU FORESTER 2022" OUTBACK = "SUBARU OUTBACK 6TH GEN" LEGACY = "SUBARU LEGACY 7TH GEN" @@ -67,6 +68,7 @@ class SubaruCarInfo(CarInfo): SubaruCarInfo("Subaru XV 2020-21"), ], CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), + CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", harness=Harness.subaru_c), CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), @@ -299,6 +301,27 @@ class SubaruCarInfo(CarInfo): b'\x1a\xf6b0\x00', ], }, + CAR.FORESTER_2022: { + (Ecu.abs, 0x7b0, None): [ + b'\xa3 !x\x00', + b'\xa3 !v\x00', + ], + (Ecu.eps, 0x746, None): [ + b'-\xc0%0', + b'-\xc0\x040', + ], + (Ecu.fwdCamera, 0x787, None): [ + b'\x04!\x01\x1eD\x07!\x00\x04,' + ], + (Ecu.engine, 0x7e0, None): [ + b'\xd5"a0\x07', + b'\xd5"`0\x07', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\x1d\x86B0\x00', + b'\x1d\xf6B0\x00', + ], + }, CAR.FORESTER_PREGLOBAL: { (Ecu.abs, 0x7b0, None): [ b'\x7d\x97\x14\x40', @@ -505,6 +528,7 @@ class SubaruCarInfo(CarInfo): CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), + CAR.FORESTER_2022: dbc_dict('subaru_global_2022_generated', None), CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), diff --git a/selfdrive/car/torque_data/substitute.yaml b/selfdrive/car/torque_data/substitute.yaml index df696bc0fcc33f..441ea79bcd2db9 100644 --- a/selfdrive/car/torque_data/substitute.yaml +++ b/selfdrive/car/torque_data/substitute.yaml @@ -69,6 +69,7 @@ SEAT LEON 3RD GEN: VOLKSWAGEN GOLF 7TH GEN SEAT ATECA 1ST GEN: VOLKSWAGEN GOLF 7TH GEN SUBARU LEGACY 7TH GEN: SUBARU OUTBACK 6TH GEN +SUBARU FORESTER 2022: SUBARU FORESTER 2019 # Old subarus don't have much data guessing it's like low torque impreza SUBARU OUTBACK 2018 - 2019: SUBARU IMPREZA LIMITED 2019 From 7923ce98ab9cd88a442f7bed4cf002dd9f22ac40 Mon Sep 17 00:00:00 2001 From: Martin Lillepuu Date: Sun, 11 Dec 2022 15:02:45 +0000 Subject: [PATCH 02/45] Use ES_Status Cruise_Activated --- selfdrive/car/subaru/carstate.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 90c40c11d74b15..354525ac8db175 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -54,7 +54,7 @@ def update(self, cp, cp_cam, cp_body): ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold if self.car_fingerprint == CAR.FORESTER_2022: - ret.cruiseState.enabled = cp_cam.vl["ES_DashStatus"]['Cruise_Activated'] != 0 + ret.cruiseState.enabled = cp_cam.vl["ES_Status"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: cp_cruise = cp_body if self.car_fingerprint in GLOBAL_GEN2 else cp @@ -109,7 +109,7 @@ def get_common_global_signals(): return signals, checks @staticmethod - def get_global_es_distance_signals(): + def get_common_global_es_signals(): signals = [ ("COUNTER", "ES_Distance"), ("Signal1", "ES_Distance"), @@ -130,9 +130,12 @@ def get_global_es_distance_signals(): ("Cruise_Set", "ES_Distance"), ("Cruise_Resume", "ES_Distance"), ("Signal6", "ES_Distance"), + + ("Cruise_Activated", "ES_Status"), ] checks = [ ("ES_Distance", 20), + ("ES_Status", 20), ] return signals, checks @@ -302,8 +305,8 @@ def get_cam_can_parser(CP): ] if CP.carFingerprint not in GLOBAL_GEN2: - signals += CarState.get_global_es_distance_signals()[0] - checks += CarState.get_global_es_distance_signals()[1] + signals += CarState.get_common_global_es_signals()[0] + checks += CarState.get_common_global_es_signals()[1] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) @@ -311,8 +314,8 @@ def get_cam_can_parser(CP): def get_body_can_parser(CP): if CP.carFingerprint in GLOBAL_GEN2: signals, checks = CarState.get_common_global_signals() - signals += CarState.get_global_es_distance_signals()[0] - checks += CarState.get_global_es_distance_signals()[1] + signals += CarState.get_common_global_es_signals()[0] + checks += CarState.get_common_global_es_signals()[1] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) return None From c742ed5f4eafe526cd9ce83a7d16d8fcd2677b88 Mon Sep 17 00:00:00 2001 From: Martin Lillepuu Date: Sun, 11 Dec 2022 15:03:59 +0000 Subject: [PATCH 03/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 3d201f5eb8df9c..b3f22eaac49560 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 3d201f5eb8df9ce69ad0d10ae0804730ba52db92 +Subproject commit b3f22eaac495600322546ea71364c0c807b4ea30 From ca826ea74e31faa828e034bc9a6d244ad3d59ec7 Mon Sep 17 00:00:00 2001 From: Martin Lillepuu Date: Wed, 5 Jul 2023 09:58:17 +0300 Subject: [PATCH 04/45] Update subaru_c harness syntax --- selfdrive/car/subaru/values.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index c9dd23797cb137..a14d0daedd3c18 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -78,7 +78,7 @@ class SubaruCarInfo(CarInfo): SubaruCarInfo("Subaru XV 2020-21"), ], CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", "All"), - CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", harness=Harness.subaru_c), + CAR.FORESTER_2022: SubaruCarInfo("Subaru Forester 2022", car_parts=CarParts.common([CarHarness.subaru_c])), CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), From e126d4ccfa0b25a1dbdf0b3a8a0234bf1a9125bd Mon Sep 17 00:00:00 2001 From: Martin Lillepuu Date: Wed, 5 Jul 2023 10:33:37 +0300 Subject: [PATCH 05/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index b3f22eaac49560..cc209c0d7842d5 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit b3f22eaac495600322546ea71364c0c807b4ea30 +Subproject commit cc209c0d7842d5423f3a9ebd8982094d88e315dc From 1233ea5997368fb84bc79eb5c8899ff34a224e75 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 11 Jul 2023 10:05:07 -0700 Subject: [PATCH 06/45] bump submodules + add test route --- opendbc | 2 +- panda | 2 +- selfdrive/car/tests/routes.py | 2 ++ 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/opendbc b/opendbc index 008104f9400b61..8207262904810f 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 008104f9400b6161461db40d66ce13b34ea9ce79 +Subproject commit 8207262904810f0f99ec78bbd74939eafc71fb76 diff --git a/panda b/panda index cc209c0d7842d5..0d413d387dcb48 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit cc209c0d7842d5423f3a9ebd8982094d88e315dc +Subproject commit 0d413d387dcb48622024a9e83df7b57bbdd5934b diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 275907f06eb474..80d728fee210c1 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -230,6 +230,8 @@ CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020), CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=10), CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3), + + CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.FORESTER_2022, segment=4), # Pre-global, dashcam CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.FORESTER_PREGLOBAL), CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.LEGACY_PREGLOBAL), From b1a4ab8b76f5f8d4c479ed27dc504bd41e1b81f6 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 11 Jul 2023 10:54:27 -0700 Subject: [PATCH 07/45] add to releases --- RELEASES.md | 1 + selfdrive/car/tests/routes.py | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/RELEASES.md b/RELEASES.md index 3754d5336e2afd..15b8c881990f5d 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -10,6 +10,7 @@ Version 0.9.4 (2023-XX-XX) * Border color always shows engagement status. Blue means disengaged, green means engaged, and grey means engaged with human overriding * Alerts are shown inside the border. Black/grey means info, orange means warning, and red means critical alert * Ford Focus 2018 support +* Subaru Forester 2022 support thanks to martinl! Version 0.9.3 (2023-06-29) ======================== diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 80d728fee210c1..6fc03f500bd3f4 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -230,8 +230,8 @@ CarTestRoute("8bf7e79a3ce64055|2021-05-24--09-36-27", SUBARU.IMPREZA_2020), CarTestRoute("1bbe6bf2d62f58a8|2022-07-14--17-11-43", SUBARU.OUTBACK, segment=10), CarTestRoute("c56e69bbc74b8fad|2022-08-18--09-43-51", SUBARU.LEGACY, segment=3), - CarTestRoute("7fd1e4f3a33c1673|2022-12-04--15-09-53", SUBARU.FORESTER_2022, segment=4), + # Pre-global, dashcam CarTestRoute("95441c38ae8c130e|2020-06-08--12-10-17", SUBARU.FORESTER_PREGLOBAL), CarTestRoute("df5ca7660000fba8|2020-06-16--17-37-19", SUBARU.LEGACY_PREGLOBAL), From 9330a9e99d2e5ba98c39a9d1d5abf70b8e6c4bcb Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 12 Jul 2023 17:19:27 -0700 Subject: [PATCH 08/45] wip --- opendbc | 2 +- panda | 2 +- release/files_common | 1 - selfdrive/car/subaru/subarucan.py | 2 +- selfdrive/car/subaru/values.py | 2 +- 5 files changed, 4 insertions(+), 5 deletions(-) diff --git a/opendbc b/opendbc index 8207262904810f..236359cf63c3ca 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 8207262904810f0f99ec78bbd74939eafc71fb76 +Subproject commit 236359cf63c3caaf8e02b972c452aabac416662a diff --git a/panda b/panda index 0d413d387dcb48..e002987d58ea4f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 0d413d387dcb48622024a9e83df7b57bbdd5934b +Subproject commit e002987d58ea4f5fa2e7ef6af55f3ad4692900f0 diff --git a/release/files_common b/release/files_common index b4a93af4799e80..1c4955b96f10be 100644 --- a/release/files_common +++ b/release/files_common @@ -571,7 +571,6 @@ opendbc/nissan_x_trail_2017_generated.dbc opendbc/nissan_leaf_2018_generated.dbc opendbc/subaru_global_2017_generated.dbc -opendbc/subaru_global_2022_generated.dbc opendbc/subaru_outback_2015_generated.dbc opendbc/subaru_outback_2019_generated.dbc opendbc/subaru_forester_2017_generated.dbc diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 9843da0de92a74..eafc53d93b71c0 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -18,7 +18,7 @@ def create_steering_control_2(packer, apply_steer, steer_req): "LKAS_Request": steer_req, "SET_3": 3 } - return packer.make_can_msg("ES_LKAS_2", 0, values) + return packer.make_can_msg("ES_LKAS_ALT", 0, values) def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index a14d0daedd3c18..de3631698543c5 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -559,7 +559,7 @@ class SubaruCarInfo(CarInfo): CAR.IMPREZA: dbc_dict('subaru_global_2017_generated', None), CAR.IMPREZA_2020: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER: dbc_dict('subaru_global_2017_generated', None), - CAR.FORESTER_2022: dbc_dict('subaru_global_2022_generated', None), + CAR.FORESTER_2022: dbc_dict('subaru_global_2017_generated', None), CAR.OUTBACK: dbc_dict('subaru_global_2017_generated', None), CAR.LEGACY: dbc_dict('subaru_global_2017_generated', None), CAR.FORESTER_PREGLOBAL: dbc_dict('subaru_forester_2017_generated', None), From a0f76a344c12c9baf6ad3e22caedb1e92f12322d Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 12 Jul 2023 19:25:54 -0700 Subject: [PATCH 09/45] bump refs --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index e002987d58ea4f..86891433aedc53 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e002987d58ea4f5fa2e7ef6af55f3ad4692900f0 +Subproject commit 86891433aedc5379ec748cd2900dc1df5daa1ef0 From 579dd3d99eb1949fa6291678124a1076247c6b27 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Wed, 12 Jul 2023 19:37:03 -0700 Subject: [PATCH 10/45] bump submodules --- cereal | 2 +- laika_repo | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cereal b/cereal index 089f96117a5137..a2f1f0cb8dd45e 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 089f96117a513711755d34d7f919ac481f23a0fc +Subproject commit a2f1f0cb8dd45ea4265255855da7de8fd89156ed diff --git a/laika_repo b/laika_repo index 400c71d9313260..3b3d41a885d16f 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 400c71d9313260b137783ecf1fac37ffa18d7f60 +Subproject commit 3b3d41a885d16f4c3739a14ef06bebca858b6e3a From 555aa6e072d03b8654c7ab74c4c9d382fcb25221 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 13 Jul 2023 09:02:54 -0700 Subject: [PATCH 11/45] fix unittest --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 86891433aedc53..ffbb46e816a825 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 86891433aedc5379ec748cd2900dc1df5daa1ef0 +Subproject commit ffbb46e816a8258e0605016abbd631402888f763 From 8b4ac8be2cf751e03d39a4fa9110bde1ac9e0baa Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 14 Jul 2023 13:51:46 -0700 Subject: [PATCH 12/45] bump submodules and group forester into global_gen3 --- opendbc | 2 +- panda | 2 +- selfdrive/car/subaru/carstate.py | 5 ++--- selfdrive/car/subaru/interface.py | 6 +++--- selfdrive/car/subaru/values.py | 1 + 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/opendbc b/opendbc index 236359cf63c3ca..b03468a714da2e 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 236359cf63c3caaf8e02b972c452aabac416662a +Subproject commit b03468a714da2eb8ef83f07a373f3f1514491cad diff --git a/panda b/panda index ffbb46e816a825..e70afd752d0090 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ffbb46e816a8258e0605016abbd631402888f763 +Subproject commit e70afd752d0090e5b3648d120004a5fdb8e6244a diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 1a776cd82fc253..0622308e77b3d8 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, SubaruFlags +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, CanBus, SubaruFlags class CarState(CarStateBase): @@ -53,7 +53,7 @@ def update(self, cp, cp_cam, cp_body): steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold - if self.car_fingerprint == CAR.FORESTER_2022: + if self.car_fingerprint in GLOBAL_GEN3: ret.cruiseState.enabled = cp_cam.vl["ES_Status"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: @@ -139,7 +139,6 @@ def get_common_global_es_signals(): ("Cruise_Set", "ES_Distance"), ("Cruise_Resume", "ES_Distance"), ("Signal6", "ES_Distance"), - ("Cruise_Activated", "ES_Status"), ] diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index c3c8d7d5caa84a..550ad9c535bf7d 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,7 +3,7 @@ from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags +from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @@ -27,8 +27,8 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] if candidate in GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 - elif candidate == CAR.FORESTER_2022: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_FORESTER_2022 + elif candidate in GLOBAL_GEN3: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN3 ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 0b83793f9ac8f1..9343b8118eeaa0 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -575,5 +575,6 @@ class SubaruCarInfo(CarInfo): CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), } +GLOBAL_GEN3 = (CAR.FORESTER_2022) GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY) PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018) From b66c77307fe82f2aebdf0972aa5477b79d113d49 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 14 Jul 2023 14:00:13 -0700 Subject: [PATCH 13/45] more cleanup --- selfdrive/car/subaru/carcontroller.py | 6 +++--- selfdrive/car/subaru/subarucan.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index fa971339450412..8256ac9196d512 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags +from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags class CarController: @@ -37,8 +37,8 @@ def update(self, CC, CS, now_nanos): if self.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) - elif self.CP.carFingerprint == CAR.FORESTER_2022: - can_sends.append(subarucan.create_steering_control_2(self.packer, apply_steer, CC.latActive)) + elif self.CP.carFingerprint in GLOBAL_GEN3: + can_sends.append(subarucan.create_steering_control_alt(self.packer, apply_steer, CC.latActive)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index eafc53d93b71c0..086310d5f3ed30 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -12,7 +12,7 @@ def create_steering_control(packer, apply_steer, steer_req): } return packer.make_can_msg("ES_LKAS", 0, values) -def create_steering_control_2(packer, apply_steer, steer_req): +def create_steering_control_alt(packer, apply_steer, steer_req): values = { "LKAS_Output": apply_steer, "LKAS_Request": steer_req, From 1270e75c8ed7381797d80f07c622d908b8093876 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Sun, 16 Jul 2023 11:02:53 -0700 Subject: [PATCH 14/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index e70afd752d0090..735dc70b20204d 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e70afd752d0090e5b3648d120004a5fdb8e6244a +Subproject commit 735dc70b20204dae8a53e6da6bb5affea8a73adf From a2f37629f4ae41627e8915ecf712f3af02e17ed0 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 17 Jul 2023 12:53:42 -0700 Subject: [PATCH 15/45] bump submodules --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index a2f1f0cb8dd45e..cf9c5cbb9196f8 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit a2f1f0cb8dd45ea4265255855da7de8fd89156ed +Subproject commit cf9c5cbb9196f80a25bd8421a9921ce4801a7561 From 82a10f68ee2216a929a32cb6882a30a7d8d3a720 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 17 Jul 2023 12:56:35 -0700 Subject: [PATCH 16/45] forester fingerprints --- selfdrive/car/subaru/values.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 9343b8118eeaa0..827b7e8d0661fa 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -337,10 +337,14 @@ class SubaruCarInfo(CarInfo): (Ecu.abs, 0x7b0, None): [ b'\xa3 !x\x00', b'\xa3 !v\x00', + b'\xa3 "v\x00', + b'\xa3 "x\x00', ], (Ecu.eps, 0x746, None): [ b'-\xc0%0', b'-\xc0\x040', + b'=\xc0%\x02', + b'=\xc04\x02', ], (Ecu.fwdCamera, 0x787, None): [ b'\x04!\x01\x1eD\x07!\x00\x04,' @@ -348,10 +352,14 @@ class SubaruCarInfo(CarInfo): (Ecu.engine, 0x7e0, None): [ b'\xd5"a0\x07', b'\xd5"`0\x07', + b'\xf1"aq\x07', + b'\xf1"`q\x07', ], (Ecu.transmission, 0x7e1, None): [ b'\x1d\x86B0\x00', b'\x1d\xf6B0\x00', + b'\x1e\x86B0\x00', + b'\x1e\xf6D0\x00', ], }, CAR.FORESTER_PREGLOBAL: { From 4e9cac8bb80ea564cb1be280dbd9d97d8d58e542 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 17 Jul 2023 13:02:13 -0700 Subject: [PATCH 17/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 735dc70b20204d..e32e39c89ceaef 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 735dc70b20204dae8a53e6da6bb5affea8a73adf +Subproject commit e32e39c89ceaef1b5adaa5542375b7f00fcd9ddf From 766a116821e0d1b19cfeba968b28e37df5d0c3df Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Mon, 17 Jul 2023 13:23:22 -0700 Subject: [PATCH 18/45] update docs --- docs/CARS.md | 3 ++- panda | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/CARS.md b/docs/CARS.md index a6148bb78135bf..fa94e8398d5c9f 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 three. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified. -# 255 Supported Cars +# 256 Supported Cars |Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|Hardware Needed
 |Video| |---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:| @@ -171,6 +171,7 @@ A supported vehicle is one that just works when you install a comma three. All s |Subaru|Crosstrek 2018-19|EyeSight Driver Assistance[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru A connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Subaru|Crosstrek 2020-23|EyeSight Driver Assistance[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru A connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Subaru|Forester 2019-21|All[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru A connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| +|Subaru|Forester 2022|EyeSight Driver Assistance[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru C connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Subaru|Impreza 2017-19|EyeSight Driver Assistance[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-empty.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru A connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Subaru|Impreza 2020-22|EyeSight Driver Assistance[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru A connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| |Subaru|Legacy 2020-22|All[7](#footnotes)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|
View- 1 RJ45 cable (7 ft)
- 1 Subaru B connector
- 1 comma power v2
- 1 comma three
- 1 harness box
- 1 mount
- 1 right angle OBD-C cable (1.5 ft)
Buy Here
|| diff --git a/panda b/panda index e32e39c89ceaef..cfa795e0852d06 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e32e39c89ceaef1b5adaa5542375b7f00fcd9ddf +Subproject commit cfa795e0852d0649852ba4c08d1ed154b51e7eeb From 5a41c814762a36f6082366e612c0a514ce1b35ac Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 03:15:22 +0000 Subject: [PATCH 19/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index cfa795e0852d06..e002987d58ea4f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit cfa795e0852d0649852ba4c08d1ed154b51e7eeb +Subproject commit e002987d58ea4f5fa2e7ef6af55f3ad4692900f0 From 4d89708ae4af2088fa62ae8bb2aa7b9fd37c8822 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 03:22:50 +0000 Subject: [PATCH 20/45] bump submodules --- opendbc | 2 +- panda | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc b/opendbc index b03468a714da2e..54051f341fd66a 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b03468a714da2eb8ef83f07a373f3f1514491cad +Subproject commit 54051f341fd66a16625f29ae1bf60176824935f0 diff --git a/panda b/panda index e002987d58ea4f..9041410ed550a1 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e002987d58ea4f5fa2e7ef6af55f3ad4692900f0 +Subproject commit 9041410ed550a1ddc441cf49f30465e9655e3e0c From ac0211e90a68b18b98a17b7f93294014a3eff81a Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 03:24:45 +0000 Subject: [PATCH 21/45] bump submodules --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 9041410ed550a1..baaf7408658fe3 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 9041410ed550a1ddc441cf49f30465e9655e3e0c +Subproject commit baaf7408658fe3946522b08f4f4f7dffef3a78c2 From d0b5081ff508c99972a48d6e361b888eaf70ac61 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 03:26:54 +0000 Subject: [PATCH 22/45] bump dbc --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index baaf7408658fe3..28bcfad9879043 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit baaf7408658fe3946522b08f4f4f7dffef3a78c2 +Subproject commit 28bcfad9879043df1e3f19988fc30d8e54b56307 From 5923c2decbdfce9fc9f56d0bc3fd5845d3ab77ea Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 10:26:28 -0700 Subject: [PATCH 23/45] split gen3 --- panda | 2 +- selfdrive/car/subaru/carcontroller.py | 4 ++-- selfdrive/car/subaru/carstate.py | 4 ++-- selfdrive/car/subaru/values.py | 4 +++- 4 files changed, 8 insertions(+), 6 deletions(-) diff --git a/panda b/panda index cfa795e0852d06..792abddf766651 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit cfa795e0852d0649852ba4c08d1ed154b51e7eeb +Subproject commit 792abddf7666510fab74c1bfb0c1ee3c96789c79 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 8256ac9196d512..4aae2476a3fc5f 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,7 +1,7 @@ from opendbc.can.packer import CANPacker from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan -from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags +from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, ALT_LKAS_MSG, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags class CarController: @@ -37,7 +37,7 @@ def update(self, CC, CS, now_nanos): if self.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) - elif self.CP.carFingerprint in GLOBAL_GEN3: + elif self.CP.carFingerprint in ALT_LKAS_MSG: can_sends.append(subarucan.create_steering_control_alt(self.packer, apply_steer, CC.latActive)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 0622308e77b3d8..a850ba7594a5e0 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -4,7 +4,7 @@ from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, CanBus, SubaruFlags +from selfdrive.car.subaru.values import DBC, CAR, GLOBAL_GEN2, ES_STATUS, PREGLOBAL_CARS, CanBus, SubaruFlags class CarState(CarStateBase): @@ -53,7 +53,7 @@ def update(self, cp, cp_cam, cp_body): steer_threshold = 75 if self.CP.carFingerprint in PREGLOBAL_CARS else 80 ret.steeringPressed = abs(ret.steeringTorque) > steer_threshold - if self.car_fingerprint in GLOBAL_GEN3: + if self.car_fingerprint in ES_STATUS: ret.cruiseState.enabled = cp_cam.vl["ES_Status"]['Cruise_Activated'] != 0 ret.cruiseState.available = cp_cam.vl["ES_DashStatus"]['Cruise_On'] != 0 else: diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 827b7e8d0661fa..f73760722263b5 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -583,6 +583,8 @@ class SubaruCarInfo(CarInfo): CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), } -GLOBAL_GEN3 = (CAR.FORESTER_2022) +ALT_LKAS_MSG = (CAR.FORESTER_2022) +ES_STATUS = (CAR.FORESTER_2022) + GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY) PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018) From 89f1df8f8f40030918a24c32202864456ffa8a2f Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 14:18:41 -0700 Subject: [PATCH 24/45] bump submodules --- opendbc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc b/opendbc index b03468a714da2e..4231b0f12d8cf1 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b03468a714da2eb8ef83f07a373f3f1514491cad +Subproject commit 4231b0f12d8cf10d0554c4eb513ac984defc1f90 From c82ee9a4dede610b476e69c544a98cd551ff9ca0 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 14:22:44 -0700 Subject: [PATCH 25/45] missing comma --- selfdrive/car/subaru/values.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index f73760722263b5..0db4bce411e650 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -583,8 +583,8 @@ class SubaruCarInfo(CarInfo): CAR.OUTBACK_PREGLOBAL_2018: dbc_dict('subaru_outback_2019_generated', None), } -ALT_LKAS_MSG = (CAR.FORESTER_2022) -ES_STATUS = (CAR.FORESTER_2022) +ALT_LKAS_MSG = (CAR.FORESTER_2022,) +ES_STATUS = (CAR.FORESTER_2022,) GLOBAL_GEN2 = (CAR.OUTBACK, CAR.LEGACY) PREGLOBAL_CARS = (CAR.FORESTER_PREGLOBAL, CAR.LEGACY_PREGLOBAL, CAR.OUTBACK_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018) From ccdb8b5c0164dbf82239724c6029dd90e2c452e0 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 14:25:45 -0700 Subject: [PATCH 26/45] fix safety parameters --- selfdrive/car/subaru/interface.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 550ad9c535bf7d..d724b7c272dfd5 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -3,7 +3,7 @@ from panda import Panda from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.subaru.values import CAR, GLOBAL_GEN2, GLOBAL_GEN3, PREGLOBAL_CARS, SubaruFlags +from selfdrive.car.subaru.values import ALT_LKAS_MSG, CAR, ES_STATUS, GLOBAL_GEN2, PREGLOBAL_CARS, SubaruFlags class CarInterface(CarInterfaceBase): @@ -27,8 +27,10 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)] if candidate in GLOBAL_GEN2: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN2 - elif candidate in GLOBAL_GEN3: - ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_GEN3 + if candidate in ALT_LKAS_MSG: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_LKAS_ALT + if candidate in ES_STATUS: + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_SUBARU_ES_STATUS ret.steerLimitTimer = 0.4 ret.steerActuatorDelay = 0.1 From 550570833391d5adf3687719ad9d761c6b4dd9a4 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Tue, 18 Jul 2023 14:30:25 -0700 Subject: [PATCH 27/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 792abddf766651..e959fc808d6186 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 792abddf7666510fab74c1bfb0c1ee3c96789c79 +Subproject commit e959fc808d61860cd34c58e24e863157224853ca From 98053a1a7c97a5a70c6f7d7b0deaf41a33c573c8 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 02:23:55 +0000 Subject: [PATCH 28/45] use old lkas message --- panda | 2 +- selfdrive/car/subaru/carcontroller.py | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/panda b/panda index e959fc808d6186..8cd09486c18d48 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e959fc808d61860cd34c58e24e863157224853ca +Subproject commit 8cd09486c18d4839d4a00a32a6c21dcc15703564 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 4aae2476a3fc5f..ccb917285f045f 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -37,10 +37,11 @@ def update(self, CC, CS, now_nanos): if self.CP.carFingerprint in PREGLOBAL_CARS: can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) - elif self.CP.carFingerprint in ALT_LKAS_MSG: - can_sends.append(subarucan.create_steering_control_alt(self.packer, apply_steer, CC.latActive)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) + + if self.CP.carFingerprint in ALT_LKAS_MSG: + can_sends.append(subarucan.create_steering_control_alt(self.packer, 0, 0)) self.apply_steer_last = apply_steer From eb5f400497a7d233fcaf9e93ab2e062b32ebcbb9 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 02:33:38 +0000 Subject: [PATCH 29/45] fix linter --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 8cd09486c18d48..db5b4072148a4a 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 8cd09486c18d4839d4a00a32a6c21dcc15703564 +Subproject commit db5b4072148a4a769737a318c5a2375c052bb50e From ee812c1103eb0f6366047f72dbaa8cd7ac47d5e6 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 03:47:05 +0000 Subject: [PATCH 30/45] allow alt lkas --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index db5b4072148a4a..2ed2d02248d5f9 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit db5b4072148a4a769737a318c5a2375c052bb50e +Subproject commit 2ed2d02248d5f94d1f6e241ce03f711dce74c32b From 12d74b607354755a5e230c4665498d488c34b49a Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 04:20:19 +0000 Subject: [PATCH 31/45] fix misra --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 2ed2d02248d5f9..062ba7fad454ad 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2ed2d02248d5f94d1f6e241ce03f711dce74c32b +Subproject commit 062ba7fad454add5b9bfccb15d165c6e754c7160 From ad8460c2a04a1e3f49ab99380b51b6b87555c981 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 13:36:44 -0700 Subject: [PATCH 32/45] steering angle based controller --- opendbc | 2 +- panda | 2 +- selfdrive/car/subaru/carcontroller.py | 33 +++++++++++++++------------ selfdrive/car/subaru/subarucan.py | 4 ++-- selfdrive/car/subaru/values.py | 22 ++++++++++-------- 5 files changed, 36 insertions(+), 27 deletions(-) diff --git a/opendbc b/opendbc index b03468a714da2e..5880fbbccf5a67 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit b03468a714da2eb8ef83f07a373f3f1514491cad +Subproject commit 5880fbbccf5a670631b51836f20e446de643795a diff --git a/panda b/panda index e959fc808d6186..ddbc87dd9dab3a 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e959fc808d61860cd34c58e24e863157224853ca +Subproject commit ddbc87dd9dab3a6d3258f21075c46badd2baed38 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 4aae2476a3fc5f..46f0a03baaa456 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,5 +1,5 @@ from opendbc.can.packer import CANPacker -from selfdrive.car import apply_driver_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits, apply_std_steer_angle_limits from selfdrive.car.subaru import subarucan from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, ALT_LKAS_MSG, PREGLOBAL_CARS, CanBus, CarControllerParams, SubaruFlags @@ -25,25 +25,30 @@ def update(self, CC, CS, now_nanos): # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: - apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) + if self.CP.carFingerprint in ALT_LKAS_MSG: + if CC.latActive: + apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, CarControllerParams) + else: + apply_steer = CS.out.steeringAngleDeg - # limits due to driver torque + can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) + else: + apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) - new_steer = int(round(apply_steer)) - apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) + # limits due to driver torque - if not CC.latActive: - apply_steer = 0 + new_steer = int(round(apply_steer)) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) - if self.CP.carFingerprint in PREGLOBAL_CARS: - can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) - elif self.CP.carFingerprint in ALT_LKAS_MSG: - can_sends.append(subarucan.create_steering_control_alt(self.packer, apply_steer, CC.latActive)) - else: - can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) + if not CC.latActive: + apply_steer = 0 - self.apply_steer_last = apply_steer + if self.CP.carFingerprint in PREGLOBAL_CARS: + can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) + else: + can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) + self.apply_steer_last = apply_steer # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: diff --git a/selfdrive/car/subaru/subarucan.py b/selfdrive/car/subaru/subarucan.py index 086310d5f3ed30..78ddaf1075f7a7 100644 --- a/selfdrive/car/subaru/subarucan.py +++ b/selfdrive/car/subaru/subarucan.py @@ -12,13 +12,13 @@ def create_steering_control(packer, apply_steer, steer_req): } return packer.make_can_msg("ES_LKAS", 0, values) -def create_steering_control_alt(packer, apply_steer, steer_req): +def create_steering_control_angle(packer, apply_steer, steer_req): values = { "LKAS_Output": apply_steer, "LKAS_Request": steer_req, "SET_3": 3 } - return packer.make_can_msg("ES_LKAS_ALT", 0, values) + return packer.make_can_msg("ES_LKAS_ANGLE", 0, values) def create_steering_status(packer): return packer.make_can_msg("ES_LKAS_State", 0, {}) diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 0db4bce411e650..d4cce9362b9e3c 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -4,7 +4,7 @@ from cereal import car from panda.python import uds -from selfdrive.car import dbc_dict +from selfdrive.car import AngleRateLimit, dbc_dict from selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column from selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 @@ -19,15 +19,19 @@ def __init__(self, CP): self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting self.STEER_DRIVER_MULTIPLIER = 50 # weight driver torque heavily self.STEER_DRIVER_FACTOR = 1 # from dbc - - if CP.carFingerprint in GLOBAL_GEN2: - self.STEER_MAX = 1000 - self.STEER_DELTA_UP = 40 - self.STEER_DELTA_DOWN = 40 - elif CP.carFingerprint == CAR.IMPREZA_2020: - self.STEER_MAX = 1439 + + if CP.carFingerprint in ALT_LKAS_MSG: + self.ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0.], angle_v=[1.]) + self.ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0.], angle_v=[1.]) else: - self.STEER_MAX = 2047 + if CP.carFingerprint in GLOBAL_GEN2: + self.STEER_MAX = 1000 + self.STEER_DELTA_UP = 40 + self.STEER_DELTA_DOWN = 40 + elif CP.carFingerprint == CAR.IMPREZA_2020: + self.STEER_MAX = 1439 + else: + self.STEER_MAX = 2047 class SubaruFlags(IntFlag): From e54d35b2c0c66434570c2e8f7121fd237ed56c24 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 13:40:42 -0700 Subject: [PATCH 33/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index ddbc87dd9dab3a..13c60e0084ea56 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ddbc87dd9dab3a6d3258f21075c46badd2baed38 +Subproject commit 13c60e0084ea568f4b25e73a2a9d661d397088f3 From 9e1bf32caa961dc0af9343f8c36b5163d238a5d5 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 13:45:10 -0700 Subject: [PATCH 34/45] cleanup --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 13c60e0084ea56..ed8dd81075f6df 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 13c60e0084ea568f4b25e73a2a9d661d397088f3 +Subproject commit ed8dd81075f6df9ccd226d06f971eae2e786cfb0 From 2fa24830d8dd60324bf782b88f6ac17914564a65 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 16:25:22 -0700 Subject: [PATCH 35/45] merge --- .github/workflows/prebuilt.yaml | 3 + .github/workflows/selfdrive_tests.yaml | 9 +- cereal | 2 +- common/gpio.cc | 2 +- common/params.cc | 1 + common/swaglog.cc | 1 - common/swaglog.h | 6 +- common/time.py | 3 + opendbc | 2 +- panda | 2 +- rednose_repo | 2 +- release/files_common | 1 - selfdrive/boardd/boardd_api_impl.pyx | 14 +- selfdrive/boardd/can_list_to_can_capnp.cc | 4 - selfdrive/boardd/pandad.py | 46 +- selfdrive/boardd/tests/test_pandad.py | 9 + selfdrive/car/ford/carcontroller.py | 4 +- selfdrive/car/ford/carstate.py | 12 +- selfdrive/car/ford/interface.py | 4 +- selfdrive/car/ford/values.py | 13 +- selfdrive/car/isotp_parallel_query.py | 8 +- selfdrive/car/nissan/values.py | 32 +- selfdrive/car/tests/test_car_interfaces.py | 53 +- selfdrive/car/tests/test_fw_fingerprint.py | 6 +- selfdrive/car/toyota/carcontroller.py | 6 +- selfdrive/car/toyota/toyotacan.py | 4 +- selfdrive/car/toyota/values.py | 8 +- selfdrive/controls/controlsd.py | 8 +- selfdrive/controls/lib/drive_helpers.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 4 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/lib/radar_helpers.py | 159 ----- selfdrive/controls/radard.py | 174 ++++-- selfdrive/manager/test/test_manager.py | 4 + selfdrive/modeld/runners/onnx_runner.py | 6 +- selfdrive/navd/helpers.py | 16 +- selfdrive/navd/main.cc | 5 +- .../test/longitudinal_maneuvers/plant.py | 2 +- selfdrive/test/process_replay/README.md | 71 ++- selfdrive/test/process_replay/__init__.py | 2 +- selfdrive/test/process_replay/helpers.py | 27 +- .../test/process_replay/process_replay.py | 560 +++++++++++------- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/process_replay/regen.py | 370 +++--------- .../test/process_replay/test_processes.py | 14 +- selfdrive/ui/SConscript | 5 +- selfdrive/ui/qt/maps/map.cc | 259 ++------ selfdrive/ui/qt/maps/map.h | 48 +- selfdrive/ui/qt/maps/map_eta.cc | 55 ++ selfdrive/ui/qt/maps/map_eta.h | 23 + selfdrive/ui/qt/maps/map_instructions.cc | 146 +++++ selfdrive/ui/qt/maps/map_instructions.h | 27 + selfdrive/ui/qt/offroad/networking.cc | 3 +- selfdrive/ui/qt/offroad/settings.cc | 53 +- selfdrive/ui/qt/onroad.cc | 8 +- selfdrive/ui/translations/main_de.ts | 18 +- selfdrive/ui/translations/main_ja.ts | 18 +- selfdrive/ui/translations/main_ko.ts | 90 +-- selfdrive/ui/translations/main_pt-BR.ts | 26 +- selfdrive/ui/translations/main_zh-CHS.ts | 18 +- selfdrive/ui/translations/main_zh-CHT.ts | 26 +- selfdrive/updated.py | 28 +- system/camerad/cameras/camera_qcom2.cc | 2 +- system/loggerd/deleter.py | 41 +- system/loggerd/encoder/encoder.cc | 7 +- system/loggerd/encoder/encoder.h | 8 +- system/loggerd/encoder/ffmpeg_encoder.cc | 2 - system/loggerd/encoder/v4l_encoder.cc | 2 - system/loggerd/loggerd.cc | 19 + system/loggerd/loggerd.h | 35 +- system/loggerd/tests/loggerd_tests_common.py | 15 +- system/loggerd/tests/test_deleter.py | 99 ++-- system/loggerd/tests/test_loggerd.py | 87 ++- system/loggerd/tests/test_uploader.py | 4 +- system/sensord/rawgps/rawgpsd.py | 34 +- system/sensord/rawgps/test_rawgps.py | 2 +- system/sensord/sensors/bmx055_accel.cc | 2 +- system/sensord/sensors/bmx055_gyro.cc | 2 +- system/sensord/sensors/bmx055_magn.cc | 2 +- system/sensord/sensors/lsm6ds3_accel.cc | 4 +- system/sensord/sensors/lsm6ds3_gyro.cc | 4 +- system/sensord/sensors/mmc5603nj_magn.cc | 2 +- system/swaglog.py | 3 + system/ubloxd/ublox_msg.cc | 2 +- tools/cabana/messageswidget.cc | 6 +- tools/cabana/messageswidget.h | 4 +- tools/cabana/signalview.cc | 2 +- tools/cabana/util.cc | 23 +- tools/cabana/util.h | 5 +- tools/replay/lib/ui_helpers.py | 2 +- 90 files changed, 1566 insertions(+), 1390 deletions(-) delete mode 100644 selfdrive/controls/lib/radar_helpers.py create mode 100644 selfdrive/ui/qt/maps/map_eta.cc create mode 100644 selfdrive/ui/qt/maps/map_eta.h create mode 100644 selfdrive/ui/qt/maps/map_instructions.cc create mode 100644 selfdrive/ui/qt/maps/map_instructions.h diff --git a/.github/workflows/prebuilt.yaml b/.github/workflows/prebuilt.yaml index 1fd093e2acf4e0..6acc7a2e9c62b7 100644 --- a/.github/workflows/prebuilt.yaml +++ b/.github/workflows/prebuilt.yaml @@ -21,6 +21,7 @@ jobs: IMAGE_NAME: openpilot-prebuilt steps: - name: Wait for green check mark + if: ${{ github.event_name != 'workflow_dispatch' }} uses: lewagon/wait-on-check-action@e2558238c09778af25867eb5de5a3ce4bbae3dcd with: ref: master @@ -38,3 +39,5 @@ jobs: run: | $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$IMAGE_NAME:latest + docker tag $DOCKER_REGISTRY/$IMAGE_NAME:latest $DOCKER_REGISTRY/$IMAGE_NAME:$GITHUB_SHA + docker push $DOCKER_REGISTRY/$IMAGE_NAME:$GITHUB_SHA diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 8847dd8b77e592..5fecdc5b51318f 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -23,7 +23,7 @@ env: RUN: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e FILEREADER_CACHE=1 -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $BASE_IMAGE /bin/sh -c BUILD_CL: | - DOCKER_BUILDKIT=1 docker build --pull --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl . + DOCKER_BUILDKIT=1 docker build --build-arg BUILDKIT_INLINE_CACHE=1 --cache-from $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest -t $CL_BASE_IMAGE:latest -f Dockerfile.openpilot_base_cl . RUN_CL: docker run --shm-size 1G -v $PWD:/tmp/openpilot -w /tmp/openpilot -e PYTHONPATH=/tmp/openpilot -e NUM_JOBS -e JOB_ID -e GITHUB_ACTION -e GITHUB_REF -e GITHUB_HEAD_REF -e GITHUB_SHA -e GITHUB_REPOSITORY -e GITHUB_RUN_ID -v /tmp/scons_cache:/tmp/scons_cache -v /tmp/comma_download_cache:/tmp/comma_download_cache -v /tmp/openpilot_cache:/tmp/openpilot_cache $CL_BASE_IMAGE /bin/sh -c UNIT_TEST: coverage run --append -m unittest discover @@ -167,7 +167,6 @@ jobs: name: docker push runs-on: ubuntu-20.04 if: github.ref == 'refs/heads/master' && github.event_name != 'pull_request' && github.repository == 'commaai/openpilot' - needs: static_analysis # hack to ensure slow tests run first since this and static_analysis are fast steps: - uses: actions/checkout@v3 with: @@ -178,12 +177,16 @@ jobs: run: | $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$BASE_IMAGE:latest + docker tag $DOCKER_REGISTRY/$BASE_IMAGE:latest $DOCKER_REGISTRY/$BASE_IMAGE:$GITHUB_SHA + docker push $DOCKER_REGISTRY/$BASE_IMAGE:$GITHUB_SHA - name: Build CL Docker image run: eval "$BUILD_CL" - name: Push to container registry run: | $DOCKER_LOGIN docker push $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest + docker tag $DOCKER_REGISTRY/$CL_BASE_IMAGE:latest $DOCKER_REGISTRY/$CL_BASE_IMAGE:$GITHUB_SHA + docker push $DOCKER_REGISTRY/$CL_BASE_IMAGE:$GITHUB_SHA static_analysis: name: static analysis @@ -305,6 +308,8 @@ jobs: with: submodules: true - uses: ./.github/workflows/setup + - name: Build base Docker image + run: eval "$BUILD" - name: Build Docker image # Sim docker is needed to get the OpenCL drivers run: eval "$BUILD_CL" diff --git a/cereal b/cereal index cf9c5cbb9196f8..6f7102581f57eb 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit cf9c5cbb9196f80a25bd8421a9921ce4801a7561 +Subproject commit 6f7102581f57eb5074b816cc2cfd984218916773 diff --git a/common/gpio.cc b/common/gpio.cc index aa65c0bd9d0014..8a16cd3703539e 100644 --- a/common/gpio.cc +++ b/common/gpio.cc @@ -53,7 +53,7 @@ int gpiochip_get_ro_value_fd(const char* consumer_label, int gpiochiop_id, int p std::string gpiochip_path = "/dev/gpiochip" + std::to_string(gpiochiop_id); int fd = open(gpiochip_path.c_str(), O_RDONLY); if (fd < 0) { - LOGE("Error opening gpiochip0 fd") + LOGE("Error opening gpiochip0 fd"); return -1; } diff --git a/common/params.cc b/common/params.cc index 288de76b0c66ba..fb672cd8aa4517 100644 --- a/common/params.cc +++ b/common/params.cc @@ -177,6 +177,7 @@ std::unordered_map keys = { {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION}, + {"PandaLogState", PERSISTENT}, {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeType", PERSISTENT}, diff --git a/common/swaglog.cc b/common/swaglog.cc index 54f1c3478a30d4..060090e18ff136 100644 --- a/common/swaglog.cc +++ b/common/swaglog.cc @@ -135,4 +135,3 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun cloudlog_t_common(levelnum, filename, lineno, func, frame_id, fmt, args); va_end(args); } - diff --git a/common/swaglog.h b/common/swaglog.h index 68b05ed2e95946..25368501acb1f9 100644 --- a/common/swaglog.h +++ b/common/swaglog.h @@ -21,11 +21,11 @@ void cloudlog_te(int levelnum, const char* filename, int lineno, const char* fun #define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \ __func__, \ - fmt, ## __VA_ARGS__); - + fmt, ## __VA_ARGS__) + #define cloudlog_t(lvl, ...) cloudlog_te(lvl, __FILE__, __LINE__, \ __func__, \ - __VA_ARGS__); + __VA_ARGS__) #define cloudlog_rl(burst, millis, lvl, fmt, ...) \ diff --git a/common/time.py b/common/time.py index 8dac17815d9b48..b9da106fd0dd78 100644 --- a/common/time.py +++ b/common/time.py @@ -1,3 +1,6 @@ import datetime MIN_DATE = datetime.datetime(year=2023, month=6, day=1) + +def system_time_valid(): + return datetime.datetime.now() > MIN_DATE \ No newline at end of file diff --git a/opendbc b/opendbc index 5880fbbccf5a67..3ef35ed2298a3a 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 5880fbbccf5a670631b51836f20e446de643795a +Subproject commit 3ef35ed2298a3a9d199f9145409547710065884c diff --git a/panda b/panda index ed8dd81075f6df..624927486fef2a 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ed8dd81075f6df9ccd226d06f971eae2e786cfb0 +Subproject commit 624927486fef2a58408efbbd6a9c05dcd3131214 diff --git a/rednose_repo b/rednose_repo index 973c04dc427ddd..de6e160b0e6ef4 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 973c04dc427ddddc3e1dee8bf20af0c8c99577dd +Subproject commit de6e160b0e6ef4e286461d6d08297eed8f6fc493 diff --git a/release/files_common b/release/files_common index 1c4955b96f10be..a3b3b92f6f2d37 100644 --- a/release/files_common +++ b/release/files_common @@ -189,7 +189,6 @@ selfdrive/controls/lib/lateral_planner.py selfdrive/controls/lib/longcontrol.py selfdrive/controls/lib/longitudinal_planner.py selfdrive/controls/lib/pid.py -selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/vehicle_model.py selfdrive/controls/lib/lateral_mpc_lib/.gitignore diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx index 0d428a9259c7af..6a552bb44771fb 100644 --- a/selfdrive/boardd/boardd_api_impl.pyx +++ b/selfdrive/boardd/boardd_api_impl.pyx @@ -4,13 +4,15 @@ from libcpp.vector cimport vector from libcpp.string cimport string from libcpp cimport bool -cdef struct can_frame: - long address - string dat - long busTime - long src +cdef extern from "panda.h": + cdef struct can_frame: + long address + string dat + long busTime + long src -cdef extern void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid) +cdef extern from "can_list_to_can_capnp.cc": + void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid) def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True): cdef vector[can_frame] can_list diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc index faa0e37373546e..c1778c51a2570e 100644 --- a/selfdrive/boardd/can_list_to_can_capnp.cc +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -1,8 +1,6 @@ #include "cereal/messaging/messaging.h" #include "panda.h" -extern "C" { - void can_list_to_can_capnp_cpp(const std::vector &can_list, std::string &out, bool sendCan, bool valid) { MessageBuilder msg; auto event = msg.initEvent(valid); @@ -21,5 +19,3 @@ void can_list_to_can_capnp_cpp(const std::vector &can_list, std::stri kj::ArrayOutputStream output_stream(kj::ArrayPtr((unsigned char *)out.data(), msg_size)); capnp::writeMessage(output_stream, msg); } - -} diff --git a/selfdrive/boardd/pandad.py b/selfdrive/boardd/pandad.py index 4d9b4d8960835c..5f308b9ad90585 100755 --- a/selfdrive/boardd/pandad.py +++ b/selfdrive/boardd/pandad.py @@ -3,6 +3,7 @@ import os import usb1 import time +import json import subprocess from typing import List, NoReturn from functools import cmp_to_key @@ -23,6 +24,48 @@ def get_expected_signature(panda: Panda) -> bytes: cloudlog.exception("Error computing expected signature") return b"" +def read_panda_logs(panda: Panda) -> None: + """ + Forward panda logs to the cloud + """ + + params = Params() + serial = panda.get_usb_serial() + + log_state = {} + try: + l = json.loads(params.get("PandaLogState")) + for k, v in l.items(): + if isinstance(k, str) and isinstance(v, int): + log_state[k] = v + except (TypeError, json.JSONDecodeError): + cloudlog.exception("failed to parse PandaLogState") + + try: + if serial in log_state: + logs = panda.get_logs(last_id=log_state[serial]) + else: + logs = panda.get_logs(get_all=True) + + # truncate logs to 100 entries if needed + MAX_LOGS = 100 + if len(logs) > MAX_LOGS: + cloudlog.warning(f"Panda {serial} has {len(logs)} logs, truncating to {MAX_LOGS}") + logs = logs[-MAX_LOGS:] + + # update log state + if len(logs) > 0: + log_state[serial] = logs[-1]["id"] + + for log in logs: + if log['timestamp'] is not None: + log['timestamp'] = log['timestamp'].isoformat() + cloudlog.event("panda_log", **log, serial=serial) + + params.put("PandaLogState", json.dumps(log_state)) + except Exception: + cloudlog.exception(f"Error getting logs for panda {serial}") + def flash_panda(panda_serial: str) -> Panda: try: @@ -47,7 +90,6 @@ def flash_panda(panda_serial: str) -> Panda: if panda.bootstub: bootstub_version = panda.get_version() cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. {bootstub_version=}, {internal_panda=}") - if internal_panda: HARDWARE.recover_internal_panda() panda.recover(reset=(not internal_panda)) @@ -139,6 +181,8 @@ def main() -> NoReturn: params.put_bool("PandaHeartbeatLost", True) cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) + read_panda_logs(panda) + if first_run: if panda.is_internal(): # update time from RTC diff --git a/selfdrive/boardd/tests/test_pandad.py b/selfdrive/boardd/tests/test_pandad.py index c1f080efe5d4c8..1d49446bf52cac 100755 --- a/selfdrive/boardd/tests/test_pandad.py +++ b/selfdrive/boardd/tests/test_pandad.py @@ -6,6 +6,7 @@ import cereal.messaging as messaging from cereal import log from common.gpio import gpio_set, gpio_init +from common.params import Params from panda import Panda, PandaDFU, PandaProtocolMismatch from selfdrive.test.helpers import phone_only from selfdrive.manager.process_config import managed_processes @@ -17,6 +18,10 @@ class TestPandad(unittest.TestCase): + def setUp(self): + self.params = Params() + self.start_log_state = self.params.get("PandaLogState") + def tearDown(self): managed_processes['pandad'].stop() @@ -30,6 +35,10 @@ def _wait_for_boardd(self, timeout=30): if sm['peripheralState'].pandaType == log.PandaState.PandaType.unknown: raise Exception("boardd failed to start") + # simple check that we did something with the panda logs + cur_log_state = self.params.get("PandaLogState") + assert cur_log_state != self.start_log_state + def _go_to_dfu(self): HARDWARE.recover_internal_panda() assert Panda.wait_for_dfu(None, 10) diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 2ef80420bd406f..dd30bc57e14bac 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -4,7 +4,7 @@ from selfdrive.car import apply_std_steer_angle_limits from selfdrive.car.ford.fordcan import CanBus, create_acc_msg, create_acc_ui_msg, create_button_msg, \ create_lat_ctl_msg, create_lat_ctl2_msg, create_lka_msg, create_lkas_ui_msg -from selfdrive.car.ford.values import CANFD_CARS, CarControllerParams +from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -69,7 +69,7 @@ def update(self, CC, CS, now_nanos): self.apply_curvature_last = apply_curvature - if self.CP.carFingerprint in CANFD_CARS: + if self.CP.carFingerprint in CANFD_CAR: # TODO: extended mode mode = 1 if CC.latActive else 0 counter = (self.frame // CarControllerParams.STEER_STEP) % 0xF diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 2f15d6797e6e1f..d9848096e77e38 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -4,7 +4,7 @@ from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase from selfdrive.car.ford.fordcan import CanBus -from selfdrive.car.ford.values import CANFD_CARS, CarControllerParams, DBC +from selfdrive.car.ford.values import CANFD_CAR, CarControllerParams, DBC GearShifter = car.CarState.GearShifter TransmissionType = car.CarParams.TransmissionType @@ -55,7 +55,7 @@ def update(self, cp, cp_cam): ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) # ret.espDisabled = False # TODO: find traction control signal - if self.CP.carFingerprint in CANFD_CARS: + if self.CP.carFingerprint in CANFD_CAR: # this signal is always 0 on non-CAN FD cars ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3) @@ -97,7 +97,7 @@ def update(self, cp, cp_cam): # blindspot sensors if self.CP.enableBsm: - cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CARS else cp + cp_bsm = cp_cam if self.CP.carFingerprint in CANFD_CAR else cp ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 @@ -191,7 +191,7 @@ def get_can_parser(CP): ("RCMStatusMessage2_FD1", 10), ] - if CP.carFingerprint in CANFD_CARS: + if CP.carFingerprint in CANFD_CAR: signals += [ ("LatCtlSte_D_Stat", "Lane_Assist_Data3_FD1"), # PSCM lateral control status ] @@ -216,7 +216,7 @@ def get_can_parser(CP): ("BCM_Lamp_Stat_FD1", 1), ] - if CP.enableBsm and CP.carFingerprint not in CANFD_CARS: + if CP.enableBsm and CP.carFingerprint not in CANFD_CAR: signals += [ ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right @@ -286,7 +286,7 @@ def get_cam_can_parser(CP): ("IPMA_Data", 1), ] - if CP.enableBsm and CP.carFingerprint in CANFD_CARS: + if CP.enableBsm and CP.carFingerprint in CANFD_CAR: signals += [ ("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left ("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 72bc06a8aceab4..d74baa3ce45feb 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -4,7 +4,7 @@ from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, get_safety_config from selfdrive.car.ford.fordcan import CanBus -from selfdrive.car.ford.values import CANFD_CARS, CAR, Ecu +from selfdrive.car.ford.values import CANFD_CAR, CAR, Ecu from selfdrive.car.interfaces import CarInterfaceBase TransmissionType = car.CarParams.TransmissionType @@ -33,7 +33,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_LONG_CONTROL ret.openpilotLongitudinalControl = True - if candidate in CANFD_CARS: + if candidate in CANFD_CAR: ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_FORD_CANFD if candidate == CAR.BRONCO_SPORT_MK1: diff --git a/selfdrive/car/ford/values.py b/selfdrive/car/ford/values.py index c4fe80ab3eb77e..7ce0abb21f87e0 100644 --- a/selfdrive/car/ford/values.py +++ b/selfdrive/car/ford/values.py @@ -49,7 +49,7 @@ class CAR: MAVERICK_MK1 = "FORD MAVERICK 1ST GEN" -CANFD_CARS = {CAR.F_150_MK14} +CANFD_CAR = {CAR.F_150_MK14} class RADAR: @@ -98,18 +98,19 @@ def init_make(self, CP: car.CarParams): FW_QUERY_CONFIG = FwQueryConfig( requests=[ + # CAN and CAN FD queries are combined. + # FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus + # TODO: properly handle auxiliary requests to separate queries and add back whitelists Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + # whitelist_ecus=[Ecu.engine], + auxiliary=True, ), Request( [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], - bus=0, - ), - Request( - [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], - [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], + # whitelist_ecus=[Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.shiftByWire], bus=0, auxiliary=True, ), diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index 8ab9728e5f0895..93033126a0931d 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -115,7 +115,13 @@ def get_data(self, timeout, total_timeout=60.): addrs_responded.add(tx_addr) response_timeouts[tx_addr] = time.monotonic() + timeout - if not dat: + if dat is None: + continue + + # Log unexpected empty responses + if len(dat) == 0: + cloudlog.error(f"iso-tp query empty response: {tx_addr}") + request_done[tx_addr] = True continue counter = request_counter[tx_addr] diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index 1ba88ac7b1d2f0..d4e10e11ca7e2d 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -79,35 +79,25 @@ class NissanCarInfo(CarInfo): ] } -NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL]) -NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40]) +NISSAN_DIAGNOSTIC_REQUEST_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, 0x81]) +NISSAN_DIAGNOSTIC_RESPONSE_KWP = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, 0x81]) NISSAN_VERSION_REQUEST_KWP = b'\x21\x83' NISSAN_VERSION_RESPONSE_KWP = b'\x61\x83' NISSAN_RX_OFFSET = 0x20 -# Try diagnostic sessions: default, standby, extended, Nissan-specific -NISSAN_DIAGNOSTIC_SESSION_TYPES = (0x81, 0x89, 0x92, 0xc0) -NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE = 0xc0 - FW_QUERY_CONFIG = FwQueryConfig( requests=[ - *[ - Request( - [NISSAN_DIAGNOSTIC_REQUEST_KWP + bytes([subfunction]), NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP + bytes([subfunction]), NISSAN_VERSION_RESPONSE_KWP], - logging=subfunction != NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE, - ) for subfunction in NISSAN_DIAGNOSTIC_SESSION_TYPES - ], - *[ - Request( - [NISSAN_DIAGNOSTIC_REQUEST_KWP + bytes([subfunction]), NISSAN_VERSION_REQUEST_KWP], - [NISSAN_DIAGNOSTIC_RESPONSE_KWP + bytes([subfunction]), NISSAN_VERSION_RESPONSE_KWP], - rx_offset=NISSAN_RX_OFFSET, - logging=subfunction != NISSAN_DEFAULT_DIAGNOSTIC_SESSION_TYPE, - ) for subfunction in NISSAN_DIAGNOSTIC_SESSION_TYPES - ], + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + ), + Request( + [NISSAN_DIAGNOSTIC_REQUEST_KWP, NISSAN_VERSION_REQUEST_KWP], + [NISSAN_DIAGNOSTIC_RESPONSE_KWP, NISSAN_VERSION_RESPONSE_KWP], + rx_offset=NISSAN_RX_OFFSET, + ), Request( [StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST], [StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE], diff --git a/selfdrive/car/tests/test_car_interfaces.py b/selfdrive/car/tests/test_car_interfaces.py index 330a522d894215..005ea114fdd000 100755 --- a/selfdrive/car/tests/test_car_interfaces.py +++ b/selfdrive/car/tests/test_car_interfaces.py @@ -7,10 +7,35 @@ from parameterized import parameterized from cereal import car +from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint from selfdrive.car.car_helpers import interfaces -from selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS, all_known_cars -from selfdrive.test.fuzzy_generation import FuzzyGenerator +from selfdrive.car.fingerprints import all_known_cars +from selfdrive.test.fuzzy_generation import DrawType, FuzzyGenerator + + +def get_fuzzy_car_interface_args(draw: DrawType) -> dict: + # Fuzzy CAN fingerprints and FW versions to test more states of the CarInterface + fingerprint_strategy = st.fixed_dictionaries({key: st.dictionaries(st.integers(min_value=0, max_value=0x800), + st.integers(min_value=0, max_value=64)) for key in + gen_empty_fingerprint()}) + + # just the most important fields + car_fw_strategy = st.lists(st.fixed_dictionaries({ + 'ecu': st.sampled_from(list(car.CarParams.Ecu.schema.enumerants.keys())), + # TODO: only use reasonable addrs for the paired ecu and brand/platform + 'address': st.integers(min_value=0, max_value=0x800), + })) + + params_strategy = st.fixed_dictionaries({ + 'fingerprints': fingerprint_strategy, + 'car_fw': car_fw_strategy, + 'experimental_long': st.booleans(), + }) + + params: dict = draw(params_strategy) + params['car_fw'] = [car.CarParams.CarFw(**fw) for fw in params['car_fw']] + return params class TestCarInterfaces(unittest.TestCase): @@ -19,19 +44,12 @@ class TestCarInterfaces(unittest.TestCase): @settings(max_examples=5) @given(data=st.data()) def test_car_interfaces(self, car_name, data): - if car_name in FINGERPRINTS: - fingerprint = FINGERPRINTS[car_name][0] - else: - fingerprint = {} - CarInterface, CarController, CarState = interfaces[car_name] - fingerprints = gen_empty_fingerprint() - fingerprints.update({k: fingerprint for k in fingerprints.keys()}) - car_fw = [] - experimental_long = data.draw(st.booleans()) + args = get_fuzzy_car_interface_args(data.draw) - car_params = CarInterface.get_params(car_name, fingerprints, car_fw, experimental_long=experimental_long, docs=False) + car_params = CarInterface.get_params(car_name, args['fingerprints'], args['car_fw'], + experimental_long=args['experimental_long'], docs=False) car_interface = CarInterface(car_params, CarController, CarState) assert car_params assert car_interface @@ -64,18 +82,21 @@ def test_car_interfaces(self, car_name, data): cc_msg = FuzzyGenerator.get_random_msg(data.draw, car.CarControl, real_floats=True) # Run car interface + now_nanos = 0 CC = car.CarControl.new_message(**cc_msg) for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, 0) - car_interface.apply(CC, 0) + car_interface.apply(CC, now_nanos) + car_interface.apply(CC, now_nanos) + now_nanos += DT_CTRL * 1e9 # 10 ms CC = car.CarControl.new_message(**cc_msg) CC.enabled = True for _ in range(10): car_interface.update(CC, []) - car_interface.apply(CC, 0) - car_interface.apply(CC, 0) + car_interface.apply(CC, now_nanos) + car_interface.apply(CC, now_nanos) + now_nanos += DT_CTRL * 1e9 # 10ms # Test radar interface RadarInterface = importlib.import_module(f'selfdrive.car.{car_params.carName}.radar_interface').RadarInterface diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index fd8c23263abf37..c1e1d68d2c6cbf 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -220,16 +220,16 @@ def test_startup_timing(self): print(f'get_vin, query time={vin_time / self.N} seconds') def test_fw_query_timing(self): - total_ref_time = 6.7 + total_ref_time = 6.0 brand_ref_times = { 1: { 'body': 0.1, 'chrysler': 0.3, - 'ford': 0.3, + 'ford': 0.2, 'honda': 0.5, 'hyundai': 0.7, 'mazda': 0.2, - 'nissan': 0.9, + 'nissan': 0.3, 'subaru': 0.1, 'tesla': 0.2, 'toyota': 1.6, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index fa9a42ee165dfc..905251b5328e6f 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -24,6 +24,7 @@ # 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 class CarController: @@ -94,7 +95,10 @@ def update(self, CC, CS, now_nanos): can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle - can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2)) + full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and + abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE) + setme_x64 = 100 if lta_active and full_torque_condition else 0 + can_sends.append(create_lta_steer_command(self.packer, 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/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 38351139a881d6..01861c534a3b97 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -9,7 +9,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_angle, steer_req, frame): +def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { @@ -17,7 +17,7 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame): "SETME_X1": 1, "SETME_X3": 3, "PERCENTAGE": 100, - "SETME_X64": 0, + "SETME_X64": setme_x64, "ANGLE": 0, "STEER_ANGLE_CMD": steer_angle, "STEER_REQUEST": steer_req, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index fe354f1dd65732..c36f0c906cc082 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -272,9 +272,8 @@ class ToyotaCarInfo(CarInfo): # Responds to KWP (0x1a8881): # - Body Control Module ((0x750, 0x40)) - # Hybrid control computer can be on one of two addresses + # Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform (Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer - (Ecu.hybrid, 0x7d2, None), # Hybrid Control Assembly & Computer # TODO: if these duplicate ECUs always exist together, remove one (Ecu.srs, 0x780, None), # SRS Airbag (Ecu.srs, 0x784, None), # SRS Airbag 2 @@ -1653,6 +1652,11 @@ class ToyotaCarInfo(CarInfo): b'\x01896634AE1001\x00\x00\x00\x00', b'\x01896634AF0000\x00\x00\x00\x00', ], + (Ecu.hybrid, 0x7d2, None): [ + b'\x02899830R41000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00', + b'\x028998342C0000\x00\x00\x00\x00899854224000\x00\x00\x00\x00', + b'\x02899830R39000\x00\x00\x00\x00899850R20000\x00\x00\x00\x00', + ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F0R03100\x00\x00\x00\x00', ], diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1eec484ff98c67..47b41f872e3fed 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -7,7 +7,7 @@ from common.numpy_fast import clip from common.realtime import sec_since_boot, config_realtime_process, Priority, Ratekeeper, DT_CTRL from common.profiler import Profiler -from common.params import Params, put_nonblocking +from common.params import Params, put_nonblocking, put_bool_nonblocking import cereal.messaging as messaging from cereal.visionipc import VisionIpcClient, VisionStreamType from common.conversions import Conversions as CV @@ -134,6 +134,8 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None): # cleanup old params if not self.CP.experimentalLongitudinalAvailable or is_release_branch(): self.params.remove("ExperimentalLongitudinalEnabled") + if not self.CP.openpilotLongitudinalControl: + self.params.remove("ExperimentalMode") self.CC = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() @@ -446,7 +448,7 @@ def data_sample(self): self.initialized = True self.set_initial_state() - Params().put_bool("ControlsReady", True) + put_bool_nonblocking("ControlsReady", True) # Check for CAN timeout if not can_strs: @@ -843,7 +845,7 @@ def step(self): self.prof.checkpoint("Ratekeeper", ignore=True) self.is_metric = self.params.get_bool("IsMetric") - self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl # Sample data from sockets and get a carState CS = self.data_sample() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 7bcfde27871c67..7cbcbc3d4991a0 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -132,7 +132,7 @@ def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: if self.CP.pcmCruise: return - initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode and self.CP.openpilotLongitudinalControl else V_CRUISE_INITIAL + initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if experimental_mode else V_CRUISE_INITIAL # 250kph or above probably means we never had a set speed if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index b5998a2564f734..46d39b34fb13ab 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -7,7 +7,7 @@ from system.swaglog import cloudlog # WARNING: imports outside of constants will not trigger a rebuild from selfdrive.modeld.constants import index_function -from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU +from selfdrive.controls.radard import _LEAD_ACCEL_TAU if __name__ == '__main__': # generating code from third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver @@ -357,7 +357,7 @@ def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalP v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, get_T_FOLLOW(personality)) + cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) self.source = SOURCES[np.argmin(x_obstacles[0])] diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6e990bd9890913..a9c3cc78042ff9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -90,7 +90,7 @@ def update(self, sm): if self.param_read_counter % 50 == 0: self.read_param() self.param_read_counter += 1 - self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode and self.CP.openpilotLongitudinalControl else 'acc' + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo v_cruise_kph = sm['controlsState'].vCruise diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py deleted file mode 100644 index 4184340dc5437f..00000000000000 --- a/selfdrive/controls/lib/radar_helpers.py +++ /dev/null @@ -1,159 +0,0 @@ -from common.numpy_fast import mean -from common.kalman.simple_kalman import KF1D - - -# Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 - -# radar tracks -SPEED, ACCEL = 0, 1 # Kalman filter states enum - -# stationary qualification parameters -v_ego_stationary = 4. # no stationary object flag below this speed - -RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car -RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame - -class Track(): - def __init__(self, v_lead, kalman_params): - self.cnt = 0 - self.aLeadTau = _LEAD_ACCEL_TAU - self.K_A = kalman_params.A - self.K_C = kalman_params.C - self.K_K = kalman_params.K - self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) - - def update(self, d_rel, y_rel, v_rel, v_lead, measured): - # relative values, copy - self.dRel = d_rel # LONG_DIST - self.yRel = y_rel # -LAT_DIST - self.vRel = v_rel # REL_SPEED - self.vLead = v_lead - self.measured = measured # measured or estimate - - # computed velocity and accelerations - if self.cnt > 0: - self.kf.update(self.vLead) - - self.vLeadK = float(self.kf.x[SPEED][0]) - self.aLeadK = float(self.kf.x[ACCEL][0]) - - # Learn if constant acceleration - if abs(self.aLeadK) < 0.5: - self.aLeadTau = _LEAD_ACCEL_TAU - else: - self.aLeadTau *= 0.9 - - self.cnt += 1 - - def get_key_for_cluster(self): - # Weigh y higher since radar is inaccurate in this dimension - return [self.dRel, self.yRel*2, self.vRel] - - def reset_a_lead(self, aLeadK, aLeadTau): - self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) - self.aLeadK = aLeadK - self.aLeadTau = aLeadTau - - -class Cluster(): - def __init__(self): - self.tracks = set() - - def add(self, t): - # add the first track - self.tracks.add(t) - - # TODO: make generic - @property - def dRel(self): - return mean([t.dRel for t in self.tracks]) - - @property - def yRel(self): - return mean([t.yRel for t in self.tracks]) - - @property - def vRel(self): - return mean([t.vRel for t in self.tracks]) - - @property - def aRel(self): - return mean([t.aRel for t in self.tracks]) - - @property - def vLead(self): - return mean([t.vLead for t in self.tracks]) - - @property - def dPath(self): - return mean([t.dPath for t in self.tracks]) - - @property - def vLat(self): - return mean([t.vLat for t in self.tracks]) - - @property - def vLeadK(self): - return mean([t.vLeadK for t in self.tracks]) - - @property - def aLeadK(self): - if all(t.cnt <= 1 for t in self.tracks): - return 0. - else: - return mean([t.aLeadK for t in self.tracks if t.cnt > 1]) - - @property - def aLeadTau(self): - if all(t.cnt <= 1 for t in self.tracks): - return _LEAD_ACCEL_TAU - else: - return mean([t.aLeadTau for t in self.tracks if t.cnt > 1]) - - @property - def measured(self): - return any(t.measured for t in self.tracks) - - def get_RadarState(self, model_prob=0.0): - return { - "dRel": float(self.dRel), - "yRel": float(self.yRel), - "vRel": float(self.vRel), - "vLead": float(self.vLead), - "vLeadK": float(self.vLeadK), - "aLeadK": float(self.aLeadK), - "status": True, - "fcw": self.is_potential_fcw(model_prob), - "modelProb": model_prob, - "radar": True, - "aLeadTau": float(self.aLeadTau) - } - - def get_RadarState_from_vision(self, lead_msg, v_ego, model_v_ego): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego - return { - "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), - "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": 0.0, - "aLeadTau": 0.3, - "fcw": False, - "modelProb": float(lead_msg.prob), - "radar": False, - "status": True - } - - def __str__(self): - ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" - return ret - - def potential_low_speed_lead(self, v_ego): - # stop for stuff in front of you and low speed, even without model confirmation - # Radar points closer than 0.75, are almost always glitches on toyota radars - return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and (0.75 < self.dRel < 25) - - def is_potential_fcw(self, model_prob): - return model_prob > .9 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 521cea816f09ac..c4d634ae2997de 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -8,9 +8,108 @@ from common.numpy_fast import interp from common.params import Params from common.realtime import Ratekeeper, Priority, config_realtime_process -from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA from system.swaglog import cloudlog -from third_party.cluster.fastcluster_py import cluster_points_centroid + +from common.kalman.simple_kalman import KF1D + + +# Default lead acceleration decay set to 50% at 1s +_LEAD_ACCEL_TAU = 1.5 + +# radar tracks +SPEED, ACCEL = 0, 1 # Kalman filter states enum + +# stationary qualification parameters +v_ego_stationary = 4. # no stationary object flag below this speed + +RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car +RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame + + +def get_RadarState_from_vision(lead_msg, v_ego, model_v_ego): + lead_v_rel_pred = lead_msg.v[0] - model_v_ego + return { + "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), + "yRel": float(-lead_msg.y[0]), + "vRel": float(lead_v_rel_pred), + "vLead": float(v_ego + lead_v_rel_pred), + "vLeadK": float(v_ego + lead_v_rel_pred), + "aLeadK": 0.0, + "aLeadTau": 0.3, + "fcw": False, + "modelProb": float(lead_msg.prob), + "radar": False, + "status": True + } + +class Track(): + def __init__(self, v_lead, kalman_params): + self.cnt = 0 + self.aLeadTau = _LEAD_ACCEL_TAU + self.K_A = kalman_params.A + self.K_C = kalman_params.C + self.K_K = kalman_params.K + self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) + + def update(self, d_rel, y_rel, v_rel, v_lead, measured): + # relative values, copy + self.dRel = d_rel # LONG_DIST + self.yRel = y_rel # -LAT_DIST + self.vRel = v_rel # REL_SPEED + self.vLead = v_lead + self.measured = measured # measured or estimate + + # computed velocity and accelerations + if self.cnt > 0: + self.kf.update(self.vLead) + + self.vLeadK = float(self.kf.x[SPEED][0]) + self.aLeadK = float(self.kf.x[ACCEL][0]) + + # Learn if constant acceleration + if abs(self.aLeadK) < 0.5: + self.aLeadTau = _LEAD_ACCEL_TAU + else: + self.aLeadTau *= 0.9 + + self.cnt += 1 + + def get_key_for_cluster(self): + # Weigh y higher since radar is inaccurate in this dimension + return [self.dRel, self.yRel*2, self.vRel] + + def reset_a_lead(self, aLeadK, aLeadTau): + self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) + self.aLeadK = aLeadK + self.aLeadTau = aLeadTau + + + def get_RadarState(self, model_prob=0.0): + return { + "dRel": float(self.dRel), + "yRel": float(self.yRel), + "vRel": float(self.vRel), + "vLead": float(self.vLead), + "vLeadK": float(self.vLeadK), + "aLeadK": float(self.aLeadK), + "status": True, + "fcw": self.is_potential_fcw(model_prob), + "modelProb": model_prob, + "radar": True, + "aLeadTau": float(self.aLeadTau) + } + + def __str__(self): + ret = f"x: {self.dRel:4.1f} y: {self.yRel:4.1f} v: {self.vRel:4.1f} a: {self.aLeadK:4.1f}" + return ret + + def potential_low_speed_lead(self, v_ego): + # stop for stuff in front of you and low speed, even without model confirmation + # Radar points closer than 0.75, are almost always glitches on toyota radars + return abs(self.yRel) < 1.0 and (v_ego < v_ego_stationary) and (0.75 < self.dRel < 25) + + def is_potential_fcw(self, model_prob): + return model_prob > .9 class KalmanParams(): @@ -40,8 +139,7 @@ def laplacian_pdf(x, mu, b): return math.exp(-abs(x-mu)/b) -def match_vision_to_cluster(v_ego, lead, clusters): - # match vision point to best statistical cluster match +def match_vision_to_track(v_ego, lead, tracks): offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA def prob(c): @@ -52,39 +150,39 @@ def prob(c): # This is isn't exactly right, but good heuristic return prob_d * prob_y * prob_v - cluster = max(clusters, key=prob) + track = max(tracks.values(), key=prob) # if no 'sane' match is found return -1 # stationary radar points can be false positives - dist_sane = abs(cluster.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) - vel_sane = (abs(cluster.vRel + v_ego - lead.v[0]) < 10) or (v_ego + cluster.vRel > 3) + dist_sane = abs(track.dRel - offset_vision_dist) < max([(offset_vision_dist)*.25, 5.0]) + vel_sane = (abs(track.vRel + v_ego - lead.v[0]) < 10) or (v_ego + track.vRel > 3) if dist_sane and vel_sane: - return cluster + return track else: return None -def get_lead(v_ego, ready, clusters, lead_msg, model_v_ego, low_speed_override=True): +def get_lead(v_ego, ready, tracks, lead_msg, model_v_ego, low_speed_override=True): # Determine leads, this is where the essential logic happens - if len(clusters) > 0 and ready and lead_msg.prob > .5: - cluster = match_vision_to_cluster(v_ego, lead_msg, clusters) + if len(tracks) > 0 and ready and lead_msg.prob > .5: + track = match_vision_to_track(v_ego, lead_msg, tracks) else: - cluster = None + track = None lead_dict = {'status': False} - if cluster is not None: - lead_dict = cluster.get_RadarState(lead_msg.prob) - elif (cluster is None) and ready and (lead_msg.prob > .5): - lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + if track is not None: + lead_dict = track.get_RadarState(lead_msg.prob) + elif (track is None) and ready and (lead_msg.prob > .5): + lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: - low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)] - if len(low_speed_clusters) > 0: - closest_cluster = min(low_speed_clusters, key=lambda c: c.dRel) + low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] + if len(low_speed_tracks) > 0: + closest_track = min(low_speed_tracks, key=lambda c: c.dRel) - # Only choose new cluster if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_cluster.dRel < lead_dict['dRel']): - lead_dict = closest_cluster.get_RadarState() + # Only choose new track if it is actually closer than the previous one + if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): + lead_dict = closest_track.get_RadarState() return lead_dict @@ -132,34 +230,6 @@ def update(self, sm, rr): self.tracks[ids] = Track(v_lead, self.kalman_params) self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) - idens = list(sorted(self.tracks.keys())) - track_pts = [self.tracks[iden].get_key_for_cluster() for iden in idens] - - # If we have multiple points, cluster them - if len(track_pts) > 1: - cluster_idxs = cluster_points_centroid(track_pts, 2.5) - clusters = [None] * (max(cluster_idxs) + 1) - - for idx in range(len(track_pts)): - cluster_i = cluster_idxs[idx] - if clusters[cluster_i] is None: - clusters[cluster_i] = Cluster() - clusters[cluster_i].add(self.tracks[idens[idx]]) - elif len(track_pts) == 1: - # FIXME: cluster_point_centroid hangs forever if len(track_pts) == 1 - cluster_idxs = [0] - clusters = [Cluster()] - clusters[0].add(self.tracks[idens[0]]) - else: - clusters = [] - - # if a new point, reset accel to the rest of the cluster - for idx in range(len(track_pts)): - if self.tracks[idens[idx]].cnt <= 1: - aLeadK = clusters[cluster_idxs[idx]].aLeadK - aLeadTau = clusters[cluster_idxs[idx]].aLeadTau - self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) - # *** publish radarState *** dat = messaging.new_message('radarState') dat.valid = sm.all_checks() and len(rr.errors) == 0 @@ -174,8 +244,8 @@ def update(self, sm, rr): model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], model_v_ego, low_speed_override=True) - radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], model_v_ego, low_speed_override=False) + radarState.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) + radarState.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) return dat diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py index 889ab3d2796eab..538ea82fc8884a 100755 --- a/selfdrive/manager/test/test_manager.py +++ b/selfdrive/manager/test/test_manager.py @@ -61,6 +61,10 @@ def test_clean_exit(self): self.assertTrue(exit_code is not None, f"{p.name} failed to exit") + # TODO: mapsd should exit cleanly + if p.name == "mapsd": + continue + # TODO: interrupted blocking read exits with 1 in cereal. use a more unique return code exit_codes = [0, 1] if p.sigkill: diff --git a/selfdrive/modeld/runners/onnx_runner.py b/selfdrive/modeld/runners/onnx_runner.py index d4a11a7c0be48c..ee51e489ed1b43 100755 --- a/selfdrive/modeld/runners/onnx_runner.py +++ b/selfdrive/modeld/runners/onnx_runner.py @@ -3,6 +3,7 @@ import os import sys import numpy as np +from typing import Tuple, Dict, Union, Any os.environ["OMP_NUM_THREADS"] = "4" os.environ["OMP_WAIT_POLICY"] = "PASSIVE" @@ -55,14 +56,15 @@ def run_loop(m, tf8_input=False): print("Onnx available providers: ", ort.get_available_providers(), file=sys.stderr) options = ort.SessionOptions() options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_DISABLE_ALL + + provider: Union[str, Tuple[str, Dict[Any, Any]]] if 'OpenVINOExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: provider = 'OpenVINOExecutionProvider' elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ: options.intra_op_num_threads = 2 - provider = 'CUDAExecutionProvider' + provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'DEFAULT'}) else: options.intra_op_num_threads = 2 - options.inter_op_num_threads = 8 options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL provider = 'CPUExecutionProvider' diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index abb7d43c491f1d..011a6c5fb8e8b4 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -131,6 +131,10 @@ def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: return SPEED_CONVERSIONS[unit] * speed +def field_valid(dat: dict, field: str) -> bool: + return field in dat and dat[field] is not None + + def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: if not len(banners): return @@ -147,19 +151,19 @@ def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuv # Primary p = current_banner['primary'] - if 'text' in p: + if field_valid(p, 'text'): instruction.maneuverPrimaryText = p['text'] - if 'type' in p: + if field_valid(p, 'type'): instruction.maneuverType = p['type'] - if 'modifier' in p: + if field_valid(p, 'modifier'): instruction.maneuverModifier = p['modifier'] # Secondary - if 'secondary' in current_banner: + if field_valid(current_banner, 'secondary'): instruction.maneuverSecondaryText = current_banner['secondary']['text'] # Lane lines - if 'sub' in current_banner: + if field_valid(current_banner, 'sub'): lanes = [] for component in current_banner['sub']['components']: if component['type'] != 'lane': @@ -170,7 +174,7 @@ def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuv 'directions': [string_to_direction(d) for d in component['directions']], } - if 'active_direction' in component: + if field_valid(component, 'active_direction'): lane['activeDirection'] = string_to_direction(component['active_direction']) lanes.append(lane) diff --git a/selfdrive/navd/main.cc b/selfdrive/navd/main.cc index 8cef07edcf8dc6..f8501bf4a5a0c9 100644 --- a/selfdrive/navd/main.cc +++ b/selfdrive/navd/main.cc @@ -1,6 +1,8 @@ +#include +#include + #include #include -#include #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/qt/maps/map_helpers.h" @@ -9,6 +11,7 @@ int main(int argc, char *argv[]) { qInstallMessageHandler(swagLogMessageHandler); + setpriority(PRIO_PROCESS, 0, -20); QApplication app(argc, argv); std::signal(SIGINT, sigTermHandler); diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 8febbf4022aaee..541f7d8747c550 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -8,7 +8,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState from selfdrive.modeld.constants import T_IDXS from selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner -from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU +from selfdrive.controls.radard import _LEAD_ACCEL_TAU class Plant: diff --git a/selfdrive/test/process_replay/README.md b/selfdrive/test/process_replay/README.md index 531ddb3a023c46..571157dfc9eb03 100644 --- a/selfdrive/test/process_replay/README.md +++ b/selfdrive/test/process_replay/README.md @@ -15,9 +15,10 @@ Currently the following processes are tested: * calibrationd * dmonitoringd * locationd -* laikad * paramsd * ubloxd +* laikad +* torqued ### Usage ``` @@ -45,3 +46,71 @@ To generate new logs: `./test_processes.py` Then, check in the new logs using git-lfs. Make sure to also update the `ref_commit` file to the current commit. + +## API + +Process replay test suite exposes programmatic APIs for simultaneously running processes or groups of processes on provided logs. + +```py +def replay_process_with_name(name: Union[str, Iterable[str]], lr: Union[LogReader, List[capnp._DynamicStructReader]], *args, **kwargs) -> List[capnp._DynamicStructReader]: + +def replay_process( + cfg: Union[ProcessConfig, Iterable[ProcessConfig]], lr: Union[LogReader, List[capnp._DynamicStructReader]], frs: Optional[Dict[str, Any]] = None, + fingerprint: Optional[str] = None, return_all_logs: bool = False, custom_params: Optional[Dict[str, Any]] = None, disable_progress: bool = False +) -> List[capnp._DynamicStructReader]: +``` + +Example usage: +```py +from selfdrive.test.process_replay import replay_process_with_name +from tools.lib.logreader import LogReader + +lr = LogReader(...) + +# provide a name of the process to replay +output_logs = replay_process_with_name('locationd', lr) + +# or list of names +output_logs = replay_process_with_name(['ubloxd', 'locationd', 'laikad'], lr) +``` + +Supported processes: +* controlsd +* radard +* plannerd +* calibrationd +* dmonitoringd +* locationd +* paramsd +* ubloxd +* laikad +* torqued +* modeld +* dmonitoringmodeld + +Certain processes may require an initial state, which is usually supplied within `Params` and persisting from segment to segment (e.g CalibrationParams, LiveParameters). The `custom_params` is dictionary used to prepopulate `Params` with arbitrary values. The `get_custom_params_from_lr` helper is provided to fetch meaningful values from log files. + +```py +from selfdrive.test.process_replay import get_custom_params_from_lr + +previous_segment_lr = LogReader(...) +current_segment_lr = LogReader(...) + +custom_params = get_custom_params_from_lr(previous_segment_lr, 'last') + +output_logs = replay_process_with_name('calibrationd', lr, custom_params=custom_params) +``` + +Replaying processes that use VisionIPC (e.g. modeld, dmonitoringmodeld) require additional `frs` dictionary with camera states as keys and `FrameReader` objects as values. + +```py +from tools.lib.framereader import FrameReader + +frs = { + 'roadCameraState': FrameReader(...), + 'wideRoadCameraState': FrameReader(...), + 'driverCameraState': FrameReader(...), +} + +output_logs = replay_process_with_name(['modeld', 'dmonitoringmodeld'], lr, frs=frs) +``` \ No newline at end of file diff --git a/selfdrive/test/process_replay/__init__.py b/selfdrive/test/process_replay/__init__.py index 6667deaa2d4485..a9dbc71830cc6c 100644 --- a/selfdrive/test/process_replay/__init__.py +++ b/selfdrive/test/process_replay/__init__.py @@ -1 +1 @@ -from selfdrive.test.process_replay.process_replay import CONFIGS, get_process_config, replay_process, replay_process_with_name # noqa: F401 +from selfdrive.test.process_replay.process_replay import CONFIGS, get_process_config, get_custom_params_from_lr, replay_process, replay_process_with_name # noqa: F401 diff --git a/selfdrive/test/process_replay/helpers.py b/selfdrive/test/process_replay/helpers.py index 8571f36c36f7cf..1b30eca103e43f 100644 --- a/selfdrive/test/process_replay/helpers.py +++ b/selfdrive/test/process_replay/helpers.py @@ -2,12 +2,15 @@ import shutil import uuid +from typing import List, Optional + from common.params import Params class OpenpilotPrefix(object): - def __init__(self, prefix: str = None) -> None: + def __init__(self, prefix: str = None, clean_dirs_on_exit: bool = True): self.prefix = prefix if prefix else str(uuid.uuid4()) self.msgq_path = os.path.join('/dev/shm', self.prefix) + self.clean_dirs_on_exit = clean_dirs_on_exit def __enter__(self): os.environ['OPENPILOT_PREFIX'] = self.prefix @@ -17,10 +20,28 @@ def __enter__(self): pass def __exit__(self, exc_type, exc_obj, exc_tb): + if self.clean_dirs_on_exit: + self.clean_dirs() + del os.environ['OPENPILOT_PREFIX'] + return False + + def clean_dirs(self): symlink_path = Params().get_param_path() if os.path.exists(symlink_path): shutil.rmtree(os.path.realpath(symlink_path), ignore_errors=True) os.remove(symlink_path) shutil.rmtree(self.msgq_path, ignore_errors=True) - del os.environ['OPENPILOT_PREFIX'] - return False + + +class DummySocket: + def __init__(self): + self.data: List[bytes] = [] + + def receive(self, non_blocking: bool = False) -> Optional[bytes]: + if non_blocking: + return None + + return self.data.pop() + + def send(self, data: bytes): + self.data.append(data) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9d1efe255d34b1..6c49ba344905a6 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -1,11 +1,14 @@ #!/usr/bin/env python3 import os import time +import copy +import json +import heapq import signal import platform from collections import OrderedDict from dataclasses import dataclass, field -from typing import Dict, List, Optional, Callable, Union, Any +from typing import Dict, List, Optional, Callable, Union, Any, Iterable, Tuple from tqdm import tqdm import capnp @@ -19,18 +22,17 @@ from panda.python import ALTERNATIVE_EXPERIENCE from selfdrive.car.car_helpers import get_car, interfaces from selfdrive.manager.process_config import managed_processes -from selfdrive.test.process_replay.helpers import OpenpilotPrefix +from selfdrive.test.process_replay.helpers import OpenpilotPrefix, DummySocket from selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams from selfdrive.test.process_replay.migration import migrate_all from tools.lib.logreader import LogReader # Numpy gives different results based on CPU features after version 19 NUMPY_TOLERANCE = 1e-7 -CI = "CI" in os.environ -TIMEOUT = 15 PROC_REPLAY_DIR = os.path.dirname(os.path.abspath(__file__)) FAKEDATA = os.path.join(PROC_REPLAY_DIR, "fakedata/") + class ReplayContext: def __init__(self, cfg): self.proc_name = cfg.proc_name @@ -40,6 +42,14 @@ def __init__(self, cfg): assert(len(self.pubs) != 0 or self.main_pub is not None) def __enter__(self): + self.open() + + return self + + def __exit__(self, exc_type, exc_obj, exc_tb): + self.close() + + def open(self): messaging.toggle_fake_events(True) messaging.set_fake_prefix(self.proc_name) @@ -50,9 +60,7 @@ def __enter__(self): else: self.events = {self.main_pub: messaging.fake_event_handle(self.main_pub, enable=True)} - return self - - def __exit__(self, exc_type, exc_obj, exc_tb): + def close(self): del self.events messaging.toggle_fake_events(False) @@ -101,8 +109,7 @@ class ProcessConfig: init_callback: Optional[Callable] = None should_recv_callback: Optional[Callable] = None tolerance: Optional[float] = None - environ: Dict[str, str] = field(default_factory=dict) - subtest_name: str = "" + processing_time: float = 0.001 field_tolerances: Dict[str, float] = field(default_factory=dict) timeout: int = 30 simulation: bool = True @@ -112,18 +119,151 @@ class ProcessConfig: ignore_alive_pubs: List[str] = field(default_factory=list) -class DummySocket: - def __init__(self): - self.data = [] +class ProcessContainer: + def __init__(self, cfg: ProcessConfig): + self.prefix = OpenpilotPrefix(clean_dirs_on_exit=False) + self.cfg = copy.deepcopy(cfg) + self.process = managed_processes[cfg.proc_name] + self.msg_queue: List[capnp._DynamicStructReader] = [] + self.cnt = 0 + self.pm: Optional[messaging.PubMaster] = None + self.sockets: Optional[List[messaging.SubSocket]] = None + self.rc: Optional[ReplayContext] = None + self.vipc_server: Optional[VisionIpcServer] = None + + @property + def has_empty_queue(self) -> bool: + return len(self.msg_queue) == 0 + + @property + def pubs(self) -> List[str]: + return self.cfg.pubs + + @property + def subs(self) -> List[str]: + return self.cfg.subs + + def _setup_env(self, params_config: Dict[str, Any], environ_config: Dict[str, Any]): + for k, v in environ_config.items(): + if len(v) != 0: + os.environ[k] = v + elif k in os.environ: + del os.environ[k] + + os.environ["PROC_NAME"] = self.cfg.proc_name + if self.cfg.simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] + + params = Params() + for k, v in params_config.items(): + if isinstance(v, bool): + params.put_bool(k, v) + else: + params.put(k, v) + + def _setup_vision_ipc(self, all_msgs): + assert len(self.cfg.vision_pubs) != 0 + + device_type = next(msg.initData.deviceType for msg in all_msgs if msg.which() == "initData") + + vipc_server = VisionIpcServer("camerad") + streams_metas = available_streams(all_msgs) + for meta in streams_metas: + if meta.camera_state in self.cfg.vision_pubs: + vipc_server.create_buffers(meta.stream, 2, False, *meta.frame_sizes[device_type]) + vipc_server.start_listener() + + self.vipc_server = vipc_server + + def start( + self, params_config: Dict[str, Any], environ_config: Dict[str, Any], + all_msgs: Union[LogReader, List[capnp._DynamicStructReader]], fingerprint: Optional[str] + ): + with self.prefix: + self._setup_env(params_config, environ_config) + + if self.cfg.config_callback is not None: + params = Params() + self.cfg.config_callback(params, self.cfg, all_msgs) + + self.rc = ReplayContext(self.cfg) + self.rc.open() + + self.pm = messaging.PubMaster(self.cfg.pubs) + self.sockets = [messaging.sub_sock(s, timeout=100) for s in self.cfg.subs] + + if len(self.cfg.vision_pubs) != 0: + self._setup_vision_ipc(all_msgs) + assert self.vipc_server is not None + + self.process.prepare() + self.process.start() + + if self.cfg.init_callback is not None: + self.cfg.init_callback(self.rc, self.pm, all_msgs, fingerprint) + + # wait for process to startup + with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(self.cfg.proc_name)}"): + while not all(self.pm.all_readers_updated(s) for s in self.cfg.pubs if s not in self.cfg.ignore_alive_pubs): + time.sleep(0) + + def stop(self): + with self.prefix: + self.process.signal(signal.SIGKILL) + self.process.stop() + self.rc.close() + self.prefix.clean_dirs() + + def run_step(self, msg: capnp._DynamicStructReader, frs: Optional[Dict[str, Any]]) -> List[capnp._DynamicStructReader]: + assert self.rc and self.pm and self.sockets and self.process.proc + + output_msgs = [] + with self.prefix, Timeout(self.cfg.timeout, error_msg=f"timed out testing process {repr(self.cfg.proc_name)}"): + end_of_cycle = True + if self.cfg.should_recv_callback is not None: + end_of_cycle = self.cfg.should_recv_callback(msg, self.cfg, self.cnt) + + self.msg_queue.append(msg) + if end_of_cycle: + self.rc.wait_for_recv_called() + + # call recv to let sub-sockets reconnect, after we know the process is ready + if self.cnt == 0: + for s in self.sockets: + messaging.recv_one_or_none(s) + + # empty recv on drained pub indicates the end of messages, only do that if there're any + trigger_empty_recv = False + if self.cfg.main_pub and self.cfg.main_pub_drained: + trigger_empty_recv = next((True for m in self.msg_queue if m.which() == self.cfg.main_pub), False) + + for m in self.msg_queue: + self.pm.send(m.which(), m.as_builder()) + # send frames if needed + if self.vipc_server is not None and m.which() in self.cfg.vision_pubs: + camera_state = getattr(m, m.which()) + camera_meta = meta_from_camera_state(m.which()) + assert frs is not None + img = frs[m.which()].get(camera_state.frameId, pix_fmt="nv12")[0] + self.vipc_server.send(camera_meta.stream, img.flatten().tobytes(), + camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) + self.msg_queue = [] - def receive(self, non_blocking=False): - if non_blocking: - return None + self.rc.unlock_sockets() + self.rc.wait_for_next_recv(trigger_empty_recv) - return self.data.pop() + for socket in self.sockets: + ms = messaging.drain_sock(socket) + for m in ms: + m = m.as_builder() + m.logMonoTime = msg.logMonoTime + int(self.cfg.processing_time * 1e9) + output_msgs.append(m.as_reader()) + self.cnt += 1 + assert self.process.proc.is_alive() - def send(self, data): - self.data.append(data) + return output_msgs def controlsd_fingerprint_callback(rc, pm, msgs, fingerprint): @@ -242,21 +382,38 @@ def __call__(self, msg, cfg, frame): if frame % max(1, int(service_list[msg.which()].frequency / service_list[s].frequency)) == 0 ] return bool(len(resp_sockets)) + + +def controlsd_config_callback(params, cfg, lr): + controlsState = None + initialized = False + for msg in lr: + if msg.which() == "controlsState": + controlsState = msg.controlsState + if initialized: + break + elif msg.which() == "carEvents": + initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents] + assert controlsState is not None and initialized, "controlsState never initialized" + params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) -def laikad_config_pubsub_callback(params, cfg): + +def laikad_config_pubsub_callback(params, cfg, lr): ublox = params.get_bool("UbloxAvailable") main_key = "ubloxGnss" if ublox else "qcomGnss" sub_keys = ({"qcomGnss", } if ublox else {"ubloxGnss", }) - return set(cfg.pubs) - sub_keys, main_key, True + cfg.pubs = set(cfg.pubs) - sub_keys + cfg.main_pub = main_key + cfg.main_pub_drained = True -def locationd_config_pubsub_callback(params, cfg): +def locationd_config_pubsub_callback(params, cfg, lr): ublox = params.get_bool("UbloxAvailable") sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", }) - return set(cfg.pubs) - sub_keys, None, False + cfg.pubs = set(cfg.pubs) - sub_keys CONFIGS = [ @@ -270,9 +427,11 @@ def locationd_config_pubsub_callback(params, cfg): ], subs=["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], + config_callback=controlsd_config_callback, init_callback=controlsd_fingerprint_callback, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, + processing_time=0.004, main_pub="can", ), ProcessConfig( @@ -327,6 +486,7 @@ def locationd_config_pubsub_callback(params, cfg): init_callback=get_car_params_callback, should_recv_callback=FrequencyBasedRcvCallback("liveLocationKalman"), tolerance=NUMPY_TOLERANCE, + processing_time=0.004, ), ProcessConfig( proc_name="ubloxd", @@ -341,8 +501,9 @@ def locationd_config_pubsub_callback(params, cfg): ignore=["logMonoTime"], config_callback=laikad_config_pubsub_callback, tolerance=NUMPY_TOLERANCE, + processing_time=0.002, timeout=60*10, # first messages are blocked on internet assistance - main_pub="ubloxGnss", # config_callback will switch this to qcom if needed + main_pub="ubloxGnss", # config_callback will switch this to qcom if needed ), ProcessConfig( proc_name="torqued", @@ -360,6 +521,7 @@ def locationd_config_pubsub_callback(params, cfg): ignore=["logMonoTime", "modelV2.frameDropPerc", "modelV2.modelExecutionTime"], should_recv_callback=ModeldCameraSyncRcvCallback(), tolerance=NUMPY_TOLERANCE, + processing_time=0.020, main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("roadCameraState").stream), main_pub_drained=False, vision_pubs=["roadCameraState", "wideRoadCameraState"], @@ -372,6 +534,7 @@ def locationd_config_pubsub_callback(params, cfg): ignore=["logMonoTime", "driverStateV2.modelExecutionTime", "driverStateV2.dspExecutionTime"], should_recv_callback=dmonitoringmodeld_rcv_callback, tolerance=NUMPY_TOLERANCE, + processing_time=0.020, main_pub=vipc_get_endpoint_name("camerad", meta_from_camera_state("driverCameraState").stream), main_pub_drained=False, vision_pubs=["driverCameraState"], @@ -380,27 +543,73 @@ def locationd_config_pubsub_callback(params, cfg): ] -def get_process_config(name): +def get_process_config(name: str) -> ProcessConfig: try: - return next(c for c in CONFIGS if c.proc_name == name) + return copy.deepcopy(next(c for c in CONFIGS if c.proc_name == name)) except StopIteration as ex: raise Exception(f"Cannot find process config with name: {name}") from ex -def replay_process_with_name(name, lr, *args, **kwargs): - cfg = get_process_config(name) - return replay_process(cfg, lr, *args, **kwargs) +def get_custom_params_from_lr(lr: Union[LogReader, List[capnp._DynamicStructReader]], initial_state: str = "first") -> Dict[str, Any]: + """ + Use this to get custom params dict based on provided logs. + Useful when replaying following processes: calibrationd, paramsd, torqued + The params may be based on first or last message of given type (carParams, liveCalibration, liveParameters, liveTorqueParameters) in the logs. + """ + + car_params = [m for m in lr if m.which() == "carParams"] + live_calibration = [m for m in lr if m.which() == "liveCalibration"] + live_parameters = [m for m in lr if m.which() == "liveParameters"] + live_torque_parameters = [m for m in lr if m.which() == "liveTorqueParameters"] + + assert initial_state in ["first", "last"] + msg_index = 0 if initial_state == "first" else -1 + + assert len(car_params) > 0, "carParams required for initial state of liveParameters and liveTorqueCarParams" + CP = car_params[msg_index].carParams + custom_params = {} + if len(live_calibration) > 0: + custom_params["CalibrationParams"] = live_calibration[msg_index].as_builder().to_bytes() + if len(live_parameters) > 0: + lp_dict = live_parameters[msg_index].to_dict() + lp_dict["carFingerprint"] = CP.carFingerprint + custom_params["LiveParameters"] = json.dumps(lp_dict) + if len(live_torque_parameters) > 0: + custom_params["LiveTorqueCarParams"] = CP.as_builder().to_bytes() + custom_params["LiveTorqueParameters"] = live_torque_parameters[msg_index].as_builder().to_bytes() -def replay_process(cfg, lr, frs=None, fingerprint=None, return_all_logs=False, custom_params=None, disable_progress=False): - all_msgs = migrate_all(lr, old_logtime=True, camera_states=len(cfg.vision_pubs) != 0) - process_logs = _replay_single_process(cfg, all_msgs, frs, fingerprint, custom_params, disable_progress) + return custom_params + + +def replay_process_with_name(name: Union[str, Iterable[str]], lr: Union[LogReader, List[capnp._DynamicStructReader]], *args, **kwargs) -> List[capnp._DynamicStructReader]: + if isinstance(name, str): + cfgs = [get_process_config(name)] + elif isinstance(name, Iterable): + cfgs = [get_process_config(n) for n in name] + else: + raise ValueError("name must be str or collections of strings") + + return replay_process(cfgs, lr, *args, **kwargs) + + +def replay_process( + cfg: Union[ProcessConfig, Iterable[ProcessConfig]], lr: Union[LogReader, List[capnp._DynamicStructReader]], frs: Optional[Dict[str, Any]] = None, + fingerprint: Optional[str] = None, return_all_logs: bool = False, custom_params: Optional[Dict[str, Any]] = None, disable_progress: bool = False +) -> List[capnp._DynamicStructReader]: + if isinstance(cfg, Iterable): + cfgs = list(cfg) + else: + cfgs = [cfg] + + all_msgs = migrate_all(lr, old_logtime=True, camera_states=any(len(cfg.vision_pubs) != 0 for cfg in cfgs)) + process_logs = _replay_multi_process(cfgs, all_msgs, frs, fingerprint, custom_params, disable_progress) if return_all_logs: - keys = set(cfg.subs) + keys = {m.which() for m in process_logs} modified_logs = [m for m in all_msgs if m.which() not in keys] modified_logs.extend(process_logs) - modified_logs.sort(key=lambda m: m.logMonoTime) + modified_logs.sort(key=lambda m: int(m.logMonoTime)) log_msgs = modified_logs else: log_msgs = process_logs @@ -408,202 +617,131 @@ def replay_process(cfg, lr, frs=None, fingerprint=None, return_all_logs=False, c return log_msgs -def _replay_single_process( - cfg: ProcessConfig, lr: Union[LogReader, List[capnp._DynamicStructReader]], frs: Optional[Dict[str, Any]], +def _replay_multi_process( + cfgs: List[ProcessConfig], lr: Union[LogReader, List[capnp._DynamicStructReader]], frs: Optional[Dict[str, Any]], fingerprint: Optional[str], custom_params: Optional[Dict[str, Any]], disable_progress: bool -): - with OpenpilotPrefix(): - controlsState = None - initialized = False - if cfg.proc_name == "controlsd": - for msg in lr: - if msg.which() == "controlsState": - controlsState = msg.controlsState - if initialized: - break - elif msg.which() == "carEvents": - initialized = car.CarEvent.EventName.controlsInitializing not in [e.name for e in msg.carEvents] - - assert controlsState is not None and initialized, "controlsState never initialized" - - if fingerprint is not None: - setup_env(cfg=cfg, controlsState=controlsState, lr=lr, fingerprint=fingerprint, custom_params=custom_params) - else: - CP = next((m.carParams for m in lr if m.which() == "carParams"), None) - assert CP is not None or "carParams" not in cfg.pubs, "carParams are missing and process needs it" - setup_env(cfg=cfg, CP=CP, controlsState=controlsState, lr=lr, custom_params=custom_params) - - if cfg.config_callback is not None: - params = Params() - cfg.pubs, cfg.main_pub, cfg.main_pub_drained = cfg.config_callback(params, cfg) - - all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) - pub_msgs = [msg for msg in all_msgs if msg.which() in set(cfg.pubs)] - - with ReplayContext(cfg) as rc: - pm = messaging.PubMaster(cfg.pubs) - sockets = {s: messaging.sub_sock(s, timeout=100) for s in cfg.subs} - - vipc_server = None - if len(cfg.vision_pubs) != 0: - assert frs is not None, "frs must be provided when replaying process using vision streams" - assert all(meta_from_camera_state(st) is not None for st in cfg.vision_pubs),f"undefined vision stream spotted, probably misconfigured process: {cfg.vision_pubs}" - assert all(st in frs for st in cfg.vision_pubs), f"frs for this process must contain following vision streams: {cfg.vision_pubs}" - vipc_server = setup_vision_ipc(cfg, lr) - - managed_processes[cfg.proc_name].prepare() - managed_processes[cfg.proc_name].start() - - if cfg.init_callback is not None: - cfg.init_callback(rc, pm, all_msgs, fingerprint) - - log_msgs, msg_queue = [], [] - try: - # Wait for process to startup - with Timeout(10, error_msg=f"timed out waiting for process to start: {repr(cfg.proc_name)}"): - while not all(pm.all_readers_updated(s) for s in cfg.pubs if s not in cfg.ignore_alive_pubs): - time.sleep(0) - - # Do the replay - cnt = 0 - for msg in tqdm(pub_msgs, disable=disable_progress): - with Timeout(cfg.timeout, error_msg=f"timed out testing process {repr(cfg.proc_name)}, {cnt}/{len(pub_msgs)} msgs done"): - resp_sockets, end_of_cycle = cfg.subs, True - if cfg.should_recv_callback is not None: - end_of_cycle = cfg.should_recv_callback(msg, cfg, cnt) - - msg_queue.append(msg) - if end_of_cycle: - rc.wait_for_recv_called() - - # call recv to let sub-sockets reconnect, after we know the process is ready - if cnt == 0: - for s in sockets.values(): - messaging.recv_one_or_none(s) - - # empty recv on drained pub indicates the end of messages, only do that if there're any - trigger_empty_recv = False - if cfg.main_pub and cfg.main_pub_drained: - trigger_empty_recv = next((True for m in msg_queue if m.which() == cfg.main_pub), False) - - for m in msg_queue: - pm.send(m.which(), m.as_builder()) - # send frames if needed - if vipc_server is not None and m.which() in cfg.vision_pubs: - camera_state = getattr(m, m.which()) - camera_meta = meta_from_camera_state(m.which()) - assert frs is not None - img = frs[m.which()].get(camera_state.frameId, pix_fmt="nv12")[0] - vipc_server.send(camera_meta.stream, img.flatten().tobytes(), - camera_state.frameId, camera_state.timestampSof, camera_state.timestampEof) - msg_queue = [] - - rc.unlock_sockets() - rc.wait_for_next_recv(trigger_empty_recv) +) -> List[capnp._DynamicStructReader]: + if fingerprint is not None: + params_config = generate_params_config(lr=lr, fingerprint=fingerprint, custom_params=custom_params) + env_config = generate_environ_config(fingerprint=fingerprint) + else: + CP = next((m.carParams for m in lr if m.which() == "carParams"), None) + params_config = generate_params_config(lr=lr, CP=CP, custom_params=custom_params) + env_config = generate_environ_config(CP=CP) + + # validate frs and vision pubs + for cfg in cfgs: + if len(cfg.vision_pubs) == 0: + continue + + assert frs is not None, "frs must be provided when replaying process using vision streams" + assert all(meta_from_camera_state(st) is not None for st in cfg.vision_pubs),f"undefined vision stream spotted, probably misconfigured process: {cfg.vision_pubs}" + assert all(st in frs for st in cfg.vision_pubs), f"frs for this process must contain following vision streams: {cfg.vision_pubs}" + + all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime) + log_msgs = [] + try: + containers = [] + for cfg in cfgs: + container = ProcessContainer(cfg) + container.start(params_config, env_config, all_msgs, fingerprint) + containers.append(container) + + all_pubs = set([pub for container in containers for pub in container.pubs]) + all_subs = set([sub for container in containers for sub in container.subs]) + lr_pubs = all_pubs - all_subs + pubs_to_containers = {pub: [container for container in containers if pub in container.pubs] for pub in all_pubs} + + pub_msgs = [msg for msg in all_msgs if msg.which() in lr_pubs] + # external queue for messages taken from logs; internal queue for messages generated by processes, which will be republished + external_pub_queue: List[capnp._DynamicStructReader] = pub_msgs.copy() + internal_pub_queue: List[capnp._DynamicStructReader] = [] + # heap for maintaining the order of messages generated by processes, where each element: (logMonoTime, index in internal_pub_queue) + internal_pub_index_heap: List[Tuple[int, int]] = [] + + pbar = tqdm(total=len(external_pub_queue), disable=disable_progress) + while len(external_pub_queue) != 0 or (len(internal_pub_index_heap) != 0 and not all(c.has_empty_queue for c in containers)): + if len(internal_pub_index_heap) == 0 or (len(external_pub_queue) != 0 and external_pub_queue[0].logMonoTime < internal_pub_index_heap[0][0]): + msg = external_pub_queue.pop(0) + pbar.update(1) + else: + _, index = heapq.heappop(internal_pub_index_heap) + msg = internal_pub_queue[index] + + target_containers = pubs_to_containers[msg.which()] + for container in target_containers: + output_msgs = container.run_step(msg, frs) + for m in output_msgs: + if m.which() in all_pubs: + internal_pub_queue.append(m) + heapq.heappush(internal_pub_index_heap, (m.logMonoTime, len(internal_pub_queue) - 1)) + log_msgs.extend(output_msgs) + finally: + for container in containers: + container.stop() - for s in resp_sockets: - ms = messaging.drain_sock(sockets[s]) - for m in ms: - m = m.as_builder() - m.logMonoTime = msg.logMonoTime - log_msgs.append(m.as_reader()) - cnt += 1 - proc = managed_processes[cfg.proc_name].proc - assert(proc and proc.is_alive()) - finally: - managed_processes[cfg.proc_name].signal(signal.SIGKILL) - managed_processes[cfg.proc_name].stop() + return log_msgs - return log_msgs +def generate_params_config(lr=None, CP=None, fingerprint=None, custom_params=None) -> Dict[str, Any]: + params_dict = { + "OpenpilotEnabledToggle": True, + "Passive": False, + "DisengageOnAccelerator": True, + "DisableLogging": False, + } + + if custom_params is not None: + params_dict.update(custom_params) + if lr is not None: + has_ublox = any(msg.which() == "ubloxGnss" for msg in lr) + params_dict["UbloxAvailable"] = has_ublox + is_rhd = next((msg.driverMonitoringState.isRHD for msg in lr if msg.which() == "driverMonitoringState"), False) + params_dict["IsRhdDetected"] = is_rhd -def setup_vision_ipc(cfg, lr): - assert len(cfg.vision_pubs) != 0 + if CP is not None: + if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: + params_dict["DisengageOnAccelerator"] = False - device_type = next(msg.initData.deviceType for msg in lr if msg.which() == "initData") + if fingerprint is None: + if CP.fingerprintSource == "fw": + params_dict["CarParamsCache"] = CP.as_builder().to_bytes() - vipc_server = VisionIpcServer("camerad") - streams_metas = available_streams(lr) - for meta in streams_metas: - if meta.camera_state in cfg.vision_pubs: - vipc_server.create_buffers(meta.stream, 2, False, *meta.frame_sizes[device_type]) - vipc_server.start_listener() + if CP.openpilotLongitudinalControl: + params_dict["ExperimentalLongitudinalEnabled"] = True - return vipc_server + return params_dict -def setup_env(cfg=None, CP=None, controlsState=None, lr=None, fingerprint=None, custom_params=None, log_dir=None): +def generate_environ_config(CP=None, fingerprint=None, log_dir=None) -> Dict[str, Any]: + environ_dict = {} if platform.system() != "Darwin": - os.environ["PARAMS_ROOT"] = "/dev/shm/params" + environ_dict["PARAMS_ROOT"] = "/dev/shm/params" if log_dir is not None: - os.environ["LOG_ROOT"] = log_dir + environ_dict["LOG_ROOT"] = log_dir - params = Params() - params.clear_all() - params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("Passive", False) - params.put_bool("DisengageOnAccelerator", True) - params.put_bool("DisableLogging", False) - if custom_params is not None: - for k, v in custom_params.items(): - if type(v) == bool: - params.put_bool(k, v) - else: - params.put(k, v) + environ_dict["NO_RADAR_SLEEP"] = "1" + environ_dict["REPLAY"] = "1" - os.environ["NO_RADAR_SLEEP"] = "1" - os.environ["REPLAY"] = "1" - if fingerprint is not None: - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = fingerprint + # Regen or python process + if CP is not None and fingerprint is None: + if CP.fingerprintSource == "fw": + environ_dict['SKIP_FW_QUERY'] = "" + environ_dict['FINGERPRINT'] = "" + else: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = CP.carFingerprint + elif fingerprint is not None: + environ_dict['SKIP_FW_QUERY'] = "1" + environ_dict['FINGERPRINT'] = fingerprint else: - os.environ["SKIP_FW_QUERY"] = "" - os.environ["FINGERPRINT"] = "" - - if lr is not None: - services = {m.which() for m in lr} - params.put_bool("UbloxAvailable", "ubloxGnss" in services) + environ_dict["SKIP_FW_QUERY"] = "" + environ_dict["FINGERPRINT"] = "" - if cfg is not None: - # Clear all custom processConfig environment variables - for config in CONFIGS: - for k, _ in config.environ.items(): - if k in os.environ: - del os.environ[k] - - os.environ.update(cfg.environ) - os.environ['PROC_NAME'] = cfg.proc_name - - if cfg is not None and cfg.simulation: - os.environ["SIMULATION"] = "1" - elif "SIMULATION" in os.environ: - del os.environ["SIMULATION"] - - # Initialize controlsd with a controlsState packet - if controlsState is not None: - params.put("ReplayControlsState", controlsState.as_builder().to_bytes()) - else: - params.remove("ReplayControlsState") - - # Regen or python process - if CP is not None: - if CP.alternativeExperience == ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS: - params.put_bool("DisengageOnAccelerator", False) - - if fingerprint is None: - if CP.fingerprintSource == "fw": - params.put("CarParamsCache", CP.as_builder().to_bytes()) - os.environ['SKIP_FW_QUERY'] = "" - os.environ['FINGERPRINT'] = "" - else: - os.environ['SKIP_FW_QUERY'] = "1" - os.environ['FINGERPRINT'] = CP.carFingerprint - - if CP.openpilotLongitudinalControl: - params.put_bool("ExperimentalLongitudinalEnabled", True) + return environ_dict -def check_openpilot_enabled(msgs): +def check_openpilot_enabled(msgs: Union[LogReader, List[capnp._DynamicStructReader]]) -> bool: cur_enabled_count = 0 max_enabled_count = 0 for msg in msgs: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3a97fe1466f33c..92331fa317ee16 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -219a815856d8984cb4933d83db9a15bf7cd09f16 +af03f2ddbc5244f9a885445c0452987a4bb81302 diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 59f0fe703ff2a5..5afc55c4063a3d 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -1,351 +1,113 @@ #!/usr/bin/env python3 -import bz2 import os -import time -import multiprocessing import argparse -from tqdm import tqdm -# run DM procs -os.environ["USE_WEBCAM"] = "1" +import time +import capnp + +from typing import Union, Iterable, Optional, List, Any, Dict, Tuple -import cereal.messaging as messaging -from cereal import car -from cereal.services import service_list -from cereal.visionipc import VisionIpcServer, VisionStreamType -from common.params import Params -from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot -from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size, tici_e_frame_size -from panda.python import Panda -from selfdrive.car.toyota.values import EPS_SCALE -from selfdrive.manager.process import ensure_running -from selfdrive.manager.process_config import managed_processes -from selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, setup_env, check_openpilot_enabled -from selfdrive.test.process_replay.migration import migrate_all +from selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, replay_process, get_process_config, check_openpilot_enabled, get_custom_params_from_lr from selfdrive.test.update_ci_routes import upload_route from tools.lib.route import Route from tools.lib.framereader import FrameReader from tools.lib.logreader import LogReader +from tools.lib.helpers import save_log -def replay_panda_states(s, msgs): - pm = messaging.PubMaster([s, 'peripheralState']) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']] - - # TODO: safety param migration should be handled automatically - safety_param_migration = { - "TOYOTA PRIUS 2017": EPS_SCALE["TOYOTA PRIUS 2017"] | Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL, - "TOYOTA RAV4 2017": EPS_SCALE["TOYOTA RAV4 2017"] | Panda.FLAG_TOYOTA_ALT_BRAKE, - "KIA EV6 2022": Panda.FLAG_HYUNDAI_EV_GAS | Panda.FLAG_HYUNDAI_CANFD_HDA2, - } - - # Migrate safety param base on carState - cp = [m for m in msgs if m.which() == 'carParams'][0].carParams - if cp.carFingerprint in safety_param_migration: - safety_param = safety_param_migration[cp.carFingerprint] - elif len(cp.safetyConfigs): - safety_param = cp.safetyConfigs[0].safetyParam - if cp.safetyConfigs[0].safetyParamDEPRECATED != 0: - safety_param = cp.safetyConfigs[0].safetyParamDEPRECATED - else: - safety_param = cp.safetyParamDEPRECATED - - while True: - for m in smsgs: - if m.which() == 'pandaStateDEPRECATED': - new_m = messaging.new_message('pandaStates', 1) - new_m.pandaStates[0] = m.pandaStateDEPRECATED - new_m.pandaStates[0].safetyParam = safety_param - pm.send(s, new_m) - else: - new_m = m.as_builder() - new_m.pandaStates[-1].safetyParam = safety_param - new_m.logMonoTime = int(sec_since_boot() * 1e9) - pm.send(s, new_m) - - new_m = messaging.new_message('peripheralState') - pm.send('peripheralState', new_m) - - rk.keep_time() - - -def replay_manager_state(s, msgs): - pm = messaging.PubMaster([s, ]) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - - while True: - new_m = messaging.new_message('managerState') - new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] - pm.send(s, new_m) - rk.keep_time() - - -def replay_device_state(s, msgs): - pm = messaging.PubMaster([s, ]) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - smsgs = [m for m in msgs if m.which() == s] - while True: - for m in smsgs: - new_m = m.as_builder() - new_m.logMonoTime = int(sec_since_boot() * 1e9) - new_m.deviceState.freeSpacePercent = 50 - new_m.deviceState.memoryUsagePercent = 50 - pm.send(s, new_m) - rk.keep_time() - - -def replay_sensor_event(s, msgs): - pm = messaging.PubMaster([s, ]) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - smsgs = [m for m in msgs if m.which() == s] - while True: - for m in smsgs: - m = m.as_builder() - m.logMonoTime = int(sec_since_boot() * 1e9) - getattr(m, m.which()).timestamp = m.logMonoTime - pm.send(m.which(), m) - rk.keep_time() - - -def replay_service(s, msgs): - pm = messaging.PubMaster([s, ]) - rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) - smsgs = [m for m in msgs if m.which() == s] - while True: - for m in smsgs: - new_m = m.as_builder() - new_m.logMonoTime = int(sec_since_boot() * 1e9) - pm.send(s, new_m) - rk.keep_time() - - -def replay_cameras(lr, frs, disable_tqdm=False): - eon_cameras = [ - ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), - ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), - ] - tici_cameras = [ - ("roadCameraState", DT_MDL, tici_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), - ("wideRoadCameraState", DT_MDL, tici_e_frame_size, VisionStreamType.VISION_STREAM_WIDE_ROAD), - ("driverCameraState", DT_DMON, tici_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), - ] - - def replay_camera(s, stream, dt, vipc_server, frames, size): - services = [(s, stream)] - pm = messaging.PubMaster([s for s, _ in services]) - rk = Ratekeeper(1 / dt, print_delay_threshold=None) - - img = b"\x00" * int(size[0] * size[1] * 3 / 2) - while True: - if frames is not None: - img = frames[rk.frame % len(frames)] - - rk.keep_time() - for s, stream in services: - m = messaging.new_message(s) - msg = getattr(m, s) - msg.frameId = rk.frame - msg.timestampSof = m.logMonoTime - msg.timestampEof = m.logMonoTime - pm.send(s, m) - - vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof) - - init_data = [m for m in lr if m.which() == 'initData'][0] - cameras = tici_cameras if (init_data.initData.deviceType in ['tici', 'tizi']) else eon_cameras - - # init vipc server and cameras - p = [] - vs = VisionIpcServer("camerad") - for (s, dt, size, stream) in cameras: - fr = frs.get(s, None) - - frames = None - if fr is not None: - print(f"Decompressing frames {s}") - frames = [] - for i in tqdm(range(fr.frame_count), disable=disable_tqdm): - img = fr.get(i, pix_fmt='nv12')[0] - frames.append(img.flatten().tobytes()) - - vs.create_buffers(stream, 40, False, size[0], size[1]) - p.append(multiprocessing.Process(target=replay_camera, - args=(s, stream, dt, vs, frames, size))) - - vs.start_listener() - return vs, p - - -def regen_segment(lr, frs=None, daemons="all", outdir=FAKEDATA, disable_tqdm=False): +def regen_segment( + lr: Union[LogReader, List[capnp._DynamicStructReader]], frs: Optional[Dict[str, Any]] = None, + daemons: Union[str, Iterable[str]] = "all", disable_tqdm: bool = False +) -> List[capnp._DynamicStructReader]: if not isinstance(daemons, str) and not hasattr(daemons, "__iter__"): raise ValueError("whitelist_proc must be a string or iterable") - lr = migrate_all(lr) - if frs is None: - frs = dict() - - # Get and setup initial state - CP = [m for m in lr if m.which() == 'carParams'][0].carParams - controlsState = [m for m in lr if m.which() == 'controlsState'][0].controlsState - liveCalibration = [m for m in lr if m.which() == 'liveCalibration'][0] - - setup_env(CP=CP, controlsState=controlsState, log_dir=outdir) - - params = Params() - params.put("CalibrationParams", liveCalibration.as_builder().to_bytes()) - - vs, cam_procs = replay_cameras(lr, frs, disable_tqdm=disable_tqdm) - fake_daemons = { - 'sensord': [ - multiprocessing.Process(target=replay_sensor_event, args=('accelerometer', lr)), - multiprocessing.Process(target=replay_sensor_event, args=('gyroscope', lr)), - multiprocessing.Process(target=replay_sensor_event, args=('magnetometer', lr)), - ], - 'pandad': [ - multiprocessing.Process(target=replay_service, args=('can', lr)), - multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), - multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), - ], - 'manager': [ - multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), - ], - 'thermald': [ - multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), - ], - 'rawgpsd': [ - multiprocessing.Process(target=replay_service, args=('qcomGnss', lr)), - multiprocessing.Process(target=replay_service, args=('gpsLocation', lr)), - ], - 'camerad': [ - *cam_procs, - ], - } - # TODO add configs for modeld, dmonitoringmodeld - fakeable_daemons = {} - for config in CONFIGS: - processes = [ - multiprocessing.Process(target=replay_service, args=(msg, lr)) - for msg in config.subs - ] - fakeable_daemons[config.proc_name] = processes + all_msgs = sorted(lr, key=lambda m: m.logMonoTime) + custom_params = get_custom_params_from_lr(all_msgs) - additional_fake_daemons = {} if daemons != "all": - additional_fake_daemons = fakeable_daemons if isinstance(daemons, str): raise ValueError(f"Invalid value for daemons: {daemons}") + replayed_processes = [] for d in daemons: - if d in fake_daemons: - raise ValueError(f"Running daemon {d} is not supported!") - - if d in fakeable_daemons: - del additional_fake_daemons[d] - - all_fake_daemons = {**fake_daemons, **additional_fake_daemons} - - try: - # TODO: make first run of onnxruntime CUDA provider fast - if "modeld" not in all_fake_daemons: - managed_processes["modeld"].start() - if "dmonitoringmodeld" not in all_fake_daemons: - managed_processes["dmonitoringmodeld"].start() - time.sleep(5) - - # start procs up - ignore = list(all_fake_daemons.keys()) \ - + ['ui', 'manage_athenad', 'uploader', 'soundd', 'micd', 'navd'] - - print("Faked daemons:", ", ".join(all_fake_daemons.keys())) - print("Running daemons:", ", ".join([key for key in managed_processes.keys() if key not in ignore])) - - ensure_running(managed_processes.values(), started=True, params=Params(), CP=car.CarParams(), not_run=ignore) - for procs in all_fake_daemons.values(): - for p in procs: - p.start() - - for _ in tqdm(range(60), disable=disable_tqdm): - # ensure all procs are running - for d, procs in all_fake_daemons.items(): - for p in procs: - if not p.is_alive(): - raise Exception(f"{d}'s {p.name} died") - time.sleep(1) - finally: - # kill everything - for p in managed_processes.values(): - p.stop() - for procs in all_fake_daemons.values(): - for p in procs: - p.terminate() + cfg = get_process_config(d) + replayed_processes.append(cfg) + else: + replayed_processes = CONFIGS - del vs + print("Replayed processes:", [p.proc_name for p in replayed_processes]) + print("\n\n", "*"*30, "\n\n", sep="") - segment = params.get("CurrentRoute", encoding='utf-8') + "--0" - seg_path = os.path.join(outdir, segment) - # check to make sure openpilot is engaged in the route - if not check_openpilot_enabled(LogReader(os.path.join(seg_path, "rlog"))): - raise Exception(f"Route did not engage for long enough: {segment}") + output_logs = replay_process(replayed_processes, all_msgs, frs, return_all_logs=True, custom_params=custom_params, disable_progress=disable_tqdm) - return seg_path + return output_logs -def regen_and_save(route, sidx, daemons="all", upload=False, use_route_meta=False, outdir=FAKEDATA, disable_tqdm=False): +def setup_data_readers(route: str, sidx: int, use_route_meta: bool) -> Tuple[LogReader, Dict[str, Any]]: if use_route_meta: r = Route(route) lr = LogReader(r.log_paths()[sidx]) - fr = FrameReader(r.camera_paths()[sidx]) - if r.ecamera_paths()[sidx] is not None: - wfr = FrameReader(r.ecamera_paths()[sidx]) - else: - wfr = None + frs = {} + if len(r.camera_paths()) > sidx and r.camera_paths()[sidx] is not None: + frs['roadCameraState'] = FrameReader(r.camera_paths()[sidx]) + if len(r.ecamera_paths()) > sidx and r.ecamera_paths()[sidx] is not None: + frs['wideCameraState'] = FrameReader(r.ecamera_paths()[sidx]) + if len(r.dcamera_paths()) > sidx and r.dcamera_paths()[sidx] is not None: + frs['driverCameraState'] = FrameReader(r.dcamera_paths()[sidx]) else: lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") - fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") - device_type = next(iter(lr)).initData.deviceType - if device_type in ['tici', 'tizi']: - wfr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/ecamera.hevc") - else: - wfr = None - - frs = {'roadCameraState': fr} - if wfr is not None: - frs['wideRoadCameraState'] = wfr - rpath = regen_segment(lr, frs, daemons, outdir=outdir, disable_tqdm=disable_tqdm) + frs = { + 'roadCameraState': FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc"), + 'driverCameraState': FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/dcamera.hevc"), + } + if next((True for m in lr if m.which() == "wideRoadCameraState"), False): + frs['wideRoadCameraState'] = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/ecamera.hevc") - # compress raw rlog before uploading - with open(os.path.join(rpath, "rlog"), "rb") as f: - data = bz2.compress(f.read()) - with open(os.path.join(rpath, "rlog.bz2"), "wb") as f: - f.write(data) - os.remove(os.path.join(rpath, "rlog")) + return lr, frs - lr = LogReader(os.path.join(rpath, 'rlog.bz2')) - controls_state_active = [m.controlsState.active for m in lr if m.which() == 'controlsState'] - assert any(controls_state_active), "Segment did not engage" - relr = os.path.relpath(rpath) +def regen_and_save( + route: str, sidx: int, daemons: Union[str, Iterable[str]] = "all", outdir: str = FAKEDATA, + upload: bool = False, use_route_meta: bool = False, disable_tqdm: bool = False +) -> str: + lr, frs = setup_data_readers(route, sidx, use_route_meta) + output_logs = regen_segment(lr, frs, daemons, disable_tqdm=disable_tqdm) + + log_dir = os.path.join(outdir, time.strftime("%Y-%m-%d--%H-%M-%S--0", time.gmtime())) + rel_log_dir = os.path.relpath(log_dir) + rpath = os.path.join(log_dir, "rlog.bz2") + + os.makedirs(log_dir) + save_log(rpath, output_logs, compress=True) + + print("\n\n", "*"*30, "\n\n", sep="") + print("New route:", rel_log_dir, "\n") + + if not check_openpilot_enabled(output_logs): + raise Exception("Route did not engage for long enough") - print("\n\n", "*"*30, "\n\n") - print("New route:", relr, "\n") if upload: - upload_route(relr, exclude_patterns=['*.hevc', ]) - return relr + upload_route(rel_log_dir) + + return rel_log_dir if __name__ == "__main__": def comma_separated_list(string): - if string == "all": - return string return string.split(",") + all_procs = [p.proc_name for p in CONFIGS] parser = argparse.ArgumentParser(description="Generate new segments from old ones") parser.add_argument("--upload", action="store_true", help="Upload the new segment to the CI bucket") parser.add_argument("--outdir", help="log output dir", default=FAKEDATA) - parser.add_argument("--whitelist-procs", type=comma_separated_list, default="all", - help="Comma-separated whitelist of processes to regen (e.g. controlsd). Pass 'all' to whitelist all processes.") + parser.add_argument("--whitelist-procs", type=comma_separated_list, default=all_procs, + help="Comma-separated whitelist of processes to regen (e.g. controlsd,radard)") + parser.add_argument("--blacklist-procs", type=comma_separated_list, default=[], + help="Comma-separated blacklist of processes to regen (e.g. controlsd,radard)") parser.add_argument("route", type=str, help="The source route") parser.add_argument("seg", type=int, help="Segment in source route") args = parser.parse_args() - regen_and_save(args.route, args.seg, args.whitelist_procs, args.upload, outdir=args.outdir) + blacklist_set = set(args.blacklist_procs) + daemons = [p for p in args.whitelist_procs if p not in blacklist_set] + regen_and_save(args.route, args.seg, daemons=daemons, upload=args.upload, outdir=args.outdir) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 1b07081cd8e168..1a717311bba430 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -82,7 +82,7 @@ def run_test_process(data): assert os.path.exists(cur_log_fn), f"Cannot find log to upload: {cur_log_fn}" upload_file(cur_log_fn, os.path.basename(cur_log_fn)) os.remove(cur_log_fn) - return (segment, cfg.proc_name, cfg.subtest_name, res) + return (segment, cfg.proc_name, res) def get_log_data(segment): @@ -224,24 +224,24 @@ def format_diff(results, log_paths, ref_commit): if cfg.proc_name not in tested_procs: continue - cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}{cfg.subtest_name}_{cur_commit}.bz2") + cur_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{cur_commit}.bz2") if args.update_refs: # reference logs will not exist if routes were just regenerated ref_log_path = get_url(*segment.rsplit("--", 1)) else: - ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}{cfg.subtest_name}_{ref_commit}.bz2") + ref_log_fn = os.path.join(FAKEDATA, f"{segment}_{cfg.proc_name}_{ref_commit}.bz2") ref_log_path = ref_log_fn if os.path.exists(ref_log_fn) else BASE_URL + os.path.basename(ref_log_fn) dat = None if args.upload_only else log_data[segment] pool_args.append((segment, cfg, args, cur_log_fn, ref_log_path, dat)) - log_paths[segment][cfg.proc_name + cfg.subtest_name]['ref'] = ref_log_path - log_paths[segment][cfg.proc_name + cfg.subtest_name]['new'] = cur_log_fn + log_paths[segment][cfg.proc_name]['ref'] = ref_log_path + log_paths[segment][cfg.proc_name]['new'] = cur_log_fn results: Any = defaultdict(dict) p2 = pool.map(run_test_process, pool_args) - for (segment, proc, subtest_name, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): + for (segment, proc, result) in tqdm(p2, desc="Running Tests", total=len(pool_args)): if not args.upload_only: - results[segment][proc + subtest_name] = result + results[segment][proc] = result diff1, diff2, failed = format_diff(results, log_paths, ref_commit) if not upload: diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 97d4060767815e..3a4077e4f69a64 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -29,7 +29,8 @@ widgets_src = ["ui.cc", "qt/widgets/input.cc", "qt/widgets/drive_stats.cc", "qt/ qt_env['CPPDEFINES'] = [] if maps: base_libs += ['qmapboxgl'] - widgets_src += ["qt/maps/map_helpers.cc", "qt/maps/map_settings.cc", "qt/maps/map.cc", "qt/maps/map_panel.cc"] + widgets_src += ["qt/maps/map_helpers.cc", "qt/maps/map_settings.cc", "qt/maps/map.cc", "qt/maps/map_panel.cc", + "qt/maps/map_eta.cc", "qt/maps/map_instructions.cc"] qt_env['CPPDEFINES'] += ["ENABLE_MAPS"] widgets = qt_env.Library("qt_widgets", widgets_src, LIBS=base_libs) @@ -104,7 +105,7 @@ if GetOption('extras'): installers = [ ("openpilot", release), ("openpilot_test", f"{release}-staging"), - ("openpilot_nightly", "master-ci"), + ("openpilot_nightly", "nightly"), ("openpilot_internal", "master"), ("dashcam", dashcam), ("dashcam_test", f"{dashcam}-staging"), diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc index 8947e5641e9ede..ba8aab3ca952c9 100644 --- a/selfdrive/ui/qt/maps/map.cc +++ b/selfdrive/ui/qt/maps/map.cc @@ -3,7 +3,6 @@ #include #include -#include #include "common/transformations/coordinates.hpp" #include "selfdrive/ui/qt/maps/map_helpers.h" @@ -12,7 +11,6 @@ const int PAN_TIMEOUT = 100; -const float MANEUVER_TRANSITION_THRESHOLD = 10; const float MAX_ZOOM = 17; const float MIN_ZOOM = 14; @@ -20,8 +18,6 @@ const float MAX_PITCH = 50; const float MIN_PITCH = 0; const float MAP_SCALE = 2; -const QString ICON_SUFFIX = ".png"; - MapWindow::MapWindow(const QMapboxGLSettings &settings) : m_settings(settings), velocity_filter(0, 10, 0.05) { QObject::connect(uiState(), &UIState::uiUpdate, this, &MapWindow::updateState); @@ -105,10 +101,33 @@ void MapWindow::initLayers() { nav["type"] = "line"; nav["source"] = "navSource"; m_map->addLayer(nav, "road-intersection"); - m_map->setPaintProperty("navLayer", "line-color", QColor("#31a1ee")); + + QVariantMap transition; + transition["duration"] = 400; // ms + m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(uiState()->scene.navigate_on_openpilot)); + m_map->setPaintProperty("navLayer", "line-color-transition", transition); m_map->setPaintProperty("navLayer", "line-width", 7.5); m_map->setLayoutProperty("navLayer", "line-cap", "round"); - m_map->addAnnotationIcon("default_marker", QImage("../assets/navigation/default_marker.svg")); + } + if (!m_map->layerExists("pinLayer")) { + qDebug() << "Initializing pinLayer"; + m_map->addImage("default_marker", QImage("../assets/navigation/default_marker.svg")); + QVariantMap pin; + pin["id"] = "pinLayer"; + pin["type"] = "symbol"; + pin["source"] = "pinSource"; + m_map->addLayer(pin); + + // FIXME: solve, workaround to remove animation on visibility property + QVariantMap transition; + transition["duration"] = 0; // ms + m_map->setPaintProperty("pinLayer", "icon-opacity-transition", transition); + m_map->setLayoutProperty("pinLayer", "icon-pitch-alignment", "viewport"); + m_map->setLayoutProperty("pinLayer", "icon-image", "default_marker"); + m_map->setLayoutProperty("pinLayer", "icon-ignore-placement", true); + m_map->setLayoutProperty("pinLayer", "icon-allow-overlap", true); + m_map->setLayoutProperty("pinLayer", "symbol-sort-key", 0); + m_map->setLayoutProperty("pinLayer", "icon-anchor", "bottom"); } if (!m_map->layerExists("carPosLayer")) { qDebug() << "Initializing carPosLayer"; @@ -124,6 +143,7 @@ void MapWindow::initLayers() { m_map->setLayoutProperty("carPosLayer", "icon-size", 0.5); m_map->setLayoutProperty("carPosLayer", "icon-ignore-placement", true); m_map->setLayoutProperty("carPosLayer", "icon-allow-overlap", true); + // TODO: remove, symbol-sort-key does not seem to matter outside of each layer m_map->setLayoutProperty("carPosLayer", "symbol-sort-key", 0); } } @@ -135,11 +155,17 @@ void MapWindow::updateState(const UIState &s) { const SubMaster &sm = *(s.sm); update(); - // update navigate on openpilot status if (sm.updated("modelV2")) { - bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled(); - if (nav_enabled && !uiState()->scene.navigate_on_openpilot) { - emit requestVisible(true); // Show map on rising edge of navigate on openpilot + // set path color on change, and show map on rising edge of navigate on openpilot + bool nav_enabled = sm["modelV2"].getModelV2().getNavEnabled() && + sm["controlsState"].getControlsState().getEnabled(); + if (nav_enabled != uiState()->scene.navigate_on_openpilot) { + if (loaded_once) { + m_map->setPaintProperty("navLayer", "line-color", getNavPathColor(nav_enabled)); + } + if (nav_enabled) { + emit requestVisible(true); + } } uiState()->scene.navigate_on_openpilot = nav_enabled; } @@ -181,7 +207,14 @@ void MapWindow::updateState(const UIState &s) { } initLayers(); - setError(locationd_valid ? "" : tr("Waiting for GPS")); + if (!locationd_valid) { + setError(tr("Waiting for GPS")); + } else if (routing_problem) { + setError(tr("Waiting for internet")); + } else { + setError(""); + } + if (locationd_valid) { // Update current location marker auto point = coordinate_to_collection(*last_position); @@ -206,6 +239,12 @@ void MapWindow::updateState(const UIState &s) { } if (sm.updated("navInstruction")) { + // an invalid navInstruction packet with a nav destination is only possible if: + // - API exception/no internet + // - route response is empty + auto dest = coordinate_from_param("NavDestination"); + routing_problem = !sm.valid("navInstruction") && dest.has_value(); + if (sm.valid("navInstruction")) { auto i = sm["navInstruction"].getNavInstruction(); map_eta->updateETA(i.getTimeRemaining(), i.getTimeRemainingTypical(), i.getDistanceRemaining()); @@ -363,6 +402,7 @@ void MapWindow::offroadTransition(bool offroad) { if (offroad) { clearRoute(); uiState()->scene.navigate_on_openpilot = false; + routing_problem = false; } else { auto dest = coordinate_from_param("NavDestination"); emit requestVisible(dest.has_value()); @@ -371,197 +411,16 @@ void MapWindow::offroadTransition(bool offroad) { } void MapWindow::updateDestinationMarker() { - if (marker_id != -1) { - m_map->removeAnnotation(marker_id); - marker_id = -1; - } + m_map->setPaintProperty("pinLayer", "icon-opacity", 0); auto nav_dest = coordinate_from_param("NavDestination"); if (nav_dest.has_value()) { - auto ano = QMapbox::SymbolAnnotation {*nav_dest, "default_marker"}; - marker_id = m_map->addAnnotation(QVariant::fromValue(ano)); - } -} - -MapInstructions::MapInstructions(QWidget *parent) : QWidget(parent) { - is_rhd = Params().getBool("IsRhdDetected"); - QHBoxLayout *main_layout = new QHBoxLayout(this); - main_layout->setContentsMargins(11, 50, 11, 11); - main_layout->addWidget(icon_01 = new QLabel, 0, Qt::AlignTop); - - QWidget *right_container = new QWidget(this); - right_container->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); - QVBoxLayout *layout = new QVBoxLayout(right_container); - - layout->addWidget(distance = new QLabel); - distance->setStyleSheet(R"(font-size: 90px;)"); - - layout->addWidget(primary = new QLabel); - primary->setStyleSheet(R"(font-size: 60px;)"); - primary->setWordWrap(true); - - layout->addWidget(secondary = new QLabel); - secondary->setStyleSheet(R"(font-size: 50px;)"); - secondary->setWordWrap(true); - - layout->addLayout(lane_layout = new QHBoxLayout); - main_layout->addWidget(right_container); - - setStyleSheet("color:white"); - QPalette pal = palette(); - pal.setColor(QPalette::Background, QColor(0, 0, 0, 150)); - setAutoFillBackground(true); - setPalette(pal); - - buildPixmapCache(); -} - -void MapInstructions::buildPixmapCache() { - QDir dir("../assets/navigation"); - for (QString fn : dir.entryList({"*" + ICON_SUFFIX}, QDir::Files)) { - QPixmap pm(dir.filePath(fn)); - QString key = fn.left(fn.size() - ICON_SUFFIX.length()); - pm = pm.scaledToWidth(200, Qt::SmoothTransformation); - - // Maneuver icons - pixmap_cache[key] = pm; - // lane direction icons - if (key.contains("turn_")) { - pixmap_cache["lane_" + key] = pm.scaled({125, 125}, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); - } - - // for rhd, reflect direction and then flip - if (key.contains("_left")) { - pixmap_cache["rhd_" + key.replace("_left", "_right")] = pm.transformed(QTransform().scale(-1, 1)); - } else if (key.contains("_right")) { - pixmap_cache["rhd_" + key.replace("_right", "_left")] = pm.transformed(QTransform().scale(-1, 1)); - } - } -} - -QString MapInstructions::getDistance(float d) { - d = std::max(d, 0.0f); - if (uiState()->scene.is_metric) { - return (d > 500) ? QString::number(d / 1000, 'f', 1) + tr(" km") - : QString::number(50 * int(d / 50)) + tr(" m"); - } else { - float feet = d * METER_TO_FOOT; - return (feet > 500) ? QString::number(d * METER_TO_MILE, 'f', 1) + tr(" mi") - : QString::number(50 * int(feet / 50)) + tr(" ft"); - } -} - -void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) { - setUpdatesEnabled(false); - - // Show instruction text - QString primary_str = QString::fromStdString(instruction.getManeuverPrimaryText()); - QString secondary_str = QString::fromStdString(instruction.getManeuverSecondaryText()); - - primary->setText(primary_str); - secondary->setVisible(secondary_str.length() > 0); - secondary->setText(secondary_str); - distance->setText(getDistance(instruction.getManeuverDistance())); - - // Show arrow with direction - QString type = QString::fromStdString(instruction.getManeuverType()); - QString modifier = QString::fromStdString(instruction.getManeuverModifier()); - if (!type.isEmpty()) { - QString fn = "direction_" + type; - if (!modifier.isEmpty()) { - fn += "_" + modifier; - } - fn = fn.replace(' ', '_'); - bool rhd = is_rhd && (fn.contains("_left") || fn.contains("_right")); - icon_01->setPixmap(pixmap_cache[!rhd ? fn : "rhd_" + fn]); - icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); - icon_01->setVisible(true); + auto point = coordinate_to_collection(*nav_dest); + QMapbox::Feature feature(QMapbox::Feature::PointType, point, {}, {}); + QVariantMap pinSource; + pinSource["type"] = "geojson"; + pinSource["data"] = QVariant::fromValue(feature); + m_map->updateSource("pinSource", pinSource); + m_map->setPaintProperty("pinLayer", "icon-opacity", 1); } - - // Show lanes - auto lanes = instruction.getLanes(); - for (int i = 0; i < lanes.size(); ++i) { - bool active = lanes[i].getActive(); - - // TODO: only use active direction if active - bool left = false, straight = false, right = false; - for (auto const &direction: lanes[i].getDirections()) { - left |= direction == cereal::NavInstruction::Direction::LEFT; - right |= direction == cereal::NavInstruction::Direction::RIGHT; - straight |= direction == cereal::NavInstruction::Direction::STRAIGHT; - } - - // TODO: Make more images based on active direction and combined directions - QString fn = "lane_direction_"; - if (left) { - fn += "turn_left"; - } else if (right) { - fn += "turn_right"; - } else if (straight) { - fn += "turn_straight"; - } - - if (!active) { - fn += "_inactive"; - } - - QLabel *label = (i < lane_labels.size()) ? lane_labels[i] : lane_labels.emplace_back(new QLabel); - if (!label->parentWidget()) { - lane_layout->addWidget(label); - } - label->setPixmap(pixmap_cache[fn]); - label->setVisible(true); - } - - for (int i = lanes.size(); i < lane_labels.size(); ++i) { - lane_labels[i]->setVisible(false); - } - - setUpdatesEnabled(true); - setVisible(true); -} - -MapETA::MapETA(QWidget *parent) : QWidget(parent) { - setVisible(false); - setAttribute(Qt::WA_TranslucentBackground); - eta_doc.setUndoRedoEnabled(false); - eta_doc.setDefaultStyleSheet("body {font-family:Inter;font-size:60px;color:white;} b{font-size:70px;font-weight:600}"); -} - -void MapETA::paintEvent(QPaintEvent *event) { - if (!eta_doc.isEmpty()) { - QPainter p(this); - p.setRenderHint(QPainter::Antialiasing); - p.setPen(Qt::NoPen); - p.setBrush(QColor(0, 0, 0, 150)); - QSizeF txt_size = eta_doc.size(); - p.drawRoundedRect((width() - txt_size.width()) / 2 - UI_BORDER_SIZE, 0, txt_size.width() + UI_BORDER_SIZE * 2, height() + 25, 25, 25); - p.translate((width() - txt_size.width()) / 2, (height() - txt_size.height()) / 2); - eta_doc.drawContents(&p); - } -} - -void MapETA::updateETA(float s, float s_typical, float d) { - // ETA - auto eta_t = QDateTime::currentDateTime().addSecs(s).time(); - auto eta = format_24h ? std::array{eta_t.toString("HH:mm"), tr("eta")} - : std::array{eta_t.toString("h:mm a").split(' ')[0], eta_t.toString("a")}; - - // Remaining time - auto remaining = s < 3600 ? std::array{QString::number(int(s / 60)), tr("min")} - : std::array{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")}; - QString color = "#25DA6E"; - if (s / s_typical > 1.5) color = "#DA3025"; - else if (s / s_typical > 1.2) color = "#DAA725"; - - // Distance - float num = uiState()->scene.is_metric ? (d / 1000.0) : (d * METER_TO_MILE); - auto distance = std::array{QString::number(num, 'f', num < 100 ? 1 : 0), - uiState()->scene.is_metric ? tr("km") : tr("mi")}; - - eta_doc.setHtml(QString(R"(%1%2 %4%5 %6%7)") - .arg(eta[0], eta[1], color, remaining[0], remaining[1], distance[0], distance[1])); - - setVisible(d >= MANEUVER_TRANSITION_THRESHOLD); - update(); } diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h index ff4b0b3401592f..83b0118f96da37 100644 --- a/selfdrive/ui/qt/maps/map.h +++ b/selfdrive/ui/qt/maps/map.h @@ -4,8 +4,6 @@ #include #include -#include -#include #include #include #include @@ -15,7 +13,6 @@ #include #include #include -#include #include #include @@ -23,42 +20,8 @@ #include "common/params.h" #include "common/util.h" #include "selfdrive/ui/ui.h" - -class MapInstructions : public QWidget { - Q_OBJECT - -private: - QLabel *distance; - QLabel *primary; - QLabel *secondary; - QLabel *icon_01; - QHBoxLayout *lane_layout; - bool is_rhd = false; - std::vector lane_labels; - QHash pixmap_cache; - -public: - MapInstructions(QWidget * parent=nullptr); - void buildPixmapCache(); - QString getDistance(float d); - void updateInstructions(cereal::NavInstruction::Reader instruction); -}; - -class MapETA : public QWidget { - Q_OBJECT - -public: - MapETA(QWidget * parent=nullptr); - void updateETA(float seconds, float seconds_typical, float distance); - -private: - void paintEvent(QPaintEvent *event) override; - void showEvent(QShowEvent *event) override { format_24h = param.getBool("NavSettingTime24h"); } - - bool format_24h = false; - QTextDocument eta_doc; - Params param; -}; +#include "selfdrive/ui/qt/maps/map_eta.h" +#include "selfdrive/ui/qt/maps/map_instructions.h" class MapWindow : public QOpenGLWidget { Q_OBJECT @@ -74,7 +37,6 @@ class MapWindow : public QOpenGLWidget { QMapboxGLSettings m_settings; QScopedPointer m_map; - QMapbox::AnnotationID marker_id = -1; void initLayers(); @@ -102,6 +64,7 @@ class MapWindow : public QOpenGLWidget { std::optional last_bearing; FirstOrderFilter velocity_filter; bool locationd_valid = false; + bool routing_problem = false; QWidget *map_overlay; QLabel *error; @@ -110,6 +73,11 @@ class MapWindow : public QOpenGLWidget { QPushButton *settings_btn; QPixmap directions_icon, settings_icon; + // Blue with normal nav, green when nav is input into the model + QColor getNavPathColor(bool nav_enabled) { + return nav_enabled ? QColor("#31ee73") : QColor("#31a1ee"); + } + void clearRoute(); void updateDestinationMarker(); uint64_t route_rcv_frame = 0; diff --git a/selfdrive/ui/qt/maps/map_eta.cc b/selfdrive/ui/qt/maps/map_eta.cc new file mode 100644 index 00000000000000..23366efbe2bfb7 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_eta.cc @@ -0,0 +1,55 @@ +#include "selfdrive/ui/qt/maps/map_eta.h" + +#include +#include + +#include "selfdrive/ui/ui.h" + +const float MANEUVER_TRANSITION_THRESHOLD = 10; + +MapETA::MapETA(QWidget *parent) : QWidget(parent) { + setVisible(false); + setAttribute(Qt::WA_TranslucentBackground); + eta_doc.setUndoRedoEnabled(false); + eta_doc.setDefaultStyleSheet("body {font-family:Inter;font-size:60px;color:white;} b{font-size:70px;font-weight:600}"); +} + +void MapETA::paintEvent(QPaintEvent *event) { + if (!eta_doc.isEmpty()) { + QPainter p(this); + p.setRenderHint(QPainter::Antialiasing); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 150)); + QSizeF txt_size = eta_doc.size(); + p.drawRoundedRect((width() - txt_size.width()) / 2 - UI_BORDER_SIZE, 0, txt_size.width() + UI_BORDER_SIZE * 2, height() + 25, 25, 25); + p.translate((width() - txt_size.width()) / 2, (height() - txt_size.height()) / 2); + eta_doc.drawContents(&p); + } +} + +void MapETA::updateETA(float s, float s_typical, float d) { + // ETA + auto eta_t = QDateTime::currentDateTime().addSecs(s).time(); + auto eta = format_24h ? std::array{eta_t.toString("HH:mm"), tr("eta")} + : std::array{eta_t.toString("h:mm a").split(' ')[0], eta_t.toString("a")}; + + // Remaining time + auto remaining = s < 3600 ? std::array{QString::number(int(s / 60)), tr("min")} + : std::array{QString("%1:%2").arg((int)s / 3600).arg(((int)s % 3600) / 60, 2, 10, QLatin1Char('0')), tr("hr")}; + QString color = "#25DA6E"; + if (s / s_typical > 1.5) + color = "#DA3025"; + else if (s / s_typical > 1.2) + color = "#DAA725"; + + // Distance + float num = uiState()->scene.is_metric ? (d / 1000.0) : (d * METER_TO_MILE); + auto distance = std::array{QString::number(num, 'f', num < 100 ? 1 : 0), + uiState()->scene.is_metric ? tr("km") : tr("mi")}; + + eta_doc.setHtml(QString(R"(%1%2 %4%5 %6%7)") + .arg(eta[0], eta[1], color, remaining[0], remaining[1], distance[0], distance[1])); + + setVisible(d >= MANEUVER_TRANSITION_THRESHOLD); + update(); +} diff --git a/selfdrive/ui/qt/maps/map_eta.h b/selfdrive/ui/qt/maps/map_eta.h new file mode 100644 index 00000000000000..6e59837de3d746 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_eta.h @@ -0,0 +1,23 @@ +#pragma once + +#include +#include +#include + +#include "common/params.h" + +class MapETA : public QWidget { + Q_OBJECT + +public: + MapETA(QWidget * parent=nullptr); + void updateETA(float seconds, float seconds_typical, float distance); + +private: + void paintEvent(QPaintEvent *event) override; + void showEvent(QShowEvent *event) override { format_24h = param.getBool("NavSettingTime24h"); } + + bool format_24h = false; + QTextDocument eta_doc; + Params param; +}; diff --git a/selfdrive/ui/qt/maps/map_instructions.cc b/selfdrive/ui/qt/maps/map_instructions.cc new file mode 100644 index 00000000000000..fc7f80690a2edf --- /dev/null +++ b/selfdrive/ui/qt/maps/map_instructions.cc @@ -0,0 +1,146 @@ +#include "selfdrive/ui/qt/maps/map_instructions.h" + +#include +#include + +#include "selfdrive/ui/ui.h" + +const QString ICON_SUFFIX = ".png"; + +MapInstructions::MapInstructions(QWidget *parent) : QWidget(parent) { + is_rhd = Params().getBool("IsRhdDetected"); + QHBoxLayout *main_layout = new QHBoxLayout(this); + main_layout->setContentsMargins(11, 50, 11, 11); + main_layout->addWidget(icon_01 = new QLabel, 0, Qt::AlignTop); + + QWidget *right_container = new QWidget(this); + right_container->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Preferred); + QVBoxLayout *layout = new QVBoxLayout(right_container); + + layout->addWidget(distance = new QLabel); + distance->setStyleSheet(R"(font-size: 90px;)"); + + layout->addWidget(primary = new QLabel); + primary->setStyleSheet(R"(font-size: 60px;)"); + primary->setWordWrap(true); + + layout->addWidget(secondary = new QLabel); + secondary->setStyleSheet(R"(font-size: 50px;)"); + secondary->setWordWrap(true); + + layout->addLayout(lane_layout = new QHBoxLayout); + main_layout->addWidget(right_container); + + setStyleSheet("color:white"); + QPalette pal = palette(); + pal.setColor(QPalette::Background, QColor(0, 0, 0, 150)); + setAutoFillBackground(true); + setPalette(pal); + + buildPixmapCache(); +} + +void MapInstructions::buildPixmapCache() { + QDir dir("../assets/navigation"); + for (QString fn : dir.entryList({"*" + ICON_SUFFIX}, QDir::Files)) { + QPixmap pm(dir.filePath(fn)); + QString key = fn.left(fn.size() - ICON_SUFFIX.length()); + pm = pm.scaledToWidth(200, Qt::SmoothTransformation); + + // Maneuver icons + pixmap_cache[key] = pm; + // lane direction icons + if (key.contains("turn_")) { + pixmap_cache["lane_" + key] = pm.scaled({125, 125}, Qt::IgnoreAspectRatio, Qt::SmoothTransformation); + } + + // for rhd, reflect direction and then flip + if (key.contains("_left")) { + pixmap_cache["rhd_" + key.replace("_left", "_right")] = pm.transformed(QTransform().scale(-1, 1)); + } else if (key.contains("_right")) { + pixmap_cache["rhd_" + key.replace("_right", "_left")] = pm.transformed(QTransform().scale(-1, 1)); + } + } +} + +QString MapInstructions::getDistance(float d) { + d = std::max(d, 0.0f); + if (uiState()->scene.is_metric) { + return (d > 500) ? QString::number(d / 1000, 'f', 1) + tr(" km") + : QString::number(50 * int(d / 50)) + tr(" m"); + } else { + float feet = d * METER_TO_FOOT; + return (feet > 500) ? QString::number(d * METER_TO_MILE, 'f', 1) + tr(" mi") + : QString::number(50 * int(feet / 50)) + tr(" ft"); + } +} + +void MapInstructions::updateInstructions(cereal::NavInstruction::Reader instruction) { + setUpdatesEnabled(false); + + // Show instruction text + QString primary_str = QString::fromStdString(instruction.getManeuverPrimaryText()); + QString secondary_str = QString::fromStdString(instruction.getManeuverSecondaryText()); + + primary->setText(primary_str); + secondary->setVisible(secondary_str.length() > 0); + secondary->setText(secondary_str); + distance->setText(getDistance(instruction.getManeuverDistance())); + + // Show arrow with direction + QString type = QString::fromStdString(instruction.getManeuverType()); + QString modifier = QString::fromStdString(instruction.getManeuverModifier()); + if (!type.isEmpty()) { + QString fn = "direction_" + type; + if (!modifier.isEmpty()) { + fn += "_" + modifier; + } + fn = fn.replace(' ', '_'); + bool rhd = is_rhd && (fn.contains("_left") || fn.contains("_right")); + icon_01->setPixmap(pixmap_cache[!rhd ? fn : "rhd_" + fn]); + icon_01->setSizePolicy(QSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed)); + icon_01->setVisible(true); + } + + // Show lanes + auto lanes = instruction.getLanes(); + for (int i = 0; i < lanes.size(); ++i) { + bool active = lanes[i].getActive(); + + // TODO: only use active direction if active + bool left = false, straight = false, right = false; + for (auto const &direction : lanes[i].getDirections()) { + left |= direction == cereal::NavInstruction::Direction::LEFT; + right |= direction == cereal::NavInstruction::Direction::RIGHT; + straight |= direction == cereal::NavInstruction::Direction::STRAIGHT; + } + + // TODO: Make more images based on active direction and combined directions + QString fn = "lane_direction_"; + if (left) { + fn += "turn_left"; + } else if (right) { + fn += "turn_right"; + } else if (straight) { + fn += "turn_straight"; + } + + if (!active) { + fn += "_inactive"; + } + + QLabel *label = (i < lane_labels.size()) ? lane_labels[i] : lane_labels.emplace_back(new QLabel); + if (!label->parentWidget()) { + lane_layout->addWidget(label); + } + label->setPixmap(pixmap_cache[fn]); + label->setVisible(true); + } + + for (int i = lanes.size(); i < lane_labels.size(); ++i) { + lane_labels[i]->setVisible(false); + } + + setUpdatesEnabled(true); + setVisible(true); +} diff --git a/selfdrive/ui/qt/maps/map_instructions.h b/selfdrive/ui/qt/maps/map_instructions.h new file mode 100644 index 00000000000000..83ad3b87a47c77 --- /dev/null +++ b/selfdrive/ui/qt/maps/map_instructions.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include + +#include "cereal/gen/cpp/log.capnp.h" + +class MapInstructions : public QWidget { + Q_OBJECT + +private: + QLabel *distance; + QLabel *primary; + QLabel *secondary; + QLabel *icon_01; + QHBoxLayout *lane_layout; + bool is_rhd = false; + std::vector lane_labels; + QHash pixmap_cache; + +public: + MapInstructions(QWidget * parent=nullptr); + void buildPixmapCache(); + QString getDistance(float d); + void updateInstructions(cereal::NavInstruction::Reader instruction); +}; diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc index cf14e5cb916cce..c146345eb49775 100644 --- a/selfdrive/ui/qt/offroad/networking.cc +++ b/selfdrive/ui/qt/offroad/networking.cc @@ -286,6 +286,7 @@ void WifiUI::refresh() { scanningLabel->setVisible(is_empty); if (is_empty) return; + const bool is_tethering_enabled = wifi->isTetheringEnabled(); QList sortedNetworks = wifi->seenNetworks.values(); std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength); @@ -313,7 +314,7 @@ void WifiUI::refresh() { } // Forget button - if (wifi->isKnownConnection(network.ssid) && !wifi->isTetheringEnabled()) { + if (wifi->isKnownConnection(network.ssid) && !is_tethering_enabled) { QPushButton *forgetBtn = new QPushButton(tr("FORGET")); forgetBtn->setObjectName("forgetBtn"); QObject::connect(forgetBtn, &QPushButton::clicked, [=]() { diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index f39cac0c2c5515..6d880134c8f4be 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -127,20 +127,22 @@ void TogglesPanel::showEvent(QShowEvent *event) { void TogglesPanel::updateToggles() { auto experimental_mode_toggle = toggles["ExperimentalMode"]; auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"]; - - QMap exp_features; - exp_features["e2e_long"] = tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. " - "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected."); - exp_features["nav_on_op"] = tr("When navigation has a destination, openpilot will input the map information into the model. This generally improves behavior and allows openpilot to keep left or right appropriately at forks/exits and take turns. " - "Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected."); - exp_features["visualization"] = tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner."); - - // Ordering of the headers - QVector> exp_features_headers { - {"e2e_long", tr("🌮 End-to-End Longitudinal Control 🌮")}, - {"nav_on_op", tr("Navigate on openpilot")}, - {"visualization", tr("New Driving Visualization")} - }; + const QString e2e_description = QString("%1
" + "

%2


" + "%3
" + "

%4


" + "%5
" + "

%6


" + "%7") + .arg(tr("openpilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. Experimental features are listed below:")) + .arg(tr("🌮 End-to-End Longitudinal Control 🌮")) + .arg(tr("Let the driving model control the gas and brakes. openpilot will drive as it thinks a human would, including stopping for red lights and stop signs. " + "Since the driving model decides the speed to drive, the set speed will only act as an upper bound. This is an alpha quality feature; mistakes should be expected.")) + .arg(tr("Navigate on openpilot")) + .arg(tr("When navigation has a destination, openpilot will input the map information into the model. This generally improves behavior and allows openpilot to keep left or right appropriately at forks/exits and take turns. " + "Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected.")) + .arg(tr("New Driving Visualization")) + .arg(tr("The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner.")); const bool is_release = params.getBool("IsReleaseBranch"); auto cp_bytes = params.get("CarParamsPersistent"); @@ -155,35 +157,34 @@ void TogglesPanel::updateToggles() { op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release); if (hasLongitudinalControl(CP)) { // normal description and toggle + experimental_mode_toggle->setEnabled(true); + experimental_mode_toggle->setDescription(e2e_description); long_personality_setting->setEnabled(true); } else { // no long for now + experimental_mode_toggle->setEnabled(false); long_personality_setting->setEnabled(false); + params.remove("ExperimentalMode"); - // prepended note that end-to-end longitudinal control will not be used - const QString unavailable = tr("End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control."); + const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control."); QString long_desc = unavailable + " " + \ tr("openpilot longitudinal control may come in a future update."); if (CP.getExperimentalLongitudinalAvailable()) { if (is_release) { - long_desc = unavailable + " " + tr("An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches."); + long_desc = unavailable + " " + tr("An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches."); } else { - long_desc = tr("Enable experimental longitudinal control to use End-to-End Longitudinal Control."); + long_desc = tr("Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode."); } } - exp_features["e2e_long"].prepend("" + long_desc + "

"); + experimental_mode_toggle->setDescription("" + long_desc + "

" + e2e_description); } + + experimental_mode_toggle->refresh(); } else { + experimental_mode_toggle->setDescription(e2e_description); op_long_toggle->setVisible(false); } - - // set experimental mode toggle description - QString e2e_description = tr("openpilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. Experimental features are listed below:") + "
"; - for (const auto& kv : exp_features_headers) { - e2e_description += QString("

%1


%2
").arg(kv.second).arg(exp_features.value(kv.first)); - } - experimental_mode_toggle->setDescription(e2e_description); } DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4c936b2125e785..ef711afd509448 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -188,7 +188,9 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals } void ExperimentalButton::changeMode() { - if (params.getBool("ExperimentalModeConfirmed")) { + const auto cp = (*uiState()->sm)["carParams"].getCarParams(); + bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed"); + if (can_change) { params.putBool("ExperimentalMode", !experimental_mode); } } @@ -452,10 +454,8 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } // paint path - const bool show_e2e_path = (sm["controlsState"].getControlsState().getExperimentalMode() && - scene.longitudinal_control); QLinearGradient bg(0, height(), 0, 0); - if (show_e2e_path) { + if (sm["controlsState"].getControlsState().getExperimentalMode()) { // The first half of track_vertices are the points for the right side of the path // and the indices match the positions of accel from uiPlan const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index d1cd7aebb3847a..b4282b6a9ca06d 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -403,6 +403,10 @@ Waiting for GPS Warten auf GPS + + Waiting for internet + Auf Internet warten + MultiOptionDialog @@ -1083,6 +1087,10 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. Die Fahrvisualisierung wechselt bei niedrigen Geschwindigkeiten zur Straßengewandten Weitwinkelkamera, um manche Kurven besser zu zeigen. Außerdem wird das Experimenteller Modus logo oben rechts angezeigt. + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + Der experimentelle Modus ist momentan für dieses Auto nicht verfügbar da es den eingebauten adaptiven Tempomaten des Autos benutzt. + openpilot longitudinal control may come in a future update. @@ -1120,15 +1128,11 @@ This may take up to a minute. - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. + Navigate on openpilot @@ -1136,7 +1140,7 @@ This may take up to a minute. - Navigate on openpilot + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 9de14519e3092d..319b08bc593ccc 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -402,6 +402,10 @@ Waiting for GPS GPS信号を探しています + + Waiting for internet + インターネット接続を待機中 + MultiOptionDialog @@ -1075,6 +1079,10 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 新しい運転画面では、低速時に広角カメラの映像を表示することで、曲がる際の道路の視覚を向上します。実験段階を表すマークが右上に表示されます。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + この車のACCがアクセル制御を行うため実験モードを利用することができません。 + openpilot longitudinal control may come in a future update. @@ -1112,15 +1120,11 @@ This may take up to a minute. - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. + Navigate on openpilot @@ -1128,7 +1132,7 @@ This may take up to a minute. - Navigate on openpilot + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index faa2a17ce0c3a9..d30df3a622621a 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -64,7 +64,7 @@ Prevent large data uploads when on a metered connection - 데이터 요금제 연결 시 대용량 데이터 업로드 방지 + 데이터 요금제 연결 시 대용량 데이터 업로드를 방지합니다 @@ -167,7 +167,7 @@ Preview the driver facing camera to ensure that driver monitoring has good visibility. (vehicle must be off) - 운전자 모니터링이 좋은 가시성을 갖도록 운전자를 향한 카메라를 미리 봅니다. (차량연결은 해제되어있어야 합니다) + 운전자 모니터링이 잘 되는지 확인하기 위해 카메라를 향한 운전자를 미리 봅니다. (차량연결은 해제되어있어야 합니다) Reset Calibration @@ -227,11 +227,11 @@ openpilot requires the device to be mounted within 4° left or right and within 5° up or 8° down. openpilot is continuously calibrating, resetting is rarely required. - openpilot은 좌우측은 4° 이내, 위쪽은 5° 아래쪽은 8° 이내로 장치를 설치해야 합니다. openpilot은 지속적으로 보정되므로 리셋은 거의 필요하지 않습니다. + openpilot 장치는 좌우측 4° 이내, 위쪽 5° 아래쪽 8° 이내로 장착되어야 합니다. openpilot은 지속적으로 보정되며 재설정은 거의 필요하지 않습니다. Your device is pointed %1° %2 and %3° %4. - 사용자의 장치가 %1° %2 및 %3° %4 위치에 설치되어있습니다. + 사용자의 장치는 %1° %2 및 %3° %4 의 위치에 장착되어 있습니다. down @@ -389,7 +389,7 @@ Manage at connect.comma.ai - connect.comma.ai에서 관리 + connect.comma.ai에서 관리됩니다 @@ -400,7 +400,11 @@ Waiting for GPS - GPS 수신중 입니다 + GPS 수신중 + + + Waiting for internet + 인터넷 대기중 @@ -426,7 +430,7 @@ for "%1" - "%1"에 접속하려면 인증이 필요합니다 + "%1"에 접속하려면 비밀번호가 필요합니다 Wrong password @@ -441,7 +445,7 @@ Connect to internet to check for updates. openpilot won't automatically start until it connects to internet to check for updates. - 인터넷에 연결하여 업데이트를 확인하세요. openpilot은 업데이트를 확인하기 위해 인터넷에 연결할 때까지 자동으로 시작되지 않습니다. + 업데이트를 확인하려면 인터넷에 연결하세요. openpilot은 업데이트를 확인하기 위해 인터넷에 연결할 때까지 자동으로 시작되지 않습니다. Unable to download updates @@ -459,11 +463,11 @@ An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install. - 백그라운드에서 운영 체제에 대한 업데이트를 다운로드되고 있습니다. 설치할 준비가 되면 업데이트하라는 메시지가 표시됩니다. + 백그라운드에서 운영 체제에 대한 업데이트가 다운로드되고 있습니다. 설치준비가 완료되면 업데이트하라는 메시지가 표시됩니다. Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, visit https://comma.ai/support. - 장치를 등록하지 못했습니다. comma.ai 서버에 연결하거나 업로드하지 않으며 comma.ai에서 지원을 받지 않습니다. 공식 장치는 https://comma.ai/support로 방문하세요 + 장치를 등록하지 못했습니다. comma.ai 서버에 연결하거나 업로드하지 않으며 comma.ai에서 지원을 받지 않습니다. 공식 장치인경우 https://comma.ai/support 에 방문하여 문의하세요. NVMe drive not mounted. @@ -475,7 +479,7 @@ openpilot was unable to identify your car. Your car is either unsupported or its ECUs are not recognized. Please submit a pull request to add the firmware versions to the proper vehicle. Need help? Join discord.comma.ai. - opepilot이 차량을 식별할수 없었습니다. 지원되지 않는 차량이거나 ECU가 인식되지 않습니다. 해당 차량에 펌웨어 버전을 추가하려면 PR을 제출하십시오. 도움이 필요하시면 discord.comma.ai에 가입하세요. + opepilot이 차량을 식별할수 없었습니다. 지원되지 않는 차량이거나 ECU가 인식되지 않습니다. 해당 차량에 펌웨어 버전을 추가하려면 PR을 제출하세요. 도움이 필요하시면 discord.comma.ai에 가입하세요. openpilot was unable to identify your car. Check integrity of cables and ensure all connections are secure, particularly that the comma power is fully inserted in the OBD-II port of the vehicle. Need help? Join discord.comma.ai. @@ -483,7 +487,7 @@ openpilot detected a change in the device's mounting position. Ensure the device is fully seated in the mount and the mount is firmly secured to the windshield. - openpilot이 장치의 장착 위치 변경을 감지했습니다. 장치가 마운트에 완전히 장착되고 마운트가 앞유리에 단단히 고정되었는지 확인하십시오. + openpilot 장치의 장착 위치 변경을 감지했습니다. 장치가 마운트에 완전히 장착되고 마운트가 앞유리에 단단히 고정되었는지 확인하세요. Device temperature too high. System cooling down before starting. Current internal component temperature: %1 @@ -509,7 +513,7 @@ PairingPopup Pair your device to your comma account - 장치를 콤마 계정과 페어링합니다 + 장치를 comma 계정과 페어링합니다 Go to https://connect.comma.ai on your phone @@ -521,7 +525,7 @@ Bookmark connect.comma.ai to your home screen to use it like an app - connect.comma.ai을 앱처럼 사용하려면 홈 화면에 바로가기를 만드십시오 + connect.comma.ai를 앱처럼 사용하려면 홈 화면에 바로가기를 만드세요. @@ -543,7 +547,7 @@ Become a comma prime member at connect.comma.ai - connect.comma.ai 접속하여 comma prime에 가입하세요 + connect.comma.ai에 접속하여 comma prime 회원이 되세요 PRIME FEATURES: @@ -642,7 +646,7 @@ Unable to mount data partition. Partition may be corrupted. Press confirm to erase and reset your device. - 데이터 파티션을 마운트할 수 없습니다. 파티션이 손상되었을 수 있습니다. 장치를 초기화하려면 확인을 누르세요. + 데이터 파티션을 마운트할 수 없습니다. 파티션이 손상되었을 수 있습니다. 모든 내용을 지우고 장치를 초기화하려면 확인을 누르세요. Press confirm to erase all content and settings. Press cancel to resume boot. @@ -651,7 +655,7 @@ Resetting device... This may take up to a minute. - 장치 초기화 중... + 장치를 초기화하는 중... 최대 1분이 소요될 수 있습니다. @@ -726,7 +730,7 @@ This may take up to a minute. for Custom Software - for Custom Software + 커스텀 소프트웨어 Downloading... @@ -765,7 +769,7 @@ This may take up to a minute. Pair your device with comma connect (connect.comma.ai) and claim your comma prime offer. - 장치를 (connect.comma.ai)에서 페어링하고 comma prime 오퍼를 청구합니다. + 장치를 comma connect (connect.comma.ai)에서 페어링하고 comma prime 제안을 요청하세요. Pair device @@ -832,11 +836,11 @@ This may take up to a minute. Wi-Fi - Wi-Fi + wifi ETH - 이더넷 + LAN 2G @@ -915,7 +919,7 @@ This may take up to a minute. up to date, last checked %1 - 최신 상태, 마지막으로 확인 %1 + 최신 상태 입니다, %1에 마지막으로 확인 DOWNLOAD @@ -958,7 +962,7 @@ This may take up to a minute. Username '%1' has no keys on GitHub - '%1'의 키가 GitHub에 없습니다 + 사용자 '%1'의 키가 GitHub에 없습니다 Request timed out @@ -966,7 +970,7 @@ This may take up to a minute. Username '%1' doesn't exist on GitHub - '%1'은 GitHub에 없습니다 + 사용자 '%1'는 GitHub에 없습니다 @@ -988,7 +992,7 @@ This may take up to a minute. Scroll to accept - 허용하려면 아래로 스크롤하세요 + 동의하려면 아래로 스크롤하세요 Agree @@ -1011,7 +1015,7 @@ This may take up to a minute. Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31 mph (50 km/h). - 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향지시등 없이 감지된 차선 위를 주행할 경우 차선이탈 경고를 표시합니다. + 차량이 50km/h(31mph) 이상의 속도로 주행하는 동안 방향지시등이 켜지지 않은 상태에서 차량이 감지된 차선을 벗어나면 차선이탈 경고를 합니다. Use Metric System @@ -1035,7 +1039,7 @@ This may take up to a minute. When enabled, pressing the accelerator pedal will disengage openpilot. - 활성화된 경우 가속 페달을 누르면 openpilot이 해제됩니다. + 활성화된 경우 가속 페달을 밟으면 openpilot이 해제됩니다. Show ETA in 24h Format @@ -1077,9 +1081,13 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 주행 시각화는 저속에서 도로를 향하는 광각 카메라로 전환되어 일부 회전을 더 잘 보여줍니다. 실험적 모드의 로고도 우측상단에 표시됩니다. + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 차량에 장착된 ACC가 롱컨트롤에 사용되기 때문에 현재 이 차량은 실험적 모드를 사용할 수 없습니다. + openpilot longitudinal control may come in a future update. - 오픈파일럿 롱컨트롤은 향후 업데이트에서 제공될 수 있습니다. + openpilot 롱컨트롤은 향후 업데이트에서 제공될 수 있습니다. openpilot Longitudinal Control (Alpha) @@ -1091,7 +1099,7 @@ This may take up to a minute. On this car, openpilot defaults to the car's built-in ACC instead of openpilot's longitudinal control. Enable this to switch to openpilot longitudinal control. Enabling Experimental mode is recommended when enabling openpilot longitudinal control alpha. - 이 차량은 openpilot 롱컨트롤 대신 차량의 내장 ACC로 기본 설정됩니다. openpilot 롱컨트롤으로 전환하려면 이 기능을 활성화하세요. openpilot 롱컨트롤 알파를 활성화하는경우 실험적 모드 활성화를 권장합니다. + 이 차량은 openpilot 롱컨트롤 대신 차량의 ACC로 기본 설정됩니다. openpilot 롱컨트롤으로 전환하려면 이 기능을 활성화하세요. openpilot 롱컨트롤 알파를 활성화하는경우 실험적 모드 활성화를 권장합니다. Aggressive @@ -1114,24 +1122,20 @@ This may take up to a minute. 표준 모드를 권장합니다. 공격적 모드에서는 openpilot은 앞차를 더 가까이 따라가며 가속과 감속을 더 공격적으로 사용합니다. 편안한 모드에서 openpilot은 선두 차량에서 더 멀리 떨어져 있습니다. - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. - + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + openpilot 롱컨트롤 알파 버전은 비 릴리스 분기에서 실험적 모드와 함께 테스트할 수 있습니다. - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. - + Navigate on openpilot + Navigate on openpilot (NOO) When navigation has a destination, openpilot will input the map information into the model. This generally improves behavior and allows openpilot to keep left or right appropriately at forks/exits and take turns. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected. - + 내비게이션에 목적지가 있으면 openpilot이 경로 정보를 모델에 입력합니다. 이것은 일반적으로 동작을 개선하고 openpilot이 분기점에서 적절하게 왼쪽 또는 오른쪽을 유지하고 회전할 수 있도록 합니다. 차선 변경 동작은 변경되지 않았으며 여전히 운전자에 의해 활성화됩니다. 이것은 알파 상태의 기능이니 사용에 주의해야 합니다. - Navigate on openpilot - + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + openpilot E2E 롱컨트롤 (알파) 토글을 활성화하여 실험적 모드를 허용합니다. @@ -1173,11 +1177,11 @@ This may take up to a minute. WiFiPromptWidget Setup Wi-Fi - Wi-Fi 설정 + wifi 설정 Connect to Wi-Fi to upload driving data and help improve openpilot - Wi-Fi에 연결하여 주행 데이터를 업로드하고 openpilot 개선에 참여하세요. + wifi에 연결하여 주행 데이터를 업로드하고 openpilot 개선에 참여하세요. Open Settings @@ -1208,7 +1212,7 @@ This may take up to a minute. Forget Wi-Fi Network "%1"? - wifi 네트워크 저장안함 "%1"? + "%1"를 저장하지 않겠습니까? Forget diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index f85601dfa29847..2cadd8bd6f5037 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -403,6 +403,10 @@ Waiting for GPS Esperando por GPS + + Waiting for internet + Esperando pela internet + MultiOptionDialog @@ -1081,6 +1085,10 @@ Isso pode levar até um minuto. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. A visualização da direção fará a transição para a câmera grande angular voltada para a estrada em baixas velocidades para mostrar melhor algumas curvas. O logotipo do modo Experimental também será exibido no canto superior direito. + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + O modo Experimental está atualmente indisponível para este carro já que o ACC original do carro é usado para controle longitudinal. + openpilot longitudinal control may come in a future update. O controle longitudinal openpilot poderá vir em uma atualização futura. @@ -1118,24 +1126,20 @@ Isso pode levar até um minuto. Neutro é o recomendado. No modo disputa o openpilot seguirá o carro da frente mais de perto e será mais agressivo com a aceleração e frenagem. No modo calmo o openpilot se manterá mais longe do carro da frente. - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. - + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + Uma versão embrionária do controle longitudinal openpilot pode ser testada em conjunto com o modo Experimental, em branches que não sejam de produção. - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. - + Navigate on openpilot + Navegação no openpilot When navigation has a destination, openpilot will input the map information into the model. This generally improves behavior and allows openpilot to keep left or right appropriately at forks/exits and take turns. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected. - + Quando a navegação tem um destino, o openpilot insere as informações do mapa no modelo. Isso geralmente melhora o comportamento e permite que o openpilot mantenha a esquerda ou a direita adequadamente nas bifurcações/saídas e entre nas curvas. O comportamento de mudança de faixa permanece inalterado e ainda é ativado pelo motorista. Este é um recurso de qualidade alfa; erros devem ser esperados. - Navigate on openpilot - + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + Habilite o controle longitudinal (embrionário) openpilot para permitir o modo Experimental. diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 57295b57114f0d..6b631b005c7c9e 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -402,6 +402,10 @@ Waiting for GPS 等待 GPS + + Waiting for internet + 等待网络连接 + MultiOptionDialog @@ -1075,6 +1079,10 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 当低速行驶时，驾驶视角将切换到前向广角摄像头，便于更完整地显示转向路径。右上角将显示试验模式图标。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 由于此车辆使用自带的ACC纵向控制，当前无法使用试验模式。 + openpilot longitudinal control may come in a future update. @@ -1112,15 +1120,11 @@ This may take up to a minute. - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. + Navigate on openpilot @@ -1128,7 +1132,7 @@ This may take up to a minute. - Navigate on openpilot + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 42459b942f79a7..c9a627771e5ee2 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -402,6 +402,10 @@ Waiting for GPS 等待 GPS + + Waiting for internet + 連接至網路中 + MultiOptionDialog @@ -1077,6 +1081,10 @@ This may take up to a minute. The driving visualization will transition to the road-facing wide-angle camera at low speeds to better show some turns. The Experimental mode logo will also be shown in the top right corner. 低速行駛時，將會切換成路側廣角鏡頭，以完整顯示轉彎路徑，右上角將出現實驗模式圖案。 + + Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control. + 因車輛使用內建ACC系統，無法在本車輛上啟動實驗模式。 + openpilot longitudinal control may come in a future update. openpilot 縱向控制可能會在未來的更新中提供。 @@ -1114,24 +1122,20 @@ This may take up to a minute. 推薦使用標準模式。在積極模式中，openpilot 會更靠近前車並在加速和剎車方面更積極。在舒適模式中，openpilot 會與前車保持較遠的距離。 - End-to-End Longitudinal Control is currently unavailable on this car since the car's stock ACC is used for longitudinal control. - 目前這輛車無法使用端到端縱向控制，因為車輛使用原廠ACC (自適應巡航控制系統) 進行縱向控制。 + An alpha version of openpilot longitudinal control can be tested, along with Experimental mode, on non-release branches. + 在正式 (release) 版以外的分支上可以測試 openpilot 縱向控制的 Alpha 版本，以及實驗模式。 - An alpha version of openpilot longitudinal control can be tested, along with End-to-End Longitudinal Control, on non-release branches. - 在非發佈版本的分支上，可以測試 openpilot 縱向控制的 Alpha 版本，包括端到端的縱向控制。 - - - Enable experimental longitudinal control to use End-to-End Longitudinal Control. - 啟用實驗性縱向控制以使用端到端縱向控制。 + Navigate on openpilot + When navigation has a destination, openpilot will input the map information into the model. This generally improves behavior and allows openpilot to keep left or right appropriately at forks/exits and take turns. Lane change behavior is unchanged and still activated by the driver. This is an alpha quality feature; mistakes should be expected. - 當導航有設定目的地時，openpilot 將會把地圖導航信息輸入模型。這通常會改善 openpilot 的行駛路徑使它能夠在分歧處/出口適當地靠左或靠右，並進行轉彎。車道變換行為保持不變，仍由駕駛員手動執行。這是一個 Alpha 版本的功能，請保持心理準備隨時有可能會出現錯誤。 + - Navigate on openpilot - openpilot 自動輔助駕駛 + Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. + diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 2cb7d1c13b6872..24df5914af5195 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -16,6 +16,7 @@ from common.basedir import BASEDIR from common.params import Params +from common.time import system_time_valid from system.hardware import AGNOS, HARDWARE from system.swaglog import cloudlog from selfdrive.controls.lib.alertmanager import set_offroad_alert @@ -256,14 +257,14 @@ def get_branch(self, path: str) -> str: def get_commit_hash(self, path: str = OVERLAY_MERGED) -> str: return run(["git", "rev-parse", "HEAD"], path).rstrip() - def set_params(self, failed_count: int, exception: Optional[str]) -> None: + def set_params(self, update_success: bool, failed_count: int, exception: Optional[str]) -> None: self.params.put("UpdateFailedCount", str(failed_count)) self.params.put_bool("UpdaterFetchAvailable", self.update_available) self.params.put("UpdaterAvailableBranches", ','.join(self.branches.keys())) last_update = datetime.datetime.utcnow() - if failed_count == 0: + if update_success: t = last_update.isoformat() self.params.put("LastUpdateTime", t.encode('utf8')) else: @@ -317,11 +318,12 @@ def get_description(basedir: str) -> str: else: extra_text = exception set_offroad_alert("Offroad_UpdateFailed", True, extra_text=extra_text) - elif dt.days > DAYS_NO_CONNECTIVITY_MAX and failed_count > 1: - set_offroad_alert("Offroad_ConnectivityNeeded", True) - elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: - remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1) - set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.") + elif failed_count > 0: + if dt.days > DAYS_NO_CONNECTIVITY_MAX: + set_offroad_alert("Offroad_ConnectivityNeeded", True) + elif dt.days > DAYS_NO_CONNECTIVITY_PROMPT: + remaining = max(DAYS_NO_CONNECTIVITY_MAX - dt.days, 1) + set_offroad_alert("Offroad_ConnectivityNeededPrompt", True, extra_text=f"{remaining} day{'' if remaining == 1 else 's'}.") def check_for_update(self) -> None: cloudlog.info("checking for updates") @@ -417,7 +419,7 @@ def main() -> None: params.put("InstallDate", t.encode('utf8')) updater = Updater() - update_failed_count = 0 # TODO: Load from param? + update_failed_count = 0 # TODO: Load from param? # no fetch on the first time wait_helper = WaitTimeHelper() @@ -437,7 +439,12 @@ def main() -> None: init_overlay() # ensure we have some params written soon after startup - updater.set_params(update_failed_count, exception) + updater.set_params(False, update_failed_count, exception) + + if not system_time_valid(): + wait_helper.sleep(60) + continue + update_failed_count += 1 # check for update @@ -466,7 +473,8 @@ def main() -> None: try: params.put("UpdaterState", "idle") - updater.set_params(update_failed_count, exception) + update_successful = (update_failed_count == 0) + updater.set_params(update_successful, update_failed_count, exception) except Exception: cloudlog.exception("uncaught updated exception while setting params, shouldn't happen") diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 4b33ced045dbc6..74ac77482e3be6 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -496,7 +496,7 @@ void CameraState::enqueue_buffer(int i, bool dp) { strcpy(sync_create.name, "NodeOutputPortFence"); ret = do_cam_control(multi_cam_state->cam_sync_fd, CAM_SYNC_CREATE, &sync_create, sizeof(sync_create)); if (ret != 0) { - LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj) + LOGE("failed to create fence: %d %d", ret, sync_create.sync_obj); } sync_objs[i] = sync_create.sync_obj; diff --git a/system/loggerd/deleter.py b/system/loggerd/deleter.py index 5fb2b9eb41f812..5e7b31f583dbae 100644 --- a/system/loggerd/deleter.py +++ b/system/loggerd/deleter.py @@ -2,15 +2,48 @@ import os import shutil import threading +from typing import List + from system.swaglog import cloudlog from system.loggerd.config import ROOT, get_available_bytes, get_available_percent from system.loggerd.uploader import listdir_by_creation +from system.loggerd.xattr_cache import getxattr MIN_BYTES = 5 * 1024 * 1024 * 1024 MIN_PERCENT = 10 DELETE_LAST = ['boot', 'crash'] +PRESERVE_ATTR_NAME = 'user.preserve' +PRESERVE_ATTR_VALUE = b'1' +PRESERVE_COUNT = 5 + + +def has_preserve_xattr(d: str) -> bool: + return getxattr(os.path.join(ROOT, d), PRESERVE_ATTR_NAME) == PRESERVE_ATTR_VALUE + + +def get_preserved_segments(dirs_by_creation: List[str]) -> List[str]: + preserved = [] + for n, d in enumerate(filter(has_preserve_xattr, reversed(dirs_by_creation))): + if n == PRESERVE_COUNT: + break + date_str, _, seg_str = d.rpartition("--") + + # ignore non-segment directories + if not date_str: + continue + try: + seg_num = int(seg_str) + except ValueError: + continue + + # preserve segment and its prior + preserved.append(d) + preserved.append(f"{date_str}--{seg_num - 1}") + + return preserved + def deleter_thread(exit_event): while not exit_event.is_set(): @@ -18,9 +51,13 @@ def deleter_thread(exit_event): out_of_percent = get_available_percent(default=MIN_PERCENT + 1) < MIN_PERCENT if out_of_percent or out_of_bytes: + dirs = listdir_by_creation(ROOT) + + # skip deleting most recent N preserved segments (and their prior segment) + preserved_dirs = get_preserved_segments(dirs) + # remove the earliest directory we can - dirs = sorted(listdir_by_creation(ROOT), key=lambda x: x in DELETE_LAST) - for delete_dir in dirs: + for delete_dir in sorted(dirs, key=lambda d: (d in DELETE_LAST, d in preserved_dirs)): delete_path = os.path.join(ROOT, delete_dir) if any(name.endswith(".lock") for name in os.listdir(delete_path)): diff --git a/system/loggerd/encoder/encoder.cc b/system/loggerd/encoder/encoder.cc index b45ad874a6ff2a..869b4617b3be70 100644 --- a/system/loggerd/encoder/encoder.cc +++ b/system/loggerd/encoder/encoder.cc @@ -1,10 +1,7 @@ -#include #include "system/loggerd/encoder/encoder.h" -VideoEncoder::~VideoEncoder() {} - -void VideoEncoder::publisher_init() { - // publish +VideoEncoder::VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height) + : encoder_info(encoder_info), in_width(in_width), in_height(in_height) { pm.reset(new PubMaster({encoder_info.publish_name})); } diff --git a/system/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h index d88274be842533..59ec4357ae81ec 100644 --- a/system/loggerd/encoder/encoder.h +++ b/system/loggerd/encoder/encoder.h @@ -14,14 +14,12 @@ class VideoEncoder { public: - VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height) - : encoder_info(encoder_info), in_width(in_width), in_height(in_height) {} - virtual ~VideoEncoder(); + VideoEncoder(const EncoderInfo &encoder_info, int in_width, int in_height); + virtual ~VideoEncoder() {}; virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; virtual void encoder_open(const char* path) = 0; virtual void encoder_close() = 0; - void publisher_init(); static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat); @@ -32,7 +30,5 @@ class VideoEncoder { private: // total frames encoded int cnt = 0; - - // publishing std::unique_ptr pm; }; diff --git a/system/loggerd/encoder/ffmpeg_encoder.cc b/system/loggerd/encoder/ffmpeg_encoder.cc index 26348988ff8e64..b73f4e8f5d0fb1 100644 --- a/system/loggerd/encoder/ffmpeg_encoder.cc +++ b/system/loggerd/encoder/ffmpeg_encoder.cc @@ -40,8 +40,6 @@ FfmpegEncoder::FfmpegEncoder(const EncoderInfo &encoder_info, int in_width, int if (in_width != encoder_info.frame_width || in_height != encoder_info.frame_height) { downscale_buf.resize(encoder_info.frame_width * encoder_info.frame_height * 3 / 2); } - - publisher_init(); } FfmpegEncoder::~FfmpegEncoder() { diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc index 13dae72d38b759..a319d414ca5a77 100644 --- a/system/loggerd/encoder/v4l_encoder.cc +++ b/system/loggerd/encoder/v4l_encoder.cc @@ -251,8 +251,6 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei for (unsigned int i = 0; i < BUF_IN_COUNT; i++) { free_buf_in.push(i); } - - publisher_init(); } void V4LEncoder::encoder_open(const char* path) { diff --git a/system/loggerd/loggerd.cc b/system/loggerd/loggerd.cc index d1d9596e02e71d..ced95958964c4c 100644 --- a/system/loggerd/loggerd.cc +++ b/system/loggerd/loggerd.cc @@ -1,3 +1,5 @@ +#include + #include #include "system/loggerd/encoder/encoder.h" @@ -170,6 +172,19 @@ int handle_encoder_msg(LoggerdState *s, Message *msg, std::string &name, struct return bytes_count; } +void handle_user_flag(LoggerdState *s) { + LOGW("preserving %s", s->segment_path); + +#ifdef __APPLE__ + int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0, 0); +#else + int ret = setxattr(s->segment_path, PRESERVE_ATTR_NAME, &PRESERVE_ATTR_VALUE, 1, 0); +#endif + if (ret) { + LOGE("setxattr %s failed for %s: %s", PRESERVE_ATTR_NAME, s->segment_path, strerror(errno)); + } +} + void loggerd_thread() { // setup messaging typedef struct QlogState { @@ -228,6 +243,10 @@ void loggerd_thread() { while (!do_exit && (msg = sock->receive(true))) { const bool in_qlog = qs.freq != -1 && (qs.counter++ % qs.freq == 0); + if (qs.name == "userFlag") { + handle_user_flag(&s); + } + if (qs.encoder) { s.last_camera_seen_tms = millis_since_boot(); bytes_count += handle_encoder_msg(&s, msg, qs.name, remote_encoders[sock], encoder_infos_dict[qs.name]); diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index 0226e631d5e07d..4100f12f8db755 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -24,6 +24,9 @@ const int MAIN_BITRATE = 10000000; const bool LOGGERD_TEST = getenv("LOGGERD_TEST"); const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60; +constexpr char PRESERVE_ATTR_NAME[] = "user.preserve"; +constexpr char PRESERVE_ATTR_VALUE = '1'; + class EncoderInfo { public: const char *publish_name; @@ -60,7 +63,7 @@ const EncoderInfo main_wide_road_encoder_info = { INIT_ENCODE_FUNCTIONS(WideRoadEncode), }; const EncoderInfo main_driver_encoder_info = { - .publish_name = "driverEncodeData", + .publish_name = "driverEncodeData", .filename = "dcamera.hevc", .record = Params().getBool("RecordFront"), INIT_ENCODE_FUNCTIONS(DriverEncode), @@ -78,24 +81,24 @@ const EncoderInfo qcam_encoder_info = { const LogCameraInfo road_camera_info{ - .thread_name = "road_cam_encoder", - .type = RoadCam, - .stream_type = VISION_STREAM_ROAD, - .encoder_infos = {main_road_encoder_info, qcam_encoder_info} - }; + .thread_name = "road_cam_encoder", + .type = RoadCam, + .stream_type = VISION_STREAM_ROAD, + .encoder_infos = {main_road_encoder_info, qcam_encoder_info} +}; const LogCameraInfo wide_road_camera_info{ - .thread_name = "wide_road_cam_encoder", - .type = WideRoadCam, - .stream_type = VISION_STREAM_WIDE_ROAD, - .encoder_infos = {main_wide_road_encoder_info} - }; + .thread_name = "wide_road_cam_encoder", + .type = WideRoadCam, + .stream_type = VISION_STREAM_WIDE_ROAD, + .encoder_infos = {main_wide_road_encoder_info} +}; const LogCameraInfo driver_camera_info{ - .thread_name = "driver_cam_encoder", - .type = DriverCam, - .stream_type = VISION_STREAM_DRIVER, - .encoder_infos = {main_driver_encoder_info} - }; + .thread_name = "driver_cam_encoder", + .type = DriverCam, + .stream_type = VISION_STREAM_DRIVER, + .encoder_infos = {main_driver_encoder_info} +}; const LogCameraInfo cameras_logged[] = {road_camera_info, wide_road_camera_info, driver_camera_info}; diff --git a/system/loggerd/tests/loggerd_tests_common.py b/system/loggerd/tests/loggerd_tests_common.py index 6d1303ca6cdf9d..7d71516dfede75 100644 --- a/system/loggerd/tests/loggerd_tests_common.py +++ b/system/loggerd/tests/loggerd_tests_common.py @@ -7,10 +7,12 @@ from pathlib import Path from typing import Optional +import system.loggerd.deleter as deleter import system.loggerd.uploader as uploader +from system.loggerd.xattr_cache import setxattr -def create_random_file(file_path: Path, size_mb: float, lock: bool = False, xattr: Optional[bytes] = None) -> None: +def create_random_file(file_path: Path, size_mb: float, lock: bool = False, upload_xattr: Optional[bytes] = None) -> None: file_path.parent.mkdir(parents=True, exist_ok=True) if lock: @@ -25,8 +27,8 @@ def create_random_file(file_path: Path, size_mb: float, lock: bool = False, xatt for _ in range(chunks): f.write(data) - if xattr is not None: - uploader.setxattr(str(file_path), uploader.UPLOAD_ATTR_NAME, xattr) + if upload_xattr is not None: + setxattr(str(file_path), uploader.UPLOAD_ATTR_NAME, upload_xattr) class MockResponse(): def __init__(self, text, status_code): @@ -105,8 +107,11 @@ def tearDown(self): raise def make_file_with_data(self, f_dir: str, fn: str, size_mb: float = .1, lock: bool = False, - xattr: Optional[bytes] = None) -> Path: + upload_xattr: Optional[bytes] = None, preserve_xattr: Optional[bytes] = None) -> Path: file_path = self.root / f_dir / fn - create_random_file(file_path, size_mb, lock, xattr) + create_random_file(file_path, size_mb, lock, upload_xattr) + + if preserve_xattr is not None: + setxattr(str(file_path.parent), deleter.PRESERVE_ATTR_NAME, preserve_xattr) return file_path diff --git a/system/loggerd/tests/test_deleter.py b/system/loggerd/tests/test_deleter.py index 596545cdeb3b6f..9474b30f82c448 100755 --- a/system/loggerd/tests/test_deleter.py +++ b/system/loggerd/tests/test_deleter.py @@ -3,9 +3,11 @@ import threading import unittest from collections import namedtuple +from pathlib import Path +from typing import Sequence -from common.timeout import Timeout, TimeoutException import system.loggerd.deleter as deleter +from common.timeout import Timeout, TimeoutException from system.loggerd.tests.loggerd_tests_common import UploaderTestCase Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize']) @@ -37,30 +39,59 @@ def test_delete(self): self.start_thread() - with Timeout(5, "Timeout waiting for file to be deleted"): - while f_path.exists(): - time.sleep(0.01) - self.join_thread() - - self.assertFalse(f_path.exists(), "File not deleted") + try: + with Timeout(2, "Timeout waiting for file to be deleted"): + while f_path.exists(): + time.sleep(0.01) + finally: + self.join_thread() - def test_delete_files_in_create_order(self): - f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type) - time.sleep(1) - self.seg_num += 1 - self.seg_dir = self.seg_format.format(self.seg_num) - f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type) + def assertDeleteOrder(self, f_paths: Sequence[Path], timeout: int = 5) -> None: + deleted_order = [] self.start_thread() + try: + with Timeout(timeout, "Timeout waiting for files to be deleted"): + while True: + for f in f_paths: + if not f.exists() and f not in deleted_order: + deleted_order.append(f) + if len(deleted_order) == len(f_paths): + break + time.sleep(0.01) + except TimeoutException: + print("Not deleted:", [f for f in f_paths if f not in deleted_order]) + raise + finally: + self.join_thread() - with Timeout(5, "Timeout waiting for file to be deleted"): - while f_path_1.exists() and f_path_2.exists(): - time.sleep(0.01) - - self.join_thread() - - self.assertFalse(f_path_1.exists(), "Older file not deleted") - self.assertTrue(f_path_2.exists(), "Newer file deleted before older file") + self.assertEqual(deleted_order, f_paths, "Files not deleted in expected order") + + def test_delete_order(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(1), self.f_type), + self.make_file_with_data(self.seg_format2.format(0), self.f_type), + ]) + + def test_delete_many_preserved(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(1), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), + self.make_file_with_data(self.seg_format.format(2), self.f_type), + ] + [ + self.make_file_with_data(self.seg_format2.format(i), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE) + for i in range(5) + ]) + + def test_delete_last(self): + self.assertDeleteOrder([ + self.make_file_with_data(self.seg_format.format(1), self.f_type), + self.make_file_with_data(self.seg_format2.format(0), self.f_type), + self.make_file_with_data(self.seg_format.format(0), self.f_type, preserve_xattr=deleter.PRESERVE_ATTR_VALUE), + self.make_file_with_data("boot", self.seg_format[:-4]), + self.make_file_with_data("crash", self.seg_format2[:-4]), + ]) def test_no_delete_when_available_space(self): f_path = self.make_file_with_data(self.seg_dir, self.f_type) @@ -70,15 +101,10 @@ def test_no_delete_when_available_space(self): self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size) self.start_thread() - - try: - with Timeout(2, "Timeout waiting for file to be deleted"): - while f_path.exists(): - time.sleep(0.01) - except TimeoutException: - pass - finally: - self.join_thread() + start_time = time.monotonic() + while f_path.exists() and time.monotonic() - start_time < 2: + time.sleep(0.01) + self.join_thread() self.assertTrue(f_path.exists(), "File deleted with available space") @@ -86,15 +112,10 @@ def test_no_delete_with_lock_file(self): f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True) self.start_thread() - - try: - with Timeout(2, "Timeout waiting for file to be deleted"): - while f_path.exists(): - time.sleep(0.01) - except TimeoutException: - pass - finally: - self.join_thread() + start_time = time.monotonic() + while f_path.exists() and time.monotonic() - start_time < 2: + time.sleep(0.01) + self.join_thread() self.assertTrue(f_path.exists(), "File deleted when locked") diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py index a2166016e024c3..7365b256d2e974 100755 --- a/system/loggerd/tests/test_loggerd.py +++ b/system/loggerd/tests/test_loggerd.py @@ -8,6 +8,7 @@ import unittest from collections import defaultdict from pathlib import Path +from typing import Dict, List import cereal.messaging as messaging from cereal import log @@ -16,6 +17,8 @@ from common.params import Params from common.timeout import Timeout from system.loggerd.config import ROOT +from system.loggerd.xattr_cache import getxattr +from system.loggerd.deleter import PRESERVE_ATTR_NAME, PRESERVE_ATTR_VALUE from selfdrive.manager.process_config import managed_processes from system.version import get_version from tools.lib.logreader import LogReader @@ -71,6 +74,30 @@ def _check_sentinel(self, msgs, route): end_type = SentinelType.endOfRoute if route else SentinelType.endOfSegment self.assertTrue(msgs[-1].sentinel.type == end_type) + def _publish_random_messages(self, services: List[str]) -> Dict[str, list]: + pm = messaging.PubMaster(services) + + managed_processes["loggerd"].start() + for s in services: + self.assertTrue(pm.wait_for_readers_to_update(s, timeout=5)) + + sent_msgs = defaultdict(list) + for _ in range(random.randint(2, 10) * 100): + for s in services: + try: + m = messaging.new_message(s) + except Exception: + m = messaging.new_message(s, random.randint(2, 10)) + pm.send(s, m) + sent_msgs[s].append(m) + time.sleep(0.01) + + for s in services: + self.assertTrue(pm.wait_for_readers_to_update(s, timeout=5)) + managed_processes["loggerd"].stop() + + return sent_msgs + def test_init_data_values(self): os.environ["CLEAN"] = random.choice(["0", "1"]) @@ -193,29 +220,7 @@ def test_qlog(self): services = random.sample(qlog_services, random.randint(2, min(10, len(qlog_services)))) + \ random.sample(no_qlog_services, random.randint(2, min(10, len(no_qlog_services)))) - - pm = messaging.PubMaster(services) - - # sleep enough for the first poll to time out - # TODO: fix loggerd bug dropping the msgs from the first poll - managed_processes["loggerd"].start() - for s in services: - while not pm.all_readers_updated(s): - time.sleep(0.1) - - sent_msgs = defaultdict(list) - for _ in range(random.randint(2, 10) * 100): - for s in services: - try: - m = messaging.new_message(s) - except Exception: - m = messaging.new_message(s, random.randint(2, 10)) - pm.send(s, m) - sent_msgs[s].append(m) - time.sleep(0.01) - - time.sleep(1) - managed_processes["loggerd"].stop() + sent_msgs = self._publish_random_messages(services) qlog_path = os.path.join(self._get_latest_log_dir(), "qlog") lr = list(LogReader(qlog_path)) @@ -241,27 +246,7 @@ def test_qlog(self): def test_rlog(self): services = random.sample(CEREAL_SERVICES, random.randint(5, 10)) - pm = messaging.PubMaster(services) - - # sleep enough for the first poll to time out - # TODO: fix loggerd bug dropping the msgs from the first poll - managed_processes["loggerd"].start() - for s in services: - while not pm.all_readers_updated(s): - time.sleep(0.1) - - sent_msgs = defaultdict(list) - for _ in range(random.randint(2, 10) * 100): - for s in services: - try: - m = messaging.new_message(s) - except Exception: - m = messaging.new_message(s, random.randint(2, 10)) - pm.send(s, m) - sent_msgs[s].append(m) - - time.sleep(2) - managed_processes["loggerd"].stop() + sent_msgs = self._publish_random_messages(services) lr = list(LogReader(os.path.join(self._get_latest_log_dir(), "rlog"))) @@ -276,6 +261,20 @@ def test_rlog(self): sent.clear_write_flag() self.assertEqual(sent.to_bytes(), m.as_builder().to_bytes()) + def test_preserving_flagged_segments(self): + services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) | {"userFlag"} + self._publish_random_messages(services) + + segment_dir = self._get_latest_log_dir() + self.assertEqual(getxattr(segment_dir, PRESERVE_ATTR_NAME), PRESERVE_ATTR_VALUE) + + def test_not_preserving_unflagged_segments(self): + services = set(random.sample(CEREAL_SERVICES, random.randint(5, 10))) - {"userFlag"} + self._publish_random_messages(services) + + segment_dir = self._get_latest_log_dir() + self.assertIsNone(getxattr(segment_dir, PRESERVE_ATTR_NAME)) + if __name__ == "__main__": unittest.main() diff --git a/system/loggerd/tests/test_uploader.py b/system/loggerd/tests/test_uploader.py index 9346b770a9ca3e..580d1efae209c8 100755 --- a/system/loggerd/tests/test_uploader.py +++ b/system/loggerd/tests/test_uploader.py @@ -55,10 +55,10 @@ def join_thread(self): def gen_files(self, lock=False, xattr: Optional[bytes] = None, boot=True) -> List[Path]: f_paths = [] for t in ["qlog", "rlog", "dcamera.hevc", "fcamera.hevc"]: - f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock, xattr=xattr)) + f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock, upload_xattr=xattr)) if boot: - f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock, xattr=xattr)) + f_paths.append(self.make_file_with_data("boot", f"{self.seg_dir}", 1, lock=lock, upload_xattr=xattr)) return f_paths def gen_order(self, seg1: List[int], seg2: List[int], boot=True) -> List[str]: diff --git a/system/sensord/rawgps/rawgpsd.py b/system/sensord/rawgps/rawgpsd.py index 5eab964b4e5057..3f8fd8c67a6185 100755 --- a/system/sensord/rawgps/rawgpsd.py +++ b/system/sensord/rawgps/rawgpsd.py @@ -8,7 +8,7 @@ import pycurl import subprocess from datetime import datetime -from multiprocessing import Process +from multiprocessing import Process, Event from typing import NoReturn, Optional from struct import unpack_from, calcsize, pack @@ -31,6 +31,7 @@ DEBUG = int(os.getenv("DEBUG", "0"))==1 ASSIST_DATA_FILE = '/tmp/xtra3grc.bin' +ASSIST_DATA_FILE_DOWNLOAD = ASSIST_DATA_FILE + '.download' ASSISTANCE_URL = 'http://xtrapath3.izatcloud.net/xtra3grc.bin' LOG_TYPES = [ @@ -87,12 +88,13 @@ def try_setup_logs(diag, log_types): - for _ in range(3): + 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=}") @@ -126,7 +128,7 @@ def download_assistance(): cloudlog.error("Qcom assistance data larger than expected") return - with open(ASSIST_DATA_FILE, 'wb') as fp: + with open(ASSIST_DATA_FILE_DOWNLOAD, 'wb') as fp: c = pycurl.Curl() c.setopt(pycurl.URL, ASSISTANCE_URL) c.setopt(pycurl.CONNECTTIMEOUT, 5) @@ -134,10 +136,17 @@ def download_assistance(): c.setopt(pycurl.WRITEDATA, fp) c.perform() c.close() + os.rename(ASSIST_DATA_FILE_DOWNLOAD, ASSIST_DATA_FILE) except pycurl.error: cloudlog.exception("Failed to download assistance file") return +def downloader_loop(event): + if os.path.exists(ASSIST_DATA_FILE): + os.remove(ASSIST_DATA_FILE) + while not os.path.exists(ASSIST_DATA_FILE) and not event.is_set(): + download_assistance() + time.sleep(10) def inject_assistance(): try: @@ -237,7 +246,9 @@ def main() -> NoReturn: wait_for_modem() - assist_fetch_proc = None + stop_download_event = Event() + assist_fetch_proc = Process(target=downloader_loop, args=(stop_download_event,)) + assist_fetch_proc.start() def cleanup(proc): cloudlog.warning("caught sig disabling quectel gps") gpio_set(GPIO.UBLOX_PWR_EN, False) @@ -249,11 +260,9 @@ def cleanup(proc): # connect to modem diag = ModemDiag() - download_assistance() - want_assistance = not os.path.exists(ASSIST_DATA_FILE) setup_quectel(diag) + want_assistance = True current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow())) - last_fetch_time = time.monotonic() cloudlog.warning("quectel setup done") gpio_init(GPIO.UBLOX_PWR_EN, True) gpio_set(GPIO.UBLOX_PWR_EN, True) @@ -267,13 +276,6 @@ def cleanup(proc): want_assistance = False else: os.remove(ASSIST_DATA_FILE) - if want_assistance and time.monotonic() - last_fetch_time > 10: - if assist_fetch_proc is None or not assist_fetch_proc.is_alive(): # type: ignore - cloudlog.warning("fetching assistance data") - assist_fetch_proc = Process(target=download_assistance) - assist_fetch_proc.start() - last_fetch_time = time.monotonic() - opcode, payload = diag.recv() if opcode != DIAG_LOG_F: @@ -358,12 +360,14 @@ def cleanup(proc): gps.source = log.GpsLocationData.SensorSource.qcomdiag gps.vNED = vNED gps.verticalAccuracy = report["q_FltVdop"] - gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi + gps.bearingAccuracyDeg = report["q_FltHeadingUncRad"] * 180/math.pi if (report["q_FltHeadingUncRad"] != 0) else 180 gps.speedAccuracy = math.sqrt(sum([x**2 for x in vNEDsigma])) # quectel gps verticalAccuracy is clipped to 500, set invalid if so gps.flags = 1 if gps.verticalAccuracy != 500 else 0 if gps.flags: want_assistance = False + stop_download_event.set() + pm.send('gpsLocation', msg) diff --git a/system/sensord/rawgps/test_rawgps.py b/system/sensord/rawgps/test_rawgps.py index 2132b77009ffe1..f87a552d666630 100755 --- a/system/sensord/rawgps/test_rawgps.py +++ b/system/sensord/rawgps/test_rawgps.py @@ -62,7 +62,7 @@ def test_startup_time(self): if i == 1: os.system("sudo systemctl stop systemd-resolved") managed_processes['rawgpsd'].start() - self._wait_for_output(7) + self._wait_for_output(10) assert self.sm.updated['qcomGnss'] managed_processes['rawgpsd'].stop() diff --git a/system/sensord/sensors/bmx055_accel.cc b/system/sensord/sensors/bmx055_accel.cc index 78f5d64e7969f8..0c48d1e3ba9daa 100644 --- a/system/sensord/sensors/bmx055_accel.cc +++ b/system/sensord/sensors/bmx055_accel.cc @@ -44,7 +44,7 @@ int BMX055_Accel::shutdown() { // enter deep suspend mode (lowest power mode) int ret = set_register(BMX055_ACCEL_I2C_REG_PMU, BMX055_ACCEL_DEEP_SUSPEND); if (ret < 0) { - LOGE("Could not move BMX055 ACCEL in deep suspend mode!") + LOGE("Could not move BMX055 ACCEL in deep suspend mode!"); } return ret; diff --git a/system/sensord/sensors/bmx055_gyro.cc b/system/sensord/sensors/bmx055_gyro.cc index 2a938044f31f09..ba41f3b47c0e06 100644 --- a/system/sensord/sensors/bmx055_gyro.cc +++ b/system/sensord/sensors/bmx055_gyro.cc @@ -54,7 +54,7 @@ int BMX055_Gyro::shutdown() { // enter deep suspend mode (lowest power mode) int ret = set_register(BMX055_GYRO_I2C_REG_LPM1, BMX055_GYRO_DEEP_SUSPEND); if (ret < 0) { - LOGE("Could not move BMX055 GYRO in deep suspend mode!") + LOGE("Could not move BMX055 GYRO in deep suspend mode!"); } return ret; diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc index 2f0d10412c6663..7716ce25c0d03e 100644 --- a/system/sensord/sensors/bmx055_magn.cc +++ b/system/sensord/sensors/bmx055_magn.cc @@ -150,7 +150,7 @@ int BMX055_Magn::shutdown() { // move to suspend mode int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0); if (ret < 0) { - LOGE("Could not move BMX055 MAGN in suspend mode!") + LOGE("Could not move BMX055 MAGN in suspend mode!"); } return ret; diff --git a/system/sensord/sensors/lsm6ds3_accel.cc b/system/sensord/sensors/lsm6ds3_accel.cc index c8eeb6e5dc15e1..2a09702c962d4f 100644 --- a/system/sensord/sensors/lsm6ds3_accel.cc +++ b/system/sensord/sensors/lsm6ds3_accel.cc @@ -194,7 +194,7 @@ int LSM6DS3_Accel::shutdown() { value &= ~(LSM6DS3_ACCEL_INT1_DRDY_XL); ret = set_register(LSM6DS3_ACCEL_I2C_REG_INT1_CTRL, value); if (ret < 0) { - LOGE("Could not disable lsm6ds3 acceleration interrupt!") + LOGE("Could not disable lsm6ds3 acceleration interrupt!"); goto fail; } @@ -208,7 +208,7 @@ int LSM6DS3_Accel::shutdown() { value &= 0x0F; ret = set_register(LSM6DS3_ACCEL_I2C_REG_CTRL1_XL, value); if (ret < 0) { - LOGE("Could not power-down lsm6ds3 accelerometer!") + LOGE("Could not power-down lsm6ds3 accelerometer!"); goto fail; } diff --git a/system/sensord/sensors/lsm6ds3_gyro.cc b/system/sensord/sensors/lsm6ds3_gyro.cc index d9f5b4cb6a6f55..9bc43485af67f3 100644 --- a/system/sensord/sensors/lsm6ds3_gyro.cc +++ b/system/sensord/sensors/lsm6ds3_gyro.cc @@ -177,7 +177,7 @@ int LSM6DS3_Gyro::shutdown() { value &= ~(LSM6DS3_GYRO_INT1_DRDY_G); ret = set_register(LSM6DS3_GYRO_I2C_REG_INT1_CTRL, value); if (ret < 0) { - LOGE("Could not disable lsm6ds3 gyroscope interrupt!") + LOGE("Could not disable lsm6ds3 gyroscope interrupt!"); goto fail; } @@ -191,7 +191,7 @@ int LSM6DS3_Gyro::shutdown() { value &= 0x0F; ret = set_register(LSM6DS3_GYRO_I2C_REG_CTRL2_G, value); if (ret < 0) { - LOGE("Could not power-down lsm6ds3 gyroscope!") + LOGE("Could not power-down lsm6ds3 gyroscope!"); goto fail; } diff --git a/system/sensord/sensors/mmc5603nj_magn.cc b/system/sensord/sensors/mmc5603nj_magn.cc index cdb82a4a120876..048095786eca9d 100644 --- a/system/sensord/sensors/mmc5603nj_magn.cc +++ b/system/sensord/sensors/mmc5603nj_magn.cc @@ -63,7 +63,7 @@ int MMC5603NJ_Magn::shutdown() { return ret; fail: - LOGE("Could not disable mmc5603nj auto set reset") + LOGE("Could not disable mmc5603nj auto set reset"); return ret; } diff --git a/system/swaglog.py b/system/swaglog.py index 953b9a93b20d81..28beb5a4d1a9f2 100644 --- a/system/swaglog.py +++ b/system/swaglog.py @@ -1,6 +1,7 @@ import logging import os import time +import warnings from pathlib import Path from logging.handlers import BaseRotatingHandler @@ -90,6 +91,8 @@ def connect(self): def emit(self, record): if os.getpid() != self.pid: + # TODO suppresses warning about forking proc with zmq socket, fix root cause + warnings.filterwarnings("ignore", category=ResourceWarning, message="unclosed.*") self.connect() msg = self.format(record).rstrip('\n') diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc index 92a2ba678c4e4b..ee8447fa6443fb 100644 --- a/system/ubloxd/ublox_msg.cc +++ b/system/ubloxd/ublox_msg.cc @@ -390,7 +390,7 @@ kj::Array UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_ eph.setP4(data->p4()); eph.setSvURA(glonass_URA_lookup.at(data->f_t())); if (msg->sv_id() != data->n()) { - LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n()) + LOGE("SV_ID != SLOT_NUMBER: %d %d", msg->sv_id(), data->n()); } eph.setSvType(data->m()); } diff --git a/tools/cabana/messageswidget.cc b/tools/cabana/messageswidget.cc index 0b2c635f88ecf9..fe4c7afb039bfe 100644 --- a/tools/cabana/messageswidget.cc +++ b/tools/cabana/messageswidget.cc @@ -28,7 +28,7 @@ MessagesWidget::MessagesWidget(QWidget *parent) : QWidget(parent) { // message table view = new MessageView(this); model = new MessageListModel(this); - header = new MessageViewHeader(this, model); + header = new MessageViewHeader(this); auto delegate = new MessageBytesDelegate(view, settings.multiple_lines_bytes); view->setItemDelegate(delegate); @@ -446,7 +446,7 @@ void MessageView::headerContextMenuEvent(const QPoint &pos) { menu->popup(header()->mapToGlobal(pos)); } -MessageViewHeader::MessageViewHeader(QWidget *parent, MessageListModel *model) : model(model), QHeaderView(Qt::Horizontal, parent) { +MessageViewHeader::MessageViewHeader(QWidget *parent) : QHeaderView(Qt::Horizontal, parent) { QObject::connect(this, &QHeaderView::sectionResized, this, &MessageViewHeader::updateHeaderPositions); QObject::connect(this, &QHeaderView::sectionMoved, this, &MessageViewHeader::updateHeaderPositions); } @@ -485,7 +485,7 @@ void MessageViewHeader::updateHeaderPositions() { void MessageViewHeader::updateGeometries() { for (int i = 0; i < count(); i++) { if (!editors[i]) { - QString column_name = model->headerData(i, Qt::Horizontal, Qt::DisplayRole).toString(); + QString column_name = model()->headerData(i, Qt::Horizontal, Qt::DisplayRole).toString(); editors[i] = new QLineEdit(this); editors[i]->setClearButtonEnabled(true); editors[i]->setPlaceholderText(tr("Filter %1").arg(column_name)); diff --git a/tools/cabana/messageswidget.h b/tools/cabana/messageswidget.h index 57011e51b4640c..2e165f5b7c93be 100644 --- a/tools/cabana/messageswidget.h +++ b/tools/cabana/messageswidget.h @@ -69,7 +69,7 @@ class MessageViewHeader : public QHeaderView { Q_OBJECT public: - MessageViewHeader(QWidget *parent, MessageListModel *model); + MessageViewHeader(QWidget *parent); void updateHeaderPositions(); void updateGeometries() override; @@ -85,8 +85,6 @@ public slots: void updateFilters(); QMap editors; - QMap> values; - MessageListModel *model; }; class MessagesWidget : public QWidget { diff --git a/tools/cabana/signalview.cc b/tools/cabana/signalview.cc index defa4e7b09b6c5..ce10576ffe7a2f 100644 --- a/tools/cabana/signalview.cc +++ b/tools/cabana/signalview.cc @@ -275,7 +275,7 @@ QSize SignalItemDelegate::sizeHint(const QStyleOptionViewItem &option, const QMo int width = option.widget->size().width() / 2; if (index.column() == 0) { int spacing = option.widget->style()->pixelMetric(QStyle::PM_TreeViewIndentation) + color_label_width + 8; - auto text = index.data(Qt::DisplayRole).toString();; + auto text = index.data(Qt::DisplayRole).toString(); auto item = (SignalModel::Item *)index.internalPointer(); if (item->type == SignalModel::Item::Sig && item->sig->type != cabana::Signal::Type::Normal) { text += item->sig->type == cabana::Signal::Type::Multiplexor ? QString(" M ") : QString(" m%1 ").arg(item->sig->multiplex_value); diff --git a/tools/cabana/util.cc b/tools/cabana/util.cc index e7ae4a2d0d7e5e..31a44867726426 100644 --- a/tools/cabana/util.cc +++ b/tools/cabana/util.cc @@ -54,12 +54,6 @@ MessageBytesDelegate::MessageBytesDelegate(QObject *parent, bool multiple_lines) byte_size = QFontMetrics(fixed_font).size(Qt::TextSingleLine, "00 ") + QSize(0, 2); } -void MessageBytesDelegate::setMultipleLines(bool v) { - if (std::exchange(multiple_lines, v) != multiple_lines) { - std::fill_n(size_cache, std::size(size_cache), QSize{}); - } -} - int MessageBytesDelegate::widthForBytes(int n) const { int h_margin = QApplication::style()->pixelMetric(QStyle::PM_FocusFrameHMargin) + 1; return n * byte_size.width() + h_margin * 2; @@ -73,19 +67,8 @@ QSize MessageBytesDelegate::sizeHint(const QStyleOptionViewItem &option, const Q } int n = data.toByteArray().size(); assert(n >= 0 && n <= 64); - - QSize size = size_cache[n]; - if (size.isEmpty()) { - if (!multiple_lines) { - size.setWidth(widthForBytes(n)); - size.setHeight(byte_size.height() + 2 * v_margin); - } else { - size.setWidth(widthForBytes(8)); - size.setHeight(byte_size.height() * std::max(1, n / 8) + 2 * v_margin); - } - size_cache[n] = size; - } - return size; + return !multiple_lines ? QSize{widthForBytes(n), byte_size.height() + 2 * v_margin} + : QSize{widthForBytes(8), byte_size.height() * std::max(1, n / 8) + 2 * v_margin}; } void MessageBytesDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const { @@ -238,7 +221,7 @@ void setTheme(int theme) { new_palette.setColor(QPalette::BrightText, QColor("#f0f0f0")); new_palette.setColor(QPalette::Disabled, QPalette::ButtonText, QColor("#777777")); new_palette.setColor(QPalette::Disabled, QPalette::WindowText, QColor("#777777")); - new_palette.setColor(QPalette::Disabled, QPalette::Text, QColor("#777777"));; + new_palette.setColor(QPalette::Disabled, QPalette::Text, QColor("#777777")); new_palette.setColor(QPalette::Light, QColor("#777777")); new_palette.setColor(QPalette::Dark, QColor("#353535")); } else { diff --git a/tools/cabana/util.h b/tools/cabana/util.h index 25dc245fe06ba0..a53abfd0bb3ca9 100644 --- a/tools/cabana/util.h +++ b/tools/cabana/util.h @@ -67,15 +67,14 @@ class MessageBytesDelegate : public QStyledItemDelegate { MessageBytesDelegate(QObject *parent, bool multiple_lines = false); void paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &index) const override; QSize sizeHint(const QStyleOptionViewItem &option, const QModelIndex &index) const override; - void setMultipleLines(bool v); - int widthForBytes(int n) const; bool multipleLines() const { return multiple_lines; } + void setMultipleLines(bool v) { multiple_lines = v; } + int widthForBytes(int n) const; private: QFont fixed_font; QSize byte_size = {}; bool multiple_lines = false; - mutable QSize size_cache[65] = {}; }; inline QString toHex(const QByteArray &dat) { return dat.toHex(' ').toUpper(); } diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index d66fe793061ac7..1c182989695dc4 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -10,7 +10,7 @@ from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, tici_f_frame_size, tici_f_focal_length, get_view_frame_from_calib_frame) -from selfdrive.controls.lib.radar_helpers import RADAR_TO_CAMERA +from selfdrive.controls.radard import RADAR_TO_CAMERA RED = (255, 0, 0) From 05c7386cc4c98a0a668747977e4c70d6bd74d4d4 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 23:32:12 -0700 Subject: [PATCH 36/45] bump submodules --- opendbc | 2 +- panda | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/opendbc b/opendbc index 3ef35ed2298a3a..5880fbbccf5a67 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 3ef35ed2298a3a9d199f9145409547710065884c +Subproject commit 5880fbbccf5a670631b51836f20e446de643795a diff --git a/panda b/panda index 624927486fef2a..5233d3a7a7c0dd 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 624927486fef2a58408efbbd6a9c05dcd3131214 +Subproject commit 5233d3a7a7c0dd12bcaf9bf17eff5bd4e31d3186 From d891f912bd6378fbed5773aabf7ffc01ccf90154 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 20 Jul 2023 23:36:58 -0700 Subject: [PATCH 37/45] bump --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index aed9fd278a7048..6f7102581f57eb 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit aed9fd278a704816aba11f4473aafefc281ed2bc +Subproject commit 6f7102581f57eb5074b816cc2cfd984218916773 From 9462f14dcfda4b65f14c192865464fca295bd423 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 00:00:59 -0700 Subject: [PATCH 38/45] bump panda --- panda | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/panda b/panda index 4b38cace8a1770..4fbba35a7051d6 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4b38cace8a1770de265a3ee8d636d55131cfc12e +Subproject commit 4fbba35a7051d6bd390ad2a4b70bee300a8cd53b From cc307506f33325abfd64664a9db1ab9a220822f7 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 00:12:06 -0700 Subject: [PATCH 39/45] bump --- panda | 2 +- selfdrive/car/subaru/carcontroller.py | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/panda b/panda index 4fbba35a7051d6..3b96f0d160cf24 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 4fbba35a7051d6bd390ad2a4b70bee300a8cd53b +Subproject commit 3b96f0d160cf24937bdc36c1a49b8c24cc1cd7e2 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 094a097d5e97fa..7a2ebba092ff7f 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -21,6 +21,8 @@ def update(self, CC, CS, now_nanos): hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel + new_actuators = actuators.copy() + can_sends = [] # *** steering *** @@ -32,6 +34,8 @@ def update(self, CC, CS, now_nanos): apply_steer = CS.out.steeringAngleDeg can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) + + new_actuators.steeringAngleDeg = apply_steer else: apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) @@ -47,6 +51,9 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) + + new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last self.apply_steer_last = apply_steer @@ -86,9 +93,5 @@ def update(self, CC, CS, now_nanos): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert)) - new_actuators = actuators.copy() - new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX - new_actuators.steerOutputCan = self.apply_steer_last - self.frame += 1 return new_actuators, can_sends From 27eda9f16587445a51c3b36a48e551891dfd8edd Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 00:22:08 -0700 Subject: [PATCH 40/45] bump cereal --- cereal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal b/cereal index 6f7102581f57eb..aed9fd278a7048 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 6f7102581f57eb5074b816cc2cfd984218916773 +Subproject commit aed9fd278a704816aba11f4473aafefc281ed2bc From a72e651af461723cbd88f670baeec017c18e0570 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 00:26:43 -0700 Subject: [PATCH 41/45] comments --- selfdrive/car/subaru/carcontroller.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 7a2ebba092ff7f..0927ede86f3378 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -27,6 +27,7 @@ def update(self, CC, CS, now_nanos): # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: + # angle based steering if self.CP.carFingerprint in LKAS_ANGLE: if CC.latActive: apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, CarControllerParams) @@ -36,6 +37,8 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) new_actuators.steeringAngleDeg = apply_steer + + # torque based steering else: apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) From b662fbb4d3e7492eaf62ba01971bf4e81df5d60f Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 00:45:33 -0700 Subject: [PATCH 42/45] fix apply steer --- selfdrive/car/subaru/carcontroller.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 0927ede86f3378..42ded67709ab50 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -30,12 +30,13 @@ def update(self, CC, CS, now_nanos): # angle based steering if self.CP.carFingerprint in LKAS_ANGLE: if CC.latActive: - apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, CarControllerParams) + apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p) else: apply_steer = CS.out.steeringAngleDeg can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) + self.apply_steer_last = apply_steer new_actuators.steeringAngleDeg = apply_steer # torque based steering @@ -43,9 +44,7 @@ def update(self, CC, CS, now_nanos): apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) # limits due to driver torque - - new_steer = int(round(apply_steer)) - apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) + apply_steer = apply_driver_steer_torque_limits(apply_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) if not CC.latActive: apply_steer = 0 @@ -54,12 +53,11 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, CC.latActive)) else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) - + + self.apply_steer_last = apply_steer new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last - self.apply_steer_last = apply_steer - # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: if self.frame % 5 == 0: From 02558ebdbc87595a7f501f8df1fbb62767a2189d Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 10:34:08 -0700 Subject: [PATCH 43/45] fix replay --- selfdrive/car/subaru/carcontroller.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 42ded67709ab50..a7cc734b55acb7 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -21,23 +21,20 @@ def update(self, CC, CS, now_nanos): hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel - new_actuators = actuators.copy() - can_sends = [] # *** steering *** if (self.frame % self.p.STEER_STEP) == 0: # angle based steering if self.CP.carFingerprint in LKAS_ANGLE: - if CC.latActive: - apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p) - else: + apply_steer = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_steer_last, CS.out.vEgoRaw, self.p) + + if not CC.latActive: apply_steer = CS.out.steeringAngleDeg can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) self.apply_steer_last = apply_steer - new_actuators.steeringAngleDeg = apply_steer # torque based steering else: @@ -55,8 +52,6 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) self.apply_steer_last = apply_steer - new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX - new_actuators.steerOutputCan = self.apply_steer_last # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: @@ -95,4 +90,13 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert)) self.frame += 1 + + new_actuators = actuators.copy() + + if self.CP.carFingerprint in LKAS_ANGLE: + new_actuators.steeringAngleDeg = apply_steer + else: + new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX + new_actuators.steerOutputCan = self.apply_steer_last + return new_actuators, can_sends From e4634c4cf2cf7dc2ebbaa19e8a43e9df565fbc55 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 10:35:51 -0700 Subject: [PATCH 44/45] minimize diff --- selfdrive/car/subaru/carcontroller.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index a7cc734b55acb7..7739e117997b56 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -34,8 +34,6 @@ def update(self, CC, CS, now_nanos): can_sends.append(subarucan.create_steering_control_angle(self.packer, apply_steer, CC.latActive)) - self.apply_steer_last = apply_steer - # torque based steering else: apply_steer = int(round(actuators.steer * self.p.STEER_MAX)) @@ -51,7 +49,7 @@ def update(self, CC, CS, now_nanos): else: can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, CC.latActive)) - self.apply_steer_last = apply_steer + self.apply_steer_last = apply_steer # *** alerts and pcm cancel *** if self.CP.carFingerprint in PREGLOBAL_CARS: @@ -89,14 +87,11 @@ def update(self, CC, CS, now_nanos): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: can_sends.append(subarucan.create_es_infotainment(self.packer, CS.es_infotainment_msg, hud_control.visualAlert)) - self.frame += 1 - new_actuators = actuators.copy() - if self.CP.carFingerprint in LKAS_ANGLE: new_actuators.steeringAngleDeg = apply_steer else: new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last - + self.frame += 1 return new_actuators, can_sends From 49fa3053494a0db2fbf7fcb568a54a0496826dab Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 21 Jul 2023 10:53:15 -0700 Subject: [PATCH 45/45] hotfix --- selfdrive/car/subaru/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 7739e117997b56..fd737aa4012b28 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -89,7 +89,7 @@ def update(self, CC, CS, now_nanos): new_actuators = actuators.copy() if self.CP.carFingerprint in LKAS_ANGLE: - new_actuators.steeringAngleDeg = apply_steer + new_actuators.steeringAngleDeg = self.apply_steer_last else: new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX new_actuators.steerOutputCan = self.apply_steer_last