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

@@ -258,6 +258,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
     {"LeadInfo", PERSISTENT},
+    {"LockDoors", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"LongPitch", PERSISTENT},
     {"ModelUI", PERSISTENT},

panda/board/safety/safety_toyota.h

@@ -47,7 +47,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
   {0x283, 0, 7}, {0x2E6, 0, 8}, {0x2E7, 0, 8}, {0x33E, 0, 7}, {0x344, 0, 8}, {0x365, 0, 7}, {0x366, 0, 7}, {0x4CB, 0, 8},  /* DSU bus 0 */  \
   {0x128, 1, 6}, {0x141, 1, 4}, {0x160, 1, 8}, {0x161, 1, 7}, {0x470, 1, 4},  /* DSU bus 1 */                                               \
   {0x2E4, 0, 5}, {0x191, 0, 8}, {0x411, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + ACC */                               \
-  {0x1D3, 0, 8},                                                                                                                            \
+  {0x1D3, 0, 8}, {0x750, 0, 8},                                                                                                             \
 
 const CanMsg TOYOTA_TX_MSGS[] = {
   TOYOTA_COMMON_TX_MSGS

selfdrive/car/toyota/carcontroller.py

@@ -24,6 +24,11 @@ MAX_USER_TORQUE = 500
 MAX_LTA_ANGLE = 94.9461  # deg
 MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows some resistance when changing lanes
 
+# 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):
@@ -42,9 +47,14 @@ class CarController:
     self.accel = 0
 
     # FrogPilot variables
+    self.lock_doors = False
     self.reverse_cruise_increase = False
 
+    self.doors_locked = False
+    self.doors_unlocked = True
+
   def update_frogpilot_variables(self, params):
+    self.lock_doors = params.get_bool("LockDoors")
     self.reverse_cruise_increase = params.get_bool("ReverseCruise")
 
   def update(self, CC, CS, now_nanos, sport_plus):
@@ -200,5 +210,16 @@ class CarController:
     new_actuators.accel = self.accel
     new_actuators.gas = self.gas
 
+    # Lock doors when in drive / unlock doors when in park
+    if self.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/frogpilot/ui/vehicle_settings.cc

@@ -101,6 +101,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
   std::vector<std::tuple<QString, QString, QString, QString>> vehicleToggles {
     {"GasRegenCmd", "Gas Regen Cmd", "", ""},
     {"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.", ""},
   };
 
   for (auto &[param, title, desc, icon] : vehicleToggles) {