From 15d94a017dffcc485c1a0260553833af05f90024 Mon Sep 17 00:00:00 2001 From: rav4 kumar Date: Mon, 1 Jul 2024 19:45:30 +0000 Subject: [PATCH] Toyota: Auto Brake Hold --- CHANGELOGS.md | 3 ++ cereal/car.capnp | 1 + common/params.cc | 1 + selfdrive/car/card.py | 3 ++ selfdrive/car/toyota/carcontroller.py | 32 +++++++++++++++++ selfdrive/car/toyota/carstate.py | 9 +++++ selfdrive/car/toyota/interface.py | 8 +++++ selfdrive/car/toyota/toyotacan.py | 34 +++++++++++++++++++ selfdrive/car/toyota/values.py | 1 + selfdrive/controls/lib/events.py | 8 +++++ .../qt/offroad/sunnypilot/vehicle_settings.cc | 13 +++++++ system/manager/manager.py | 1 + 12 files changed, 114 insertions(+) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index e703e17e06..f8e34cc602 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -15,6 +15,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) * NOTE: Only enable this feature if your Toyota/Lexus vehicle has factory Blind Spot Monitor equipped, and mentioned in the supported platforms list * UPDATED: Toyota: TSS2 longitudinal: Custom Tuning * Re-tuned and tested by the community (July 1, 2024) +* 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 * FIXED: Driving Personality: * Maniac mode now correctly enforced when selected * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL! diff --git a/cereal/car.capnp b/cereal/car.capnp index cd4cbf87b6..30bf496d01 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -136,6 +136,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { speedLimitPreActive @138; speedLimitConfirmed @139; torqueNNLoad @140; + spAutoBrakeHold @141; radarCanErrorDEPRECATED @15; communityFeatureDisallowedDEPRECATED @62; diff --git a/common/params.cc b/common/params.cc index c09c75aa4a..b0832d94a5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -317,6 +317,7 @@ std::unordered_map keys = { {"TorqueFriction", PERSISTENT | BACKUP}, {"TorqueMaxLatAccel", PERSISTENT | BACKUP}, {"TorquedOverride", PERSISTENT | BACKUP}, + {"ToyotaAutoHold", PERSISTENT | BACKUP}, {"ToyotaEnhancedBsm", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 98be4bf82f..63e7d22da4 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -56,6 +56,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 @@ -63,6 +64,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 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b83ef9b3c2..93cbcbbc1b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_ UNSUPPORTED_DSU_CAR from opendbc.can.packer import CANPacker +GearShifter = car.CarState.GearShifter SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -55,6 +56,12 @@ class CarController(CarControllerBase): self.right_blindspot_debug_enabled = False self.last_blindspot_frame = 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 + def update(self, CC, CS, now_nanos): actuators = CC.actuators hud_control = CC.hudControl @@ -148,6 +155,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) @@ -266,3 +276,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 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index fded06c1dc..456282982b 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 056d73bd69..451265f9cd 100644 --- a/selfdrive/car/toyota/interface.py +++ b/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 @@ -286,6 +289,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 diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index abde32bc07..979879e368 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 6952bb41bd..b4b277cc08 100644 --- a/selfdrive/car/toyota/values.py +++ b/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): diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bcdc4a64af..cc606353ca 100755 --- a/selfdrive/controls/lib/events.py +++ b/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"), diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index 4d75ad818b..ea2398f9ca 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc @@ -124,6 +124,18 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge toyotaTss2LongTune->setConfirmation(true, false); addItem(toyotaTss2LongTune); + auto toyotaAbh = new ParamControl( + "ToyotaAutoHold", + tr("Enable Automatic Brake Hold (AHB)"), + QString("%1

%2

%3") + .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"), @@ -158,6 +170,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); diff --git a/system/manager/manager.py b/system/manager/manager.py index 20e5c33708..57dbc194b8 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -97,6 +97,7 @@ def manager_init() -> None: ("TorqueDeadzoneDeg", "0"), ("TorqueFriction", "1"), ("TorqueMaxLatAccel", "250"), + ("ToyotaAutoHold", "0"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),