From 20c1aee7ec7b376a4df74bb3e808856cc738835a Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 27 Mar 2024 00:31:14 -0700 Subject: [PATCH] Lock doors when in drive for Toyota/Lexus Added toggle to automatically lock the doors when in drive for Toyota/Lexus vehicles. Credit goes to AlexandreSato! https: //github.com/AlexandreSato/openpilot Co-Authored-By: Alexandre Nobuharu Sato <66435071+alexandresato@users.noreply.github.com> --- common/params.cc | 3 +++ selfdrive/car/toyota/carcontroller.py | 24 +++++++++++++++++++ selfdrive/controls/controlsd.py | 4 ++++ .../ui/qt/offroad/vehicle_settings.cc | 6 +++++ .../ui/qt/offroad/vehicle_settings.h | 2 +- 5 files changed, 38 insertions(+), 1 deletion(-) diff --git a/common/params.cc b/common/params.cc index 16b73637b..78f2e57ef 100644 --- a/common/params.cc +++ b/common/params.cc @@ -294,6 +294,7 @@ std::unordered_map keys = { {"LateralTune", PERSISTENT}, {"LeadDepartingAlert", PERSISTENT}, {"LeadInfo", PERSISTENT}, + {"LockDoors", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"LongPitch", PERSISTENT}, {"LowVoltageShutdown", PERSISTENT}, @@ -326,7 +327,9 @@ std::unordered_map keys = { {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, {"StoppingDistance", PERSISTENT}, + {"ToyotaDoors", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, + {"UnlockDoors", PERSISTENT}, {"UseSI", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 878d00af2..9409767dd 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -10,6 +10,8 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_ UNSUPPORTED_DSU_CAR, TSS2_CAR from opendbc.can.packer import CANPacker +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUISING_SPEED + LongCtrlState = car.CarControl.Actuators.LongControlState SteerControlType = car.CarParams.SteerControlType VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -33,6 +35,12 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows COMPENSATORY_CALCULATION_THRESHOLD_V = [-0.3, -0.25, 0.] # m/s^2 COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.] # m/s +# Lock / unlock door commands - Credit goes to AlexandreSato! +LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00' +UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00' + +PARK = car.CarState.GearShifter.park + def compute_gb_toyota(accel, speed): creep_brake = 0.0 @@ -68,6 +76,9 @@ class CarController(CarControllerBase): self.cydia_tune = params.get_bool("CydiaTune") self.frogs_go_moo_tune = params.get_bool("FrogsGoMooTune") + self.doors_locked = False + self.doors_unlocked = True + self.pcm_accel_comp = 0 def update(self, CC, CS, now_nanos, frogpilot_variables): @@ -279,5 +290,18 @@ class CarController(CarControllerBase): new_actuators.accel = self.accel new_actuators.gas = self.gas + # Lock doors when in drive / unlock doors when in park + if self.doors_unlocked and CS.out.gearShifter != PARK and CS.out.vEgo >= CRUISING_SPEED: + if frogpilot_variables.lock_doors: + can_sends.append(make_can_msg(0x750, LOCK_CMD, 0)) + self.doors_locked = True + self.doors_unlocked = False + + elif self.doors_locked and CS.out.gearShifter == PARK: + if frogpilot_variables.unlock_doors: + can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0)) + self.doors_locked = False + self.doors_unlocked = True + self.frame += 1 return new_actuators, can_sends diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a63e8d6b6..2e158bf19 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -994,6 +994,10 @@ class Controls: self.frogpilot_variables.custom_cruise_increase_long = self.params.get_int("CustomCruiseLong") if quality_of_life else 5 self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") + toyota_doors = self.params.get_bool("ToyotaDoors") + self.frogpilot_variables.lock_doors = toyota_doors and self.params.get_bool("LockDoors") + self.frogpilot_variables.unlock_doors = toyota_doors and self.params.get_bool("UnlockDoors") + def main(): config_realtime_process(4, Priority.CTRL_HIGH) controls = Controls() diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc index fd103115b..9333662f7 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc @@ -115,6 +115,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil {"LongPitch", tr("Long Pitch Compensation"), tr("Smoothen out the gas and pedal controls."), ""}, {"GasRegenCmd", tr("Truck Tune"), tr("Increase the acceleration and smoothen out the brake control when coming to a stop. For use on Silverado/Sierra only."), ""}, + {"ToyotaDoors", tr("Automatically Lock/Unlock Doors"), tr("Automatically lock the doors when in drive and unlock when in park."), ""}, {"LongitudinalTune", tr("Longitudinal Tune"), tr("Use a custom Toyota longitudinal tune.\n\nCydia = More focused on TSS-P vehicles but works for all Toyotas\n\nDragonPilot = Focused on TSS2 vehicles\n\nFrogPilot = Takes the best of both worlds with some personal tweaks focused around FrogsGoMoo's 2019 Lexus ES 350"), ""}, }; @@ -138,6 +139,11 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil } }); + } else if (param == "ToyotaDoors") { + std::vector lockToggles{"LockDoors", "UnlockDoors"}; + std::vector lockToggleNames{tr("Lock"), tr("Unlock")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, lockToggles, lockToggleNames); + } else { toggle = new ParamControl(param, title, desc, icon, this); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h index 60e393d8c..79e49d5e9 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h @@ -31,7 +31,7 @@ private: std::set gmKeys = {"GasRegenCmd", "LongPitch"}; std::set subaruKeys = {}; - std::set toyotaKeys = {"LongitudinalTune"}; + std::set toyotaKeys = {"LongitudinalTune", "ToyotaDoors"}; std::map toggles;