diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index cb4694514..09dc9679b 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -54,6 +54,9 @@ class CarInterface(CarInterfaceBase): # openpilot to send it. 0x2AA is sent by a similar device which intercepts # the radar instead of DSU on NO_DSU_CARs. use_sdsu = bool(0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR)) + if use_sdsu: + # sDSU / radar filter hardware needs the Toyota safety long-filter TX set. + ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value # In TSS2 cars, the camera does long control found_ecus = [fw.ecu for fw in car_fw] diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index e2ee6519f..0ae977a21 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -56,6 +56,7 @@ class ToyotaSafetyFlags(IntFlag): STOCK_LONGITUDINAL = (2 << 8) LTA = (4 << 8) SECOC = (8 << 8) + LONG_FILTER = (16 << 8) class ToyotaFlags(IntFlag): diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index d084077d3..0b370759d 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -31,6 +31,17 @@ /* ACC */ \ {0x343, 0, 8, .check_relay = true}, \ +#define TOYOTA_COMMON_LONG_TX_MSGS_FILTER \ + TOYOTA_COMMON_TX_MSGS \ + /* DSU bus 0 */ \ + {0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \ + {0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \ + /* DSU bus 1 */ \ + {0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \ + {0x470, 1, 4, .check_relay = false}, \ + /* PCS_HUD */ \ + {0x411, 0, 8, .check_relay = false}, \ + #define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \ TOYOTA_COMMON_SECOC_TX_MSGS \ {0x343, 0, 8, .check_relay = true}, \ @@ -63,6 +74,7 @@ static bool toyota_alt_brake = false; static bool toyota_stock_longitudinal = false; static bool toyota_lta = false; static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file +static bool toyota_long_filter = false; static uint32_t toyota_compute_checksum(const CANPacket_t *msg) { int len = GET_LEN(msg); @@ -368,6 +380,10 @@ static safety_config toyota_init(uint16_t param) { TOYOTA_COMMON_LONG_TX_MSGS }; + static const CanMsg TOYOTA_LONG_TX_MSGS_FILTER[] = { + TOYOTA_COMMON_LONG_TX_MSGS_FILTER + }; + static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = { TOYOTA_COMMON_SECOC_LONG_TX_MSGS }; @@ -379,6 +395,7 @@ static safety_config toyota_init(uint16_t param) { const uint32_t TOYOTA_PARAM_ALT_BRAKE = 1UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_STOCK_LONGITUDINAL = 2UL << TOYOTA_PARAM_OFFSET; const uint32_t TOYOTA_PARAM_LTA = 4UL << TOYOTA_PARAM_OFFSET; + const uint32_t TOYOTA_PARAM_LONG_FILTER = 16UL << TOYOTA_PARAM_OFFSET; #ifdef ALLOW_DEBUG const uint32_t TOYOTA_PARAM_SECOC = 8UL << TOYOTA_PARAM_OFFSET; @@ -388,6 +405,7 @@ static safety_config toyota_init(uint16_t param) { toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE); toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL); toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA); + toyota_long_filter = GET_FLAG(param, TOYOTA_PARAM_LONG_FILTER); toyota_dbc_eps_torque_factor = param & TOYOTA_EPS_FACTOR; safety_config ret; @@ -401,7 +419,11 @@ static safety_config toyota_init(uint16_t param) { if (toyota_stock_longitudinal) { SET_TX_MSGS(TOYOTA_TX_MSGS, ret); } else { - SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); + if (toyota_long_filter) { + SET_TX_MSGS(TOYOTA_LONG_TX_MSGS_FILTER, ret); + } else { + SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret); + } } } diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 28d5af54a..f3a28aa67 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -234,7 +234,20 @@ class Controls: ) cancel_requested = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise) CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol - CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and not self.sm['longitudinalPlan'].shouldStop + + legacy_resume_hack = False + if len(long_plan.speeds): + planned_resume = long_plan.speeds[-1] > 0.1 + # Some stock-ACC SNG hacks still rely on the legacy "planner wants speed" + # signal rather than longitudinalPlan.shouldStop going false first. + legacy_resume_hack = planned_resume and ( + (self.CP.brand == "toyota" and self.CP.openpilotLongitudinalControl and not self.CP.autoResumeSng) or + (self.CP.brand == "gm" and getattr(self.starpilot_toggles, "volt_sng", False)) + ) + + CC.cruiseControl.resume = CC.enabled and CS.cruiseState.standstill and ( + not self.sm['longitudinalPlan'].shouldStop or legacy_resume_hack + ) hudControl = CC.hudControl hud_set_speed = float(CS.vCruiseCluster * CV.KPH_TO_MS)