diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index 4dd19d24e3..40ed0ea047 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -185,6 +185,7 @@ routes = [ CarTestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.MIRAI), CarTestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.CHR), CarTestRoute("ea8fbe72b96a185c|2023-02-08--15-11-46", TOYOTA.CHR_TSS2), + CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.CHR_TSS2), # openpilot longitudinal, with smartDSU CarTestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHRH), CarTestRoute("6719965b0e1d1737|2023-02-09--22-44-05", TOYOTA.CHRH_TSS2), CarTestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 050f8747a2..68adc2ee57 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -115,7 +115,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 | RADAR_ACC_CAR): - 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["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars @@ -235,12 +236,17 @@ class CarState(CarStateBase): checks.append(("BSM", 1)) if CP.carFingerprint in RADAR_ACC_CAR: + if not CP.flags & ToyotaFlags.SMART_DSU.value: + signals += [ + ("ACC_TYPE", "ACC_CONTROL"), + ] + checks += [ + ("ACC_CONTROL", 33), + ] signals += [ - ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), ] checks += [ - ("ACC_CONTROL", 33), ("ACC_HUD", 1), ] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 33a87451e9..6e8664c340 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -201,14 +201,18 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor=tire_stiffness_factor) ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR - # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it - smartDsu = 0x2FF in fingerprint[0] - # In TSS2 cars the camera does long control + + # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it + if 0x2FF in fingerprint[0]: + ret.flags |= ToyotaFlags.SMART_DSU.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) and not smartDsu + 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) ret.enableGasInterceptor = 0x201 in fingerprint[0] + # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") - ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) + ret.openpilotLongitudinalControl = bool(ret.flags & ToyotaFlags.SMART_DSU) or ret.enableDsu or candidate in (TSS2_CAR - RADAR_ACC_CAR) ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR if not ret.openpilotLongitudinalControl: diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f0e846cc54..8584c6b078 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -33,6 +33,7 @@ class CarControllerParams: class ToyotaFlags(IntFlag): HYBRID = 1 + SMART_DSU = 2 class CAR: