From 1648a100e39eb86fc53ca981f5db64fdf4beba93 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 24 Sep 2026 17:38:49 -0500 Subject: [PATCH] alt path brake hold --- .../opendbc/car/toyota/carcontroller.py | 25 +++++++++++-- opendbc_repo/opendbc/car/toyota/carstate.py | 13 +++++++ opendbc_repo/opendbc/car/toyota/interface.py | 7 ++-- .../opendbc/car/toyota/tests/test_toyota.py | 36 +++++++++++++++++-- opendbc_repo/opendbc/car/toyota/toyotacan.py | 32 +++++++++++++++++ opendbc_repo/opendbc/car/toyota/values.py | 4 +++ opendbc_repo/opendbc/safety/modes/toyota.h | 17 ++++++++- .../opendbc/safety/tests/test_toyota.py | 28 +++++++++++++++ 8 files changed, 153 insertions(+), 9 deletions(-) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index f22216c66f..2d5b395470 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -11,7 +11,7 @@ from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan from opendbc.car.toyota.values import CAR, MIN_ACC_SPEED, NO_STOP_TIMER_CAR, PEDAL_TRANSITION, TSS2_CAR, \ CarControllerParams, ToyotaFlags, \ - UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS + UNSUPPORTED_DSU_CAR, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS from opendbc.can import CANPacker Ecu = structs.CarParams.Ecu @@ -336,6 +336,22 @@ class CarController(CarControllerBase): return self.brake_hold_active + def create_auto_brake_hold_messages(self, CS: structs.CarState, brake_hold_allowed_timer: int = 100): + brake_hold_allowed = (CS.out.standstill and CS.out.cruiseState.available and + not CS.out.gasPressed and not CS.out.cruiseState.enabled and + CS.out.gearShifter not in (PARK, REVERSE)) + + if brake_hold_allowed and not self.brake_hold_active and CS.out.brakePressed: + self._brake_hold_counter += 1 + self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer + elif not brake_hold_allowed: + self._brake_hold_counter = 0 + self.brake_hold_active = False + + if self.frame % 2 == 0: + return [toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active)] + return [] + def reset_auto_hold_state(self): self._brake_hold_counter = 0 self.brake_hold_active = False @@ -436,7 +452,10 @@ class CarController(CarControllerBase): self._update_standstill_request(CC, CS, actuators, starpilot_toggles) if supports_toyota_auto_hold(self.CP, getattr(starpilot_toggles, "toyota_auto_hold", False)): - self.update_auto_hold_state(CS, pcm_cancel_cmd) + if self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS: + can_sends.extend(self.create_auto_brake_hold_messages(CS)) + else: + self.update_auto_hold_state(CS, pcm_cancel_cmd) else: self.reset_auto_hold_state() @@ -546,7 +565,7 @@ class CarController(CarControllerBase): pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) - if self.brake_hold_active: + if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: pcm_accel_cmd = TOYOTA_AUTO_HOLD_ACCEL self.permit_braking = True self.standstill_req = True diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 06fdb039a2..5f31e61ded 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -9,6 +9,7 @@ from opendbc.car.interfaces import CarStateBase from opendbc.car.toyota.values import ToyotaFlags, ToyotaStarPilotFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \ SECOC_CAR, LEGACY_PRIUS_CAR +from opendbc.safety import ALTERNATIVE_EXPERIENCE ButtonType = structs.CarState.ButtonEvent.Type SteerControlType = structs.CarParams.SteerControlType @@ -90,6 +91,11 @@ class CarState(CarStateBase): self.has_can_filter = self.FPCP.flags & ToyotaStarPilotFlags.RADAR_CAN_FILTER.value self.has_SDSU = self.FPCP.flags & ToyotaStarPilotFlags.SMART_DSU.value self.has_ZSS = self.FPCP.flags & ToyotaStarPilotFlags.ZSS.value + self.auto_brake_hold = bool( + self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value and + getattr(self.CP, "alternativeExperience", 0) & ALTERNATIVE_EXPERIENCE.ALLOW_AEB + ) + self.pre_collision_2 = {} def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] @@ -225,6 +231,9 @@ class CarState(CarStateBase): if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) + if self.auto_brake_hold: + self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"]) + if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR: self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"] @@ -309,6 +318,10 @@ class CarState(CarStateBase): if CP.carFingerprint in DISTANCE_BUTTON_CAR: pt_messages.append(("PCM_CRUISE_4", 1)) + if (CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value and + getattr(CP, "alternativeExperience", 0) & ALTERNATIVE_EXPERIENCE.ALLOW_AEB): + cam_messages.append(("PRE_COLLISION_2", 50)) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 71472bae1e..5c21640803 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -4,7 +4,8 @@ from opendbc.car.toyota.carcontroller import CarController from opendbc.car.toyota.radar_interface import RadarInterface from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \ - ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS + ToyotaSafetyFlags, LEGACY_PRIUS_CAR, TOYOTA_AUTO_HOLD_CARS, \ + TOYOTA_AUTO_HOLD_AEB_CARS from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -165,7 +166,9 @@ class CarInterface(CarInterfaceBase): toyota_auto_hold = Params(return_defaults=True).get_bool("ToyotaAutoHold") if toyota_auto_hold and ret.openpilotLongitudinalControl and candidate in TOYOTA_AUTO_HOLD_CARS: - ret.alternativeExperience |= ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD + ret.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.ALLOW_AEB + if candidate in TOYOTA_AUTO_HOLD_AEB_CARS + else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD) ret.flags |= ToyotaFlags.AUTO_BRAKE_HOLD.value if not ret.openpilotLongitudinalControl: diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 1e637c549d..550d9d7fcf 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -25,6 +25,7 @@ from opendbc.car.toyota.radar_interface import RadarInterface, TSSP_RADAR_EGO_SP from opendbc.car.toyota.values import CAR, DBC, MIN_ACC_SPEED, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ ToyotaFlags, ToyotaSafetyFlags, ToyotaStarPilotFlags, TOYOTA_AUTO_HOLD_CARS, \ + TOYOTA_AUTO_HOLD_AEB_CARS, \ get_platform_codes from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params @@ -211,13 +212,17 @@ class TestToyotaInterfaces: params.remove("ToyotaAutoHold") assert car_params.flags & ToyotaFlags.AUTO_BRAKE_HOLD.value - assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD - assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB + if candidate in TOYOTA_AUTO_HOLD_AEB_CARS: + assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB + assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD + else: + assert car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD + assert not car_params.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALLOW_AEB can_parsers = CarState.get_can_parsers(car_params) car_state = CarState(car_params, SimpleNamespace(flags=0)) car_state.update(can_parsers, SimpleNamespace(cluster_offset=1.0)) - assert "PRE_COLLISION_2" not in can_parsers[Bus.cam].vl + assert (0x344 in can_parsers[Bus.cam].vl) == (candidate in TOYOTA_AUTO_HOLD_AEB_CARS) @pytest.mark.parametrize("candidate", [CAR.TOYOTA_CAMRY_TSS2, CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H]) def test_auto_hold_is_disabled_by_default(self, candidate): @@ -889,6 +894,31 @@ class TestToyotaCarController: controller.update_auto_hold_state(cs, activation_frames=0) assert not controller.brake_hold_active + def test_camry_auto_hold_uses_legacy_aeb_brake_path(self): + controller = self._make_controller() + controller.CP.carFingerprint = CAR.TOYOTA_CAMRY_TSS2 + controller.packer = CANPacker(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt]) + controller.frame = 0 + + cs = SimpleNamespace( + out=SimpleNamespace( + standstill=True, + cruiseState=SimpleNamespace(available=True, enabled=False), + gasPressed=False, + brakePressed=True, + gearShifter=structs.CarState.GearShifter.drive, + ), + pre_collision_2={}, + ) + + can_sends = controller.create_auto_brake_hold_messages(cs, brake_hold_allowed_timer=0) + parser = CANParser(DBC[CAR.TOYOTA_CAMRY_TSS2][Bus.pt], [("PRE_COLLISION_2", 0)], 0) + parser.update([(1, can_sends)]) + + assert controller.brake_hold_active + assert parser.vl["PRE_COLLISION_2"]["DSS1GDRV"] == -1.0 + assert parser.vl["PRE_COLLISION_2"]["PBRTRGR"] == 1 + def test_prius_resume_request_releases_standstill_latch(self): controller = self._make_controller(standstill_req=True, last_standstill=True) diff --git a/opendbc_repo/opendbc/car/toyota/toyotacan.py b/opendbc_repo/opendbc/car/toyota/toyotacan.py index 2d6c1bdb17..43a7a8d383 100644 --- a/opendbc_repo/opendbc/car/toyota/toyotacan.py +++ b/opendbc_repo/opendbc/car/toyota/toyotacan.py @@ -89,6 +89,38 @@ def create_pcs_commands(packer, accel, active, mass): return [msg1, msg2] +def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active): + values = {s: pre_collision_2[s] for s in [ + "DSS1GDRV", + "DS1STAT2", + "DS1STBK2", + "PCSWAR", + "PCSALM", + "PCSOPR", + "PCSABK", + "PBATRGR", + "PPTRGR", + "IBTRGR", + "CLEXTRGR", + "IRLT_REQ", + "BRKHLD", + "AVSTRGR", + "VGRSTRGR", + "PREFILL", + "PBRTRGR", + "PCSDIS", + "PBPREPMP", + ] if s in pre_collision_2} + + if brake_hold_active: + values = { + "DSS1GDRV": 0x3FF, + "PBRTRGR": frame % 730 < 727, + } + + return packer.make_can_msg("PRE_COLLISION_2", 0, values) + + def create_acc_cancel_command(packer): values = { "GAS_RELEASED": 0, diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index dede289f4d..9f9b4e54c2 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -629,6 +629,10 @@ TOYOTA_AUTO_HOLD_CARS = (TSS2_CAR - RADAR_ACC_CAR - SECOC_CAR) | { CAR.TOYOTA_RAV4H, } +# The Camry uses the legacy camera AEB replacement for Auto Hold. Other +# supported Toyota models use the ACC_CONTROL hold request. +TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2} + # no resume button press required NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index cd6d293e97..0a36ce314e 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -404,7 +404,12 @@ static bool toyota_tx_hook(const CANPacket_t *msg) { tx = false; } - if ((msg->addr == 0x344U) && toyota_stock_longitudinal) { + // Camry Auto Hold replaces the camera AEB message only while stopped. + if ((msg->addr == 0x344U) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0)) { + if (vehicle_moving || gas_pressed || !acc_main_on) { + tx = false; + } + } else if ((msg->addr == 0x344U) && toyota_stock_longitudinal) { tx = false; } } @@ -571,11 +576,21 @@ static safety_config toyota_init(uint16_t param) { return ret; } +static bool toyota_fwd_hook(int bus_num, int addr) { + bool block_msg = false; + if (bus_num == 2) { + block_msg = (addr == 0x344) && ((alternative_experience & ALT_EXP_ALLOW_AEB) != 0) && + !vehicle_moving && !gas_pressed && acc_main_on; + } + return block_msg; +} + const safety_hooks toyota_hooks = { .init = toyota_init, .rx = toyota_rx_hook, .rx_all = toyota_rx_all_hook, .tx = toyota_tx_hook, + .fwd = toyota_fwd_hook, .get_checksum = toyota_get_checksum, .compute_checksum = toyota_compute_checksum, .get_quality_flag_valid = toyota_get_quality_flag_valid, diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index e25a7569cd..1c184cd0c1 100644 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -147,6 +147,34 @@ class TestToyotaSafetyBase(common.CarSafetyTest, common.LongitudinalAccelSafetyT self.safety.set_alternative_experience(0) self.assertFalse(self._tx(hold_msg)) + def test_auto_brake_hold_aeb_replacement_only_at_standstill(self): + if (not self.LONGITUDINAL or + self.safety.get_current_safety_param() & (ToyotaSafetyFlags.STOCK_LONGITUDINAL.value | ToyotaSafetyFlags.SECOC.value)): + raise unittest.SkipTest("Toyota AEB Auto Hold requires non-SecOC openpilot longitudinal control") + + self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALLOW_AEB) + hold_msg = libsafety_py.make_CANPacket(0x344, 0, b"\xfd\x80\x00\x00\x00\x00\x00\xcc") + + self._rx(self._speed_msg(0)) + self._rx(self._toggle_aol(True)) + self._rx(self._user_gas_msg(False)) + self.assertTrue(self._tx(hold_msg)) + self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._speed_msg(1.0)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._speed_msg(0)) + self._rx(self._user_gas_msg(True)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + + self._rx(self._user_gas_msg(False)) + self._rx(self._toggle_aol(False)) + self.assertFalse(self._tx(hold_msg)) + self.assertEqual(0, self.safety.safety_fwd_hook(2, 0x344)) + # Only allow LTA msgs with no actuation def test_lta_steer_cmd(self): for engaged, req, req2, torque_wind_down, angle in itertools.product([True, False],