diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 0e11b1803d..25bcc69c91 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -19,6 +19,14 @@ 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 +* 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 +* FIXED: Driving Personality: + * Maniac mode now correctly enforced when selected * Kia Ceed Plug-in Hybrid Non-SCC 2022 support thanks to TerminatorNL! sunnypilot - 0.9.7.1 (2024-06-13) 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 c96e1b94eb..256eafc558 100644 --- a/common/params.cc +++ b/common/params.cc @@ -318,6 +318,9 @@ std::unordered_map keys = { {"TorqueFriction", PERSISTENT | BACKUP}, {"TorqueMaxLatAccel", PERSISTENT | BACKUP}, {"TorquedOverride", PERSISTENT | BACKUP}, + {"ToyotaAutoHold", PERSISTENT | BACKUP}, + {"ToyotaAutoLockBySpeed", PERSISTENT | BACKUP}, + {"ToyotaAutoUnlockByShifter", PERSISTENT | BACKUP}, {"ToyotaEnhancedBsm", PERSISTENT | BACKUP}, {"ToyotaSnG", PERSISTENT | BACKUP}, {"ToyotaTSS2Long", PERSISTENT | BACKUP}, diff --git a/panda b/panda index 2b1869bb1b..9b76d69484 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 2b1869bb1b2d2cc3c38ce0ac297dd114649a5e01 +Subproject commit 9b76d69484d1780c7588bd1932079dee7e43dcf7 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..210ae23fa1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,4 +1,5 @@ from cereal import car +from common.conversions import Conversions as CV from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ @@ -10,6 +11,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 @@ -29,6 +31,9 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows LEFT_BLINDSPOT = b"\x41" RIGHT_BLINDSPOT = b"\x42" +UNLOCK_CMD = b"\x40\x05\x30\x11\x00\x40\x00\x00" +LOCK_CMD = b"\x40\x05\x30\x11\x00\x80\x00\x00" + class CarController(CarControllerBase): def __init__(self, dbc_name, CP, VM): @@ -48,6 +53,7 @@ class CarController(CarControllerBase): self.accel = 0 self.param_s = Params() + self._is_metric = self.param_s.get_bool("IsMetric") self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange") self._sng_hack = self.param_s.get_bool("ToyotaSnG") @@ -55,7 +61,25 @@ 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 + + self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed") + self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter") + self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) + self._auto_lock_once = False + self._gear_prev = GearShifter.park + def update(self, CC, CS, now_nanos): + if self.frame % 200 == 0: + self._is_metric = self.param_s.get_bool("IsMetric") + self._auto_lock_by_speed = self.param_s.get_bool("ToyotaAutoLockBySpeed") + self._auto_unlock_by_shifter = self.param_s.get_bool("ToyotaAutoUnlockByShifter") + self._auto_lock_speed = 10 * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) + actuators = CC.actuators hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel @@ -64,6 +88,21 @@ class CarController(CarControllerBase): # *** control msgs *** can_sends = [] + # automatic door locking and unlocking logic (@dragonpilot-community) + # thanks to AlexandreSato & cydia2020 + # https://github.com/AlexandreSato/animalpilot/blob/personal/doors.py + gear = CS.out.gearShifter + if not CS.out.doorOpen: + if gear == GearShifter.park and self._gear_prev != gear: + if self._auto_unlock_by_shifter: + can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0)) + self._auto_lock_once = False + elif gear == GearShifter.drive and not self._auto_lock_once and CS.out.vEgo >= self._auto_lock_speed: + if self._auto_lock_by_speed: + can_sends.append(make_can_msg(0x750, LOCK_CMD, 0)) + self._auto_lock_once = True + self._gear_prev = gear + # *** steer torque *** new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) @@ -148,6 +187,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 +308,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/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c74f93c006..9e554865f0 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -191,11 +191,11 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsState'].personality) + self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsStateSP'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index 4d75ad818b..ef0954c1ed 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"), @@ -142,6 +154,24 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(VehiclePanel *parent) : ListWidge toyotaSngHack->setConfirmation(true, false); addItem(toyotaSngHack); + auto toyotaAutoLock = new ParamControl( + "ToyotaAutoLock", + tr("Enable Toyota Door Auto Locking"), + tr("sunnypilot will attempt to lock the doors when drive above 10 km/h (6.2 mph).\nReboot Required."), + "../assets/offroad/icon_blank.png" + ); + toyotaAutoLock->setConfirmation(true, false); + addItem(toyotaAutoLock); + + auto toyotaAutoUnlock = new ParamControl( + "ToyotaAutoUnlockByShifter", + tr("Enable Toyota Door Auto Unlocking"), + tr("sunnypilot will attempt to unlock the doors when shift to gear P.\nReboot Required."), + "../assets/offroad/icon_blank.png" + ); + toyotaAutoUnlock->setConfirmation(true, false); + addItem(toyotaAutoUnlock); + // Volkswagen addItem(new LabelControl(tr("Volkswagen"))); auto volkswagenCCOnly = new ParamControl( @@ -158,6 +188,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 674cdcb666..973eb4091d 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -97,6 +97,9 @@ def manager_init() -> None: ("TorqueDeadzoneDeg", "0"), ("TorqueFriction", "1"), ("TorqueMaxLatAccel", "250"), + ("ToyotaAutoHold", "0"), + ("ToyotaAutoLockBySpeed", "0"), + ("ToyotaAutoUnlockByShifter", "0"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),