From 70f7340c173f0a4b0d3bb98dda9032cd304f890e Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 23 Aug 2022 17:48:34 -0700 Subject: [PATCH] Toyota: log stockAeb on non-TSS2 cars (#25489) * check PRE_COLLISION * need to make sure this is right * revert * temp, stash * fixes * uncomment that * it's not really cruise/pcm, but acc remove improt * revert * Fix CI * revert exception * Revert "revert exception" This reverts commit 7e2f39097651f17cf3d2ac9f442fab5071e1b9d0. * this tested enableDsu, but we have other routes that do that * use segment from db * remove exception again --- selfdrive/car/tests/routes.py | 3 +-- selfdrive/car/toyota/carstate.py | 38 ++++++++++++++++++-------------- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index a47a3447e2..b769cbe5f7 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -140,11 +140,10 @@ routes = [ CarTestRoute("7e34a988419b5307|2019-12-18--19-13-30", TOYOTA.RAV4H_TSS2), CarTestRoute("2475fb3eb2ffcc2e|2022-04-29--12-46-23", TOYOTA.RAV4H_TSS2_2022), CarTestRoute("e6a24be49a6cd46e|2019-10-29--10-52-42", TOYOTA.LEXUS_ES_TSS2), - CarTestRoute("25057fa6a5a63dfb|2020-03-04--08-44-23", TOYOTA.LEXUS_CTH), + CarTestRoute("da23c367491f53e2|2021-05-21--09-09-11", TOYOTA.LEXUS_CTH, segment=3), CarTestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ESH_TSS2), CarTestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ESH), CarTestRoute("32696cea52831b02|2021-11-19--18-13-30", TOYOTA.LEXUS_RC), - CarTestRoute("886fcd8408d570e9|2020-01-29--05-11-22", TOYOTA.LEXUS_RX), CarTestRoute("886fcd8408d570e9|2020-01-29--02-18-55", TOYOTA.LEXUS_RX), CarTestRoute("d27ad752e9b08d4f|2021-05-26--19-39-51", TOYOTA.LEXUS_RXH), CarTestRoute("01b22eb2ed121565|2020-02-02--11-25-51", TOYOTA.LEXUS_RX_TSS2), diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 537915919c..843e7806dc 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -94,12 +94,11 @@ class CarState(CarStateBase): ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0 ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS - if self.CP.carFingerprint in RADAR_ACC_CAR: - self.acc_type = cp.vl["ACC_CONTROL"]["ACC_TYPE"] - ret.stockFcw = bool(cp.vl["ACC_HUD"]["FCW"]) - elif self.CP.carFingerprint in TSS2_CAR: - self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"] - ret.stockFcw = bool(cp_cam.vl["ACC_HUD"]["FCW"]) + 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"] + ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"]) # some TSS2 cars have low speed lockout permanently set, so ignore on those cars # these cars are identified by an ACC_TYPE value of 2. @@ -120,10 +119,11 @@ class CarState(CarStateBase): ret.cruiseState.nonAdaptive = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] in (1, 2, 3, 4, 5, 6) ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"]) - ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) - ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0 + if not self.CP.enableDsu: + ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.enableBsm: ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1) ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1) @@ -219,25 +219,31 @@ class CarState(CarStateBase): ("ACC_HUD", 1), ] + if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu: + signals += [ + ("FORCE", "PRE_COLLISION"), + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ] + checks += [ + ("PRE_COLLISION", 33), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): - signals = [ - ("FORCE", "PRE_COLLISION"), - ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), - ] - - checks = [ - ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent - ] + signals = [] + checks = [] if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): signals += [ + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ("FORCE", "PRE_COLLISION"), ("ACC_TYPE", "ACC_CONTROL"), ("FCW", "ACC_HUD"), ] checks += [ + ("PRE_COLLISION", 33), ("ACC_CONTROL", 33), ("ACC_HUD", 1), ]