mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-10-04 05:13:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

alt path brake hold

Browse Source
This commit is contained in:
firestar5683
2026-09-24 17:38:49 -05:00
parent d3a74f61e4
commit 1648a100e3
8 changed files with 153 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/toyota/carcontroller.py
+22 -3
View File
@@ -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
opendbc_repo/opendbc/car/toyota/carstate.py
+13
View File
@@ -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),
opendbc_repo/opendbc/car/toyota/interface.py
+5 -2
View File
@@ -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:
opendbc_repo/opendbc/car/toyota/tests/test_toyota.py
+33 -3
View File
@@ -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)
opendbc_repo/opendbc/car/toyota/toyotacan.py
+32
View File
@@ -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,
opendbc_repo/opendbc/car/toyota/values.py
+4
View File
@@ -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)
opendbc_repo/opendbc/safety/modes/toyota.h
+16 -1
View File
@@ -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,
opendbc_repo/opendbc/safety/tests/test_toyota.py
+28
View File
@@ -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],