Reapply "Toyota: Auto Brake Hold"

This reverts commit be72a8ed06.

CHANGELOGS.md

@@ -32,6 +32,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
   * In response to the official deprecation of support for Smart DSU (SDSU) and Radar CAN Filter in the upstream ([commaai/openpilot#32777](https://github.com/commaai/openpilot/pull/32777)), sunnypilot will continue maintaining software support for Smart DSU (SDSU) and Radar CAN Filter
 * UPDATED: Continued support for Mapbox navigation
   * In response to the official temporary deprecation of support for Mapbox navigation in the upstream ([commaai/openpilot#32773](https://github.com/commaai/openpilot/pull/32773)), sunnypilot will continue maintaining software support for Mapbox navigation
+* NEW❗: Toyota - Automatic Brake Hold (AHB) thanks to AlexandreSato!
+  * When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold
+  * NOTE: Only for Toyota/Lexus vehicles with TSS2/LSS2
 * NEW❗: Toyota - Automatic Door Locking and Unlocking thanks to AlexandreSato, cydia2020, and dragonpilot-community!
   * Auto Lock by Speed: All doors are automatically locked when vehicle speed is approximately 6 mph (10 km/h) or higher
   * Auto Unlock by Shift to P: All doors are automatically unlocked when shifting the shift lever to P

cereal/car.capnp

@@ -137,6 +137,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     speedLimitPreActive @139;
     speedLimitConfirmed @140;
     torqueNNLoad @141;
+    spAutoBrakeHold @142;
 
     radarCanErrorDEPRECATED @15;
     communityFeatureDisallowedDEPRECATED @62;

common/params.cc

@@ -318,6 +318,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TorqueFriction", PERSISTENT | BACKUP},
     {"TorqueMaxLatAccel", PERSISTENT | BACKUP},
     {"TorquedOverride", PERSISTENT | BACKUP},
+    {"ToyotaAutoHold", PERSISTENT | BACKUP},
     {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP},
     {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP},
     {"ToyotaEnhancedBsm", PERSISTENT | BACKUP},

selfdrive/car/card.py

@@ -60,6 +60,7 @@ class Car:
     self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
     self.mads_dlob = self.enable_mads and self.mads_disengage_lateral_on_brake
     self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
+    self.sp_toyota_auto_brake_hold = self.params.get_bool("ToyotaAutoHold")
     self.CP.alternativeExperience = 0
     if not self.disengage_on_accelerator:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS
@@ -67,6 +68,8 @@ class Car:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS
     elif self.mads_ndlob:
       self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.MADS_DISABLE_DISENGAGE_LATERAL_ON_BRAKE
+    if self.sp_toyota_auto_brake_hold:
+      self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALLOW_AEB
 
     if self.CP.customStockLongAvailable and self.CP.pcmCruise and self.params.get_bool("CustomStockLong"):
       self.CP.pcmCruiseSpeed = False

selfdrive/car/toyota/carcontroller.py

@@ -55,6 +55,13 @@ class CarController(CarControllerBase):
     self.last_blindspot_frame = 0
 
     self._auto_lock_speed = 0.0
+
+    if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      self.brake_hold_active: bool = False
+      self._brake_hold_counter: int = 0
+      self._brake_hold_reset: bool = False
+      self._prev_brake_pressed: bool = False
+
     self._auto_lock_once = False
     self._gear_prev = GearShifter.park
 
@@ -168,6 +175,9 @@ class CarController(CarControllerBase):
 
     self.last_standstill = CS.out.standstill
 
+    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      can_sends.extend(self.create_auto_brake_hold_messages(CS))
+
     # handle UI messages
     fcw_alert = hud_control.visualAlert == VisualAlert.fcw
     steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
@@ -286,3 +296,25 @@ class CarController(CarControllerBase):
         # print("bsm poll right")
 
     return can_sends
+
+  # auto brake hold (https://github.com/AlexandreSato/)
+  def create_auto_brake_hold_messages(self, CS: car.CarState, brake_hold_allowed_timer: int = 100):
+    can_sends = []
+    disallowed_gears = [GearShifter.park, GearShifter.reverse]
+    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 disallowed_gears)
+
+    if brake_hold_allowed:
+      self._brake_hold_counter += 1
+      self.brake_hold_active = self._brake_hold_counter > brake_hold_allowed_timer and not self._brake_hold_reset
+      self._brake_hold_reset = not self._prev_brake_pressed and CS.out.brakePressed and not self._brake_hold_reset
+    else:
+      self._brake_hold_counter = 0
+      self.brake_hold_active = False
+      self._brake_hold_reset = False
+    self._prev_brake_pressed = CS.out.brakePressed
+
+    if self.frame % 2 == 0:
+      can_sends.append(toyotacan.create_brake_hold_command(self.packer, self.frame, CS.pre_collision_2, self.brake_hold_active))
+
+    return can_sends

selfdrive/car/toyota/carstate.py

@@ -79,6 +79,9 @@ class CarState(CarStateBase):
     self._right_blindspot_d2 = 0
     self._right_blindspot_counter = 0
 
+    if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      self.pre_collision_2 = {}
+
     self.frame = 0
 
   def update(self, cp, cp_cam):
@@ -260,6 +263,9 @@ class CarState(CarStateBase):
     if self.CP.spFlags & ToyotaFlagsSP.SP_ENHANCED_BSM and self.frame > 199:
       ret.leftBlindspot, ret.rightBlindspot = self.sp_get_enhanced_bsm(cp)
 
+    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      self.pre_collision_2 = copy.copy(cp_cam.vl["PRE_COLLISION_2"])
+
     self._update_traffic_signals(cp_cam)
     ret.cruiseState.speedLimit = self._calculate_speed_limit()
     self.frame += 1
@@ -463,4 +469,7 @@ class CarState(CarStateBase):
         ("PCS_HUD", 1),
       ]
 
+      if CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+        messages.append(("PRE_COLLISION_2", 33))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

selfdrive/car/toyota/interface.py

@@ -195,6 +195,9 @@ class CarInterface(CarInterfaceBase):
     if candidate == CAR.TOYOTA_PRIUS_TSS2:
       ret.spFlags |= ToyotaFlagsSP.SP_NEED_DEBUG_BSM.value
 
+    if Params().get_bool("ToyotaAutoHold") and candidate in (TSS2_CAR - RADAR_ACC_CAR):
+      ret.spFlags |= ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD.value
+
     return ret
 
   @staticmethod
@@ -271,6 +274,11 @@ class CarInterface(CarInterfaceBase):
           # while in standstill, send a user alert
           events.add(EventName.manualRestart)
 
+    # auto brake hold
+    if self.CP.spFlags & ToyotaFlagsSP.SP_AUTO_BRAKE_HOLD:
+      if self.CC.brake_hold_active and not ret.brakeHoldActive:
+        events.add(EventName.spAutoBrakeHold)
+
     ret.events = events.to_msg()
 
     return ret

selfdrive/car/toyota/toyotacan.py

@@ -130,3 +130,37 @@ def create_set_bsm_debug_mode(lr_blindspot, enabled):
 
 def create_bsm_polling_status(lr_blindspot):
   return make_can_msg(0x750, lr_blindspot + b"\x02\x21\x69\x00\x00\x00\x00", 0)
+
+
+# auto brake hold
+def create_brake_hold_command(packer, frame, pre_collision_2, brake_hold_active):
+  # forward PRE_COLLISION_2 when auto brake hold is not 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 brake_hold_active:
+    values = {
+      "DSS1GDRV": 0x3FF,
+      "PBRTRGR": frame % 730 < 727,  # cut actuation for 3 frames
+    }
+
+  return packer.make_can_msg("PRE_COLLISION_2", 0, values)

selfdrive/car/toyota/values.py

@@ -64,6 +64,7 @@ class ToyotaFlagsSP(IntFlag):
   SP_ZSS = 1
   SP_ENHANCED_BSM = 2
   SP_NEED_DEBUG_BSM = 4
+  SP_AUTO_BRAKE_HOLD = 8
 
 
 class Footnote(Enum):

selfdrive/controls/lib/events.py

@@ -744,6 +744,14 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
   },
 
+  EventName.spAutoBrakeHold: {
+    ET.PERMANENT: Alert(
+      "sunnypilot Brake Hold Active",
+      "",
+      AlertStatus.normal, AlertSize.small,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.prompt, 0.),
+  },
+
   EventName.parkBrake: {
     ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
     ET.NO_ENTRY: NoEntryAlert("Parking Brake Engaged"),

selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle_settings.cc

@@ -148,7 +148,19 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
   toyotaTss2LongTune->setConfirmation(true, false);
   addItem(toyotaTss2LongTune);
 
-  toyotaEnhancedBsm = new ParamControlSP(
+  auto toyotaAbh = new ParamControl(
+    "ToyotaAutoHold",
+    tr("Enable Automatic Brake Hold (AHB)"),
+    QString("<b>%1</b><br><br>%2<br><br><b>%3</b>")
+    .arg(tr("WARNING: Only for Toyota/Lexus vehicles with TSS2/LSS2. USE AT YOUR OWN RISK."))
+    .arg(tr("When you stop the vehicle completely by depressing the brake pedal, sunnypilot will activate Auto Brake Hold."))
+    .arg(tr("Changing this setting takes effect when the car is powered off.")),
+    "../assets/offroad/icon_blank.png"
+  );
+  toyotaAbh->setConfirmation(true, false);
+  addItem(toyotaAbh);
+
+  toyotaEnhancedBsm = new ParamControl(
     "ToyotaEnhancedBsm",
     tr("Enable Enhanced Blind Spot Monitor"),
     "",
@@ -195,6 +207,7 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge
     is_onroad = !offroad;
     hkgSmoothStop->setEnabled(offroad);
     toyotaTss2LongTune->setEnabled(offroad);
+    toyotaAbh->setEnabled(offroad);
     toyotaEnhancedBsm->setEnabled(offroad);
     toyotaSngHack->setEnabled(offroad);
     volkswagenCCOnly->setEnabled(offroad);

system/manager/manager.py

@@ -99,6 +99,7 @@ def manager_init() -> None:
     ("TorqueDeadzoneDeg", "0"),
     ("TorqueFriction", "1"),
     ("TorqueMaxLatAccel", "250"),
+    ("ToyotaAutoHold", "0"),
     ("ToyotaAutoLockBySpeed", "0"),
     ("ToyotaAutoUnlockByShifter", "0"),
     ("ToyotaEnhancedBsm", "0"),