diff --git a/CHANGELOGS.md b/CHANGELOGS.md index f8e34cc602..b64eaf2c99 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -18,6 +18,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) * 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! diff --git a/common/params.cc b/common/params.cc index 877cf3ee61..07741914d0 100644 --- a/common/params.cc +++ b/common/params.cc @@ -319,6 +319,8 @@ std::unordered_map keys = { {"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/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 93cbcbbc1b..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, \ @@ -30,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): @@ -49,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") @@ -62,7 +67,19 @@ class CarController(CarControllerBase): 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 @@ -71,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) diff --git a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc index ea2398f9ca..ef0954c1ed 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot/vehicle_settings.cc @@ -154,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( diff --git a/system/manager/manager.py b/system/manager/manager.py index f35686f130..3db3c02b9b 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -99,6 +99,8 @@ def manager_init() -> None: ("TorqueFriction", "1"), ("TorqueMaxLatAccel", "250"), ("ToyotaAutoHold", "0"), + ("ToyotaAutoLockBySpeed", "0"), + ("ToyotaAutoUnlockByShifter", "0"), ("ToyotaEnhancedBsm", "0"), ("TrueVEgoUi", "0"), ("TurnSpeedControl", "0"),