Alexandre Sato's Auto Brake Hold

common/params.cc

@@ -234,6 +234,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_device_is_clone", PERSISTENT},
     {"dp_device_dm_unavailable", PERSISTENT},
     {"dp_toyota_enhanced_bsm", PERSISTENT},
+    {"dp_toyota_auto_brake_hold", PERSISTENT},
 };
 
 } // namespace

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

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)
 

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"])
 

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')))