diff --git a/common/params.cc b/common/params.cc index 6fa37d902..a3538375c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -234,6 +234,7 @@ std::unordered_map keys = { {"dp_device_is_clone", PERSISTENT}, {"dp_device_dm_unavailable", PERSISTENT}, {"dp_toyota_enhanced_bsm", PERSISTENT}, + {"dp_toyota_auto_brake_hold", PERSISTENT}, }; } // namespace diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 754092d4c..d03bd917a 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -59,6 +59,9 @@ class Car: if self.params.get_bool("dp_alka"): self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALKA + if self.params.get_bool("dp_toyota_auto_brake_hold"): + self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB + openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle and not self.CP.dashcamOnly diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 7c4c80726..eefadd6b6 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -17,6 +17,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState # dp - for enhanced bsm from openpilot.dp_ext.selfdrive.car.toyota.bsm.controller import BSMController +# dp - for auto brake hold +from openpilot.dp_ext.selfdrive.car.toyota.brake_hold.controller import BrakeHoldController SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -55,6 +57,7 @@ class CarController(CarControllerBase): self.dlc = DoorLockController() self.pcc = PCMCompensationController(CP, self.params, Params().get_bool("dp_toyota_pcm_compensation")) self.bsmc = BSMController(self.CP) + self.bhc = BrakeHoldController() def update(self, CC, CS, now_nanos): actuators = CC.actuators @@ -67,6 +70,9 @@ class CarController(CarControllerBase): # *** control msgs *** can_sends = [] + # dp - for auto brake hold + can_sends = self.bhc.get_can_sends(self.frame, can_sends, CS.brakehold_governor, CS.stock_aeb) + # dp - for enhanced bsm can_sends = self.bsmc.get_can_sends(self.frame, CS.out.vEgo, can_sends) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 795b5dd21..063427f14 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -13,6 +13,7 @@ from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_T from openpilot.dp_ext.selfdrive.car.toyota.zss.controller import ZSSController from openpilot.dp_ext.selfdrive.car.toyota.bsm.state import BSMState +from openpilot.dp_ext.selfdrive.car.toyota.brake_hold.state import BrakeHoldState SteerControlType = car.CarParams.SteerControlType @@ -58,6 +59,10 @@ class CarState(CarStateBase): self.pcm_neutral_force = 0. # enhanced bsm self.bsm_state = BSMState() + # auto brake hold + self.brake_hold_state = BrakeHoldState(CP.carFingerprint in TSS2_CAR) + self.brakehold_governor = False + self.stock_aeb = {} def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -180,6 +185,10 @@ class CarState(CarStateBase): self.bsm_state.update_states(ret.leftBlindspot, ret.rightBlindspot) ret.leftBlindspot, ret.rightBlindspot = self.bsm_state.get_signals() + # dp - brake hold + self.brake_hold_state.update_states(ret.standstill, ret.cruiseState, ret.gasPressed, ret.brakePressed, ret.gearShifter) + self.brakehold_governor, self.stock_aeb = self.brake_hold_state.get_values() + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"]) diff --git a/system/manager/manager.py b/system/manager/manager.py index fde56b5f4..081da7345 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -73,6 +73,7 @@ def manager_init() -> None: ("dp_device_is_clone", "0"), ("dp_device_dm_unavailable", "0"), ("dp_toyota_enhanced_bsm", "0"), + ("dp_toyota_auto_brake_hold", "0"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))