diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index b1156f0e8..dd3a8f633 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -225,6 +225,7 @@ routes = [ CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.TOYOTA_CHR), CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.TOYOTA_CHR), # hybrid CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.TOYOTA_CHR_TSS2), + CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.TOYOTA_CHR_TSS2), # openpilot longitudinal, with smartDSU CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid CarTestRoute("6719965b0e1d1737|2023-08-29--06-40-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.TOYOTA_PRIUS_V), diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a596881c3..8315f24ae 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -132,7 +132,8 @@ class CarState(CarStateBase): cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: - self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] + if not (self.CP.flags & ToyotaFlags.SMART_DSU.value): + self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"] ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars @@ -166,11 +167,13 @@ class CarState(CarStateBase): if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] - if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): # distance button is wired to the ACC module (camera or radar) self.prev_distance_button = self.distance_button if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] + else: + self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] return ret @@ -205,9 +208,12 @@ class CarState(CarStateBase): messages.append(("BSM", 1)) if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value: + if not CP.flags & ToyotaFlags.SMART_DSU.value: + messages += [ + ("ACC_CONTROL", 33), + ] messages += [ ("PCS_HUD", 1), - ("ACC_CONTROL", 33), ] if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu and not CP.flags & ToyotaFlags.DISABLE_RADAR.value: @@ -215,6 +221,11 @@ class CarState(CarStateBase): ("PRE_COLLISION", 33), ] + if CP.flags & ToyotaFlags.SMART_DSU and not CP.flags & ToyotaFlags.RADAR_CAN_FILTER: + messages += [ + ("SDSU", 100), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 6487257bb..98f63597e 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -44,9 +44,18 @@ class CarInterface(CarInterfaceBase): stop_and_go = candidate in TSS2_CAR + # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it + # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs + if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): + ret.flags |= ToyotaFlags.SMART_DSU.value + + if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR: + ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value + # In TSS2 cars, the camera does long control found_ecus = [fw.ecu for fw in car_fw] - ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) + ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \ + and not (ret.flags & ToyotaFlags.SMART_DSU) if candidate == CAR.TOYOTA_PRIUS: stop_and_go = True @@ -89,7 +98,7 @@ class CarInterface(CarInterfaceBase): # TODO: these models can do stop and go, but unclear if it requires sDSU or unplugging DSU. # For now, don't list stop and go functionality in the docs if ret.flags & ToyotaFlags.SNG_WITHOUT_DSU: - stop_and_go = stop_and_go or (ret.enableDsu and not docs) + stop_and_go = stop_and_go or bool(ret.flags & ToyotaFlags.SMART_DSU.value) or (ret.enableDsu and not docs) ret.centerToFront = ret.wheelbase * 0.44 @@ -101,20 +110,28 @@ class CarInterface(CarInterfaceBase): # TODO: make an adas dbc file for dsu-less models ret.radarUnavailable = DBC[candidate]['radar'] is None or candidate in (NO_DSU_CAR - TSS2_CAR) + # if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar. # since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle + use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU) if candidate in (RADAR_ACC_CAR | NO_DSU_CAR): - ret.experimentalLongitudinalAvailable = candidate in RADAR_ACC_CAR + ret.experimentalLongitudinalAvailable = use_sdsu or candidate in RADAR_ACC_CAR - # Disabling radar is only supported on TSS2 radar-ACC cars - if experimental_long and candidate in RADAR_ACC_CAR: - ret.flags |= ToyotaFlags.DISABLE_RADAR.value + if not use_sdsu: + # Disabling radar is only supported on TSS2 radar-ACC cars + if experimental_long and candidate in RADAR_ACC_CAR: + ret.flags |= ToyotaFlags.DISABLE_RADAR.value + else: + use_sdsu = use_sdsu and experimental_long # openpilot longitudinal enabled by default: + # - non-(TSS2 radar ACC cars) w/ smartDSU installed # - cars w/ DSU disconnected # - TSS2 cars with camera sending ACC_CONTROL where we can block it # openpilot longitudinal behind experimental long toggle: - # - TSS2 radar ACC cars (disables radar) - ret.openpilotLongitudinalControl = ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) + # - TSS2 radar ACC cars w/ smartDSU installed + # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) + # - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet) + ret.openpilotLongitudinalControl = use_sdsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) or bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR if not ret.openpilotLongitudinalControl: @@ -139,7 +156,7 @@ class CarInterface(CarInterfaceBase): @staticmethod def init(CP, logcan, sendcan): - # disable radar if alpha longitudinal toggled on radar-ACC car + # disable radar if alpha longitudinal toggled on radar-ACC car without CAN filter/smartDSU if CP.flags & ToyotaFlags.DISABLE_RADAR.value: communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL]) disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) @@ -148,7 +165,7 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) - if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): + if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): ret.buttonEvents = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}) # events diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 8022e8b3a..2e781ad0a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -44,7 +44,9 @@ class CarControllerParams: class ToyotaFlags(IntFlag): # Detected flags HYBRID = 1 + SMART_DSU = 2 DISABLE_RADAR = 4 + RADAR_CAN_FILTER = 1024 # Static flags TSS2 = 8 @@ -54,7 +56,7 @@ class ToyotaFlags(IntFlag): # these cars use the Lane Tracing Assist (LTA) message for lateral control ANGLE_CONTROL = 128 NO_STOP_TIMER = 256 - # these cars are speculated to allow stop and go when the DSU is unplugged + # these cars are speculated to allow stop and go when the DSU is unplugged or disabled with sDSU SNG_WITHOUT_DSU = 512