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

@@ -282,6 +282,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LateralTune", PERSISTENT},
     {"LeadDepartingAlert", PERSISTENT},
     {"LeadInfo", PERSISTENT},
+    {"LockDoors", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"LongPitch", PERSISTENT},
     {"ManualUpdateInitiated", CLEAR_ON_MANAGER_START},

selfdrive/car/toyota/carcontroller.py

@@ -32,6 +32,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
+
+
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
@@ -55,6 +61,9 @@ class CarController:
     self.cydia_tune = params.get_bool("CydiaTune")
     self.frogs_go_moo_tune = params.get_bool("FrogsGoMooTune")
 
+    self.doors_locked = False
+    self.doors_unlocked = True
+
   def update(self, CC, CS, now_nanos, frogpilot_variables):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -234,5 +243,16 @@ class CarController:
     new_actuators.accel = self.accel
     new_actuators.gas = self.gas
 
+    # Lock doors when in drive / unlock doors when in park
+    if frogpilot_variables.lock_doors:
+      if self.doors_unlocked and CS.out.gearShifter != PARK:
+        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:
+        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

@@ -1073,6 +1073,7 @@ class Controls:
     lateral_tune = self.params.get_bool("LateralTune")
     self.force_auto_tune = lateral_tune and self.params.get_float("ForceAutoTune")
 
+    self.frogpilot_variables.lock_doors = self.params.get_bool("LockDoors")
     self.frogpilot_variables.long_pitch = self.params.get_bool("LongPitch")
 
     longitudinal_tune = self.params.get_bool("LongitudinalTune")

selfdrive/frogpilot/ui/vehicle_settings.cc

@@ -113,6 +113,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
     {"GasRegenCmd", "GM Truck Gas Tune", "Increase acceleration and smoothen brake to stop. For use on Silverado/Sierra only.", ""},
     {"LongPitch", "Long Pitch Compensation", "Reduce speed and acceleration error for greater passenger comfort and improved vehicle efficiency.", ""},
 
+    {"LockDoors", "Lock Doors In Drive", "Automatically lock the doors when in drive and unlock when in park.", ""},
     {"LongitudinalTune", "Longitudinal Tune", "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 my 2019 Lexus ES 350", ""},
   };
 

selfdrive/frogpilot/ui/vehicle_settings.h

@@ -30,7 +30,7 @@ private:
 
   std::set<QString> gmKeys = {"GasRegenCmd", "LongPitch"};
   std::set<QString> subaruKeys = {};
-  std::set<QString> toyotaKeys = {"LongitudinalTune"};
+  std::set<QString> toyotaKeys = {"LockDoors", "LongitudinalTune"};
 
   Params params;
   Params paramsMemory{"/dev/shm/params"};