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

@@ -294,6 +294,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LateralTune", PERSISTENT},
     {"LeadDepartingAlert", PERSISTENT},
     {"LeadInfo", PERSISTENT},
+    {"LockDoors", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"LongPitch", PERSISTENT},
     {"LowVoltageShutdown", PERSISTENT},
@@ -326,7 +327,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"StandardJerk", PERSISTENT},
     {"StockTune", PERSISTENT},
     {"StoppingDistance", PERSISTENT},
+    {"ToyotaDoors", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
+    {"UnlockDoors", PERSISTENT},
     {"UseSI", PERSISTENT},
     {"WarningImmediateVolume", PERSISTENT},
     {"WarningSoftVolume", PERSISTENT},

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

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()

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<QString> lockToggles{"LockDoors", "UnlockDoors"};
+      std::vector<QString> lockToggleNames{tr("Lock"), tr("Unlock")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, lockToggles, lockToggleNames);
+
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
     }

selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h

@@ -31,7 +31,7 @@ private:
 
   std::set<QString> gmKeys = {"GasRegenCmd", "LongPitch"};
   std::set<QString> subaruKeys = {};
-  std::set<QString> toyotaKeys = {"LongitudinalTune"};
+  std::set<QString> toyotaKeys = {"LongitudinalTune", "ToyotaDoors"};
 
   std::map<std::string, ParamControl*> toggles;