From d2a06a7a0e2bdc76a61521a901c212a1c617bb19 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 6 Oct 2026 02:55:02 -0500 Subject: [PATCH] Toyota Hybrid Brake Hold --- .../opendbc/car/toyota/carcontroller.py | 18 ++-- opendbc_repo/opendbc/car/toyota/carstate.py | 6 +- opendbc_repo/opendbc/car/toyota/interface.py | 4 +- .../car/toyota/tests/test_auto_hold.py | 96 ++++++++++++++++++- opendbc_repo/opendbc/car/toyota/values.py | 5 + .../starpilot/ui/feature_settings_owner.py | 7 +- .../ui/tests/test_feature_settings_owner.py | 18 ++++ 7 files changed, 134 insertions(+), 20 deletions(-) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 2e87c57a4f..52da65b515 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -9,7 +9,7 @@ from opendbc.car.secoc import add_mac, build_sync_mac from opendbc.car.interfaces import CarControllerBase from opendbc.car.toyota import toyotacan from opendbc.car.toyota import prius_longitudinal -from opendbc.car.toyota.values import CAR, CarControllerParams, ToyotaFlags, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS +from opendbc.car.toyota.values import CAR, CarControllerParams, ToyotaFlags, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb from opendbc.can import CANPacker from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -43,7 +43,7 @@ def supports_toyota_auto_hold(CP): return (CP.carFingerprint in TOYOTA_AUTO_HOLD_CARS and CP.openpilotLongitudinalControl and not CP.flags & ToyotaFlags.SECOC and bool(CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD) and (CP.alternativeExperience & (ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD | ALTERNATIVE_EXPERIENCE.TOYOTA_AEB_HOLD)) == - (ALTERNATIVE_EXPERIENCE.TOYOTA_AEB_HOLD if CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS else + (ALTERNATIVE_EXPERIENCE.TOYOTA_AEB_HOLD if uses_toyota_auto_hold_aeb(CP) else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD)) @@ -99,12 +99,12 @@ class CarController(CarControllerBase): def update_auto_hold_state(self, CS, cancel_requested=False, activation_frames=TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES, *, long_active=False, stopping=False): - aeb_hold = self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS + aeb_hold = uses_toyota_auto_hold_aeb(self.CP) allowed = (not cancel_requested and CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed and (not CS.out.cruiseState.enabled or (long_active and not aeb_hold)) and CS.out.gearShifter not in (structs.CarState.GearShifter.park, structs.CarState.GearShifter.reverse)) if aeb_hold: - # Camry retains its separate manual AEB hold behavior. + # Manual AEB hold requires a driver brake stop. if allowed and not self.brake_hold_active and CS.out.brakePressed: self._brake_hold_counter += 1 self.brake_hold_active = self._brake_hold_counter > activation_frames @@ -130,7 +130,7 @@ class CarController(CarControllerBase): return self.brake_hold_active def reset_auto_hold_state(self): - if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: + if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP): self.standstill_req = False self._brake_hold_counter = 0 self.brake_hold_active = False @@ -235,10 +235,10 @@ class CarController(CarControllerBase): lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged # *** gas and brake *** - if supports_toyota_auto_hold(self.CP): - self.update_auto_hold_state(CS, pcm_cancel_cmd if self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS else False, + if supports_toyota_auto_hold(self.CP) and CS.out.canValid and not CS.out.canTimeout: + self.update_auto_hold_state(CS, pcm_cancel_cmd if not uses_toyota_auto_hold_aeb(self.CP) else False, long_active=long_active, stopping=stopping) - if (self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS and self.frame % 2 == 0 and + if (uses_toyota_auto_hold_aeb(self.CP) and self.frame % 2 == 0 and CS.out.standstill and CS.out.cruiseState.available and not CS.out.gasPressed): can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active)) else: @@ -340,7 +340,7 @@ class CarController(CarControllerBase): pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, lead) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) - if self.brake_hold_active and self.CP.carFingerprint not in TOYOTA_AUTO_HOLD_AEB_CARS: + if self.brake_hold_active and not uses_toyota_auto_hold_aeb(self.CP): 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 e18948b103..efe6e653f6 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -9,7 +9,7 @@ from opendbc.car.interfaces import CarStateBase from opendbc.car.dashboard_speed_limit import Tracker as LimitTracker, parser_expiry, toyota_sign from opendbc.can.dbc import DBC as CANDBC from opendbc.car.toyota.prius_longitudinal import configured as prius_filter_configured -from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, EPS_SCALE, TOYOTA_AUTO_HOLD_AEB_CARS +from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, EPS_SCALE, uses_toyota_auto_hold_aeb ButtonType = structs.CarState.ButtonEvent.Type SteerControlType = structs.CarParams.SteerControlType @@ -62,7 +62,7 @@ class CarState(CarStateBase): cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] - if self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD and self.CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS: + if self.CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD and uses_toyota_auto_hold_aeb(self.CP): self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"]) ret = structs.CarState() @@ -228,7 +228,7 @@ class CarState(CarStateBase): def get_can_parsers(CP): dbc_name = DBC[CP.carFingerprint][Bus.pt] cam_messages = [("RSA1", math.nan)] if "RSA1" in CANDBC(dbc_name).name_to_msg else [] - if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD and CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS: + if CP.flags & ToyotaFlags.AUTO_BRAKE_HOLD and uses_toyota_auto_hold_aeb(CP): cam_messages.append(("PRE_COLLISION_2", 50)) pt_messages = [ ("BLINKERS_STATE", float('nan')), diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 02529d2132..048927d202 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -3,7 +3,7 @@ from opendbc.car.toyota.carstate import CarState 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, MIN_ACC_SPEED, \ - EPS_SCALE, ToyotaSafetyFlags, TOYOTA_AUTO_HOLD_CARS, TOYOTA_AUTO_HOLD_AEB_CARS + EPS_SCALE, ToyotaSafetyFlags, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb from opendbc.car.disable_ecu import disable_ecu from opendbc.car.interfaces import CarInterfaceBase from opendbc.safety import ALTERNATIVE_EXPERIENCE @@ -28,7 +28,7 @@ def apply_toyota_auto_hold(CP, enabled: bool) -> bool: admitted = enabled and toyota_auto_hold_supported(CP) if admitted: CP.flags |= int(ToyotaFlags.AUTO_BRAKE_HOLD) - CP.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.TOYOTA_AEB_HOLD if CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS + CP.alternativeExperience |= (ALTERNATIVE_EXPERIENCE.TOYOTA_AEB_HOLD if uses_toyota_auto_hold_aeb(CP) else ALTERNATIVE_EXPERIENCE.TOYOTA_AUTO_HOLD) return bool(admitted) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_auto_hold.py b/opendbc_repo/opendbc/car/toyota/tests/test_auto_hold.py index 96d16662a4..a2adf88bea 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_auto_hold.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_auto_hold.py @@ -6,15 +6,16 @@ from opendbc.car.toyota import toyotacan from opendbc.car.toyota.carcontroller import CarController from opendbc.car.toyota.carstate import CarState from opendbc.car.toyota.interface import CarInterface, apply_toyota_auto_hold, toyota_auto_hold_supported -from opendbc.car.toyota.values import CAR, DBC, ToyotaFlags, ToyotaSafetyFlags, TOYOTA_AUTO_HOLD_CARS +from opendbc.car.toyota.values import CAR, DBC, ToyotaFlags, ToyotaSafetyFlags, TOYOTA_AUTO_HOLD_CARS, uses_toyota_auto_hold_aeb from opendbc.safety import ALTERNATIVE_EXPERIENCE as AE def setup_hold(car=CAR.TOYOTA_COROLLA_TSS2, enabled=True, *, car_fw=()): - cp = CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, car_fw, False, False, False) + cp = CarInterface.get_params(car, {0: {}, 1: {}, 2: {}}, list(car_fw), False, False, False) apply_toyota_auto_hold(cp, enabled) state = CarState(cp) state.out = structs.CarState() + state.out.canValid = True state.out.standstill = True state.out.brakePressed = True state.out.cruiseState.available = True @@ -50,6 +51,95 @@ def setup_cruise_hold(car=CAR.TOYOTA_COROLLA_TSS2, enabled=True, *, car_fw=()): class TestToyotaAutoHold(unittest.TestCase): + def test_invalid_can_withdraws_hold_and_requires_fresh_stop(self): + for car, hybrid in ((CAR.TOYOTA_CAMRY_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True), + (CAR.TOYOTA_COROLLA_TSS2, False)): + for timeout in (False, True): + with self.subTest(car=car, timeout=timeout): + fw = [structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.hybrid)] if hybrid else [] + cp, owner, command, state = setup_hold(car, car_fw=fw) + for _ in range(101): + step_hold(owner, command, state) + self.assertTrue(owner.brake_hold_active) + if uses_toyota_auto_hold_aeb(cp): + frames = [frame for _ in range(2) for frame in step_hold(owner, command, state)] + held = next(frame for frame in frames if frame[0] == 0x344) + self.assertEqual(decode(cp, held, 'PRE_COLLISION_2')['DSS1GDRV'], -1.0) + else: + self.assert_acc_hold(tick_acc(cp, owner, command, state)) + state.out.canValid = timeout + state.out.canTimeout = timeout + frames = [frame for _ in range(3) for frame in step_hold(owner, command, state)] + self.assertFalse(owner.brake_hold_active) + self.assertEqual(owner._brake_hold_counter, 0) + self.assertFalse(any(frame[0] == 0x344 for frame in frames)) + for frame in frames: + if frame[0] == 0x343: + self.assertNotEqual(decode(cp, frame, 'ACC_CONTROL')['ACCEL_CMD'], -1.0) + state.out.canValid, state.out.canTimeout = True, False + state.out.brakePressed = False + for _ in range(105): + step_hold(owner, command, state) + self.assertFalse(owner.brake_hold_active) + state.out.brakePressed = True + for _ in range(100): + step_hold(owner, command, state) + self.assertFalse(owner.brake_hold_active) + step_hold(owner, command, state) + self.assertTrue(owner.brake_hold_active) + + def test_detected_early_hybrid_uses_manual_aeb_hold(self): + for car, hybrid, aeb in ((CAR.TOYOTA_RAV4_TSS2, True, True), + (CAR.TOYOTA_RAV4_TSS2, False, False), + (CAR.TOYOTA_RAV4_TSS2_2022, True, False), + (CAR.TOYOTA_CAMRY_TSS2, False, True)): + with self.subTest(car=car, hybrid=hybrid): + fw = [structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.hybrid)] if hybrid else [] + cp = CarInterface.get_params(car, {0: {0x3F6: 8}, 1: {}, 2: {}}, fw, True, False, False) + self.assertEqual(bool(cp.flags & ToyotaFlags.HYBRID), hybrid) + self.assertTrue(cp.flags & ToyotaFlags.HAS_BSM) + self.assertEqual(uses_toyota_auto_hold_aeb(cp), aeb) + admitted = apply_toyota_auto_hold(cp, True) + if car == CAR.TOYOTA_RAV4_TSS2_2022: + self.assertFalse(admitted) + self.assertFalse(cp.flags & ToyotaFlags.AUTO_BRAKE_HOLD) + self.assertEqual(cp.alternativeExperience, 0) + self.assertNotIn('PRE_COLLISION_2', CarState.get_can_parsers(cp)[Bus.cam].vl) + continue + self.assertTrue(admitted) + self.assertEqual(cp.alternativeExperience, AE.TOYOTA_AEB_HOLD if aeb else AE.TOYOTA_AUTO_HOLD) + state = CarState(cp) + parsers = state.get_can_parsers(cp) + self.assertEqual('PRE_COLLISION_2' in parsers[Bus.cam].vl, aeb) + if not aeb: + continue + camera = CANPacker(DBC[car][Bus.pt]).make_can_msg('PRE_COLLISION_2', 2, + {'DSS1GDRV': -0.5, 'PBRTRGR': 1, 'DS1STAT2': 3}) + parsers[Bus.cam].update([(1_000_000_000, [camera])]) + state.update(parsers) + self.assertEqual(toyotacan.create_brake_hold_command( + CANPacker(DBC[car][Bus.pt]), 0, state.pre_collision_2, False)[1], camera[1]) + state.out = structs.CarState(canValid=True) + state.out.standstill = state.out.brakePressed = state.out.cruiseState.available = True + state.out.gearShifter = structs.CarState.GearShifter.drive + owner = CarController(DBC[car], cp) + command = structs.CarControl() + state.out.cruiseState.enabled = command.longActive = True + for _ in range(105): + step_hold(owner, command, state) + self.assertFalse(owner.brake_hold_active) + state.out.cruiseState.enabled = command.longActive = False + for _ in range(100): + step_hold(owner, command, state) + self.assertFalse(owner.brake_hold_active) + frames = [frame for _ in range(2) for frame in step_hold(owner, command, state)] + self.assertTrue(owner.brake_hold_active) + frame = next(frame for frame in frames if frame[0] == 0x344) + self.assertEqual(decode(cp, frame, 'PRE_COLLISION_2')['DSS1GDRV'], -1.0) + state.out.gasPressed = True + step_hold(owner, command, state) + self.assertFalse(owner.brake_hold_active) + def assert_acc_hold(self, values): self.assertEqual(values['ACCEL_CMD'], -1.0) self.assertEqual(values['PERMIT_BRAKING'], 1) @@ -146,7 +236,7 @@ class TestToyotaAutoHold(unittest.TestCase): self.assert_acc_hold(tick_acc(cp, controller, command, state)) def test_cruise_stop_blocks_planner_resume_until_gas(self): - for car, hybrid in ((CAR.TOYOTA_COROLLA_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, True)): + for car, hybrid in ((CAR.TOYOTA_COROLLA_TSS2, False), (CAR.TOYOTA_RAV4_TSS2, False)): with self.subTest(car=car): car_fw = [structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.hybrid)] if hybrid else [] cp, controller, command, state = setup_cruise_hold(car, car_fw=car_fw) diff --git a/opendbc_repo/opendbc/car/toyota/values.py b/opendbc_repo/opendbc/car/toyota/values.py index 45a227c70d..de1e482d36 100644 --- a/opendbc_repo/opendbc/car/toyota/values.py +++ b/opendbc_repo/opendbc/car/toyota/values.py @@ -615,3 +615,8 @@ TOYOTA_AUTO_HOLD_CARS = {car for car in CAR if car.config.flags & ToyotaFlags.TS CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, } TOYOTA_AUTO_HOLD_AEB_CARS = {CAR.TOYOTA_CAMRY_TSS2} + + +def uses_toyota_auto_hold_aeb(CP) -> bool: + return (CP.carFingerprint in TOYOTA_AUTO_HOLD_AEB_CARS or + (CP.carFingerprint == CAR.TOYOTA_RAV4_TSS2 and bool(CP.flags & ToyotaFlags.HYBRID))) diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index bcc1ebd81a..d10acf9c93 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/openpilot/starpilot/ui/feature_settings_owner.py @@ -592,7 +592,8 @@ class FeatureSettingsOwner: FeatureRow("", "Always On Lateral", "Keep steering assistance active independently of cruise control", page=FeaturePage.AOL, available=True), FeatureRow("", "Wheel Controls", "Assign steering-wheel buttons and cruise behavior", page=FeaturePage.WHEEL, available=True)] if (self._long_pitch_capability() is not None or self._bolt_disable_capability() is not None or - self._pedal_setup_capability() is not None or self._gm_stop_capability("VoltSNG") is not None or + self._auto_hold_capability() is not None or self._pedal_setup_capability() is not None or + self._gm_stop_capability("VoltSNG") is not None or self._gm_stop_capability("GMAutoHold") is not None or self.tesla_screen.capability() is not None or self.gm_tune.capability() is not None): rows.append(FeatureRow("", "Vehicle Settings", "Configure features supported by your vehicle", page=FeaturePage.VEHICLE, available=True)) rows.extend(FeatureRow("", name.title() + " Personality", "", page=name, available=True) @@ -601,8 +602,8 @@ class FeatureSettingsOwner: title = "Vehicle Settings" capability = self._auto_hold_capability() allowed = configurable and capability is not None and self.authority("vehicle") - from opendbc.car.toyota.values import TOYOTA_AUTO_HOLD_AEB_CARS - stops = "manual stops" if capability is not None and capability[0] in TOYOTA_AUTO_HOLD_AEB_CARS else "manual and cruise-controlled stops" + from opendbc.car.toyota.values import uses_toyota_auto_hold_aeb + stops = "manual stops" if capability is not None and uses_toyota_auto_hold_aeb(self.vehicle_params()) else "manual and cruise-controlled stops" row = self._bool_row("ToyotaAutoHold", "Automatic Brake Hold", allowed, f"Holds {stops} with cruise main on until you press the gas. Applies after the next startup.") rows = [replace(row, available=row.available and self._readable("ToyotaAutoHold"), capability=capability)] if capability is not None else [] diff --git a/openpilot/starpilot/ui/tests/test_feature_settings_owner.py b/openpilot/starpilot/ui/tests/test_feature_settings_owner.py index 5d95f9b675..f568340f32 100644 --- a/openpilot/starpilot/ui/tests/test_feature_settings_owner.py +++ b/openpilot/starpilot/ui/tests/test_feature_settings_owner.py @@ -31,6 +31,24 @@ def required_document(raw: object) -> dict: class FeatureSettingsOwnerTests(unittest.TestCase): + def test_toyota_hold_is_reachable_from_hub_with_correct_stop_description(self): + from opendbc.car import structs + from opendbc.car.toyota.interface import CarInterface + from opendbc.car.toyota.values import CAR + for hybrid, description in ((True, "manual stops"), (False, "manual and cruise-controlled stops")): + fw = [structs.CarParams.CarFw(ecu=structs.CarParams.Ecu.hybrid)] if hybrid else [] + cp = CarInterface.get_params(CAR.TOYOTA_RAV4_TSS2, {0: {0x3F6: 8}, 1: {}, 2: {}}, fw, False, False, False) + self.fingerprint = cp.carFingerprint + self.owner.vehicle_params = lambda cp=cp: cp + hub = self.owner.snapshot("hub", parked=True, system_long=True, lateral_context=True, metric=False) + self.assertTrue(any(row.label == "Vehicle Settings" and row.available for row in hub.rows)) + row = self._row("vehicle", "ToyotaAutoHold") + self.assertTrue(row.available) + self.assertIn(description, row.reason) + request = replace(required_change(row), confirmation=True) + self.assertTrue(self.owner.apply(request)) + self.params.put_bool("ToyotaAutoHold", False, block=True) + def test_gm_tune_configured_choice_preserves_intent_and_guards_commit(self): from opendbc.car.gm.tests.test_volt_transitions import volt_camera_pedal_params cp = volt_camera_pedal_params(release=True)