mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-04 19:43:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Toyota Hybrid Brake Hold

Browse Source
This commit is contained in:
firestar5683
2026-10-06 02:55:02 -05:00
parent 56d90ef0a6
commit d2a06a7a0e
7 changed files with 134 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/toyota/carcontroller.py
+9 -9
View File
@@ -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
opendbc_repo/opendbc/car/toyota/carstate.py
+3 -3
View File
@@ -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')),
opendbc_repo/opendbc/car/toyota/interface.py
+2 -2
View File
@@ -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)
opendbc_repo/opendbc/car/toyota/tests/test_auto_hold.py
+93 -3
View File
@@ -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)
opendbc_repo/opendbc/car/toyota/values.py
+5
View File
@@ -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)))
openpilot/starpilot/ui/feature_settings_owner.py
+4 -3
View File
@@ -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 []
openpilot/starpilot/ui/tests/test_feature_settings_owner.py
+18
View File
@@ -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)