Lock doors when shifting out of park for Toyota/Lexus

Co-Authored-By: Alexandre Nobuharu Sato <66435071+alexandresato@users.noreply.github.com>

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -2,6 +2,7 @@ import math
 import numpy as np
 from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
 from opendbc.car.lateral import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance
+from opendbc.car.can_definitions import CanData
 from opendbc.car.carlog import carlog
 from opendbc.car.common.filter_simple import FirstOrderFilter, HighPassFilter
 from opendbc.car.common.pid import PIDController
@@ -35,6 +36,11 @@ MAX_STEER_RATE_FRAMES = 18  # tx control frames needed before torque can be cut
 MAX_USER_TORQUE = 500
 
 # FrogPilot variables
+PARK = structs.CarState.GearShifter.park
+
+# 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"
 
 
 def get_long_tune(CP, params):
@@ -81,6 +87,7 @@ class CarController(CarControllerBase):
     self.secoc_prev_reset_counter = 0
 
     # FrogPilot variables
+    self.doors_locked = False
 
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
     actuators = CC.actuators
@@ -328,5 +335,13 @@ class CarController(CarControllerBase):
     self.frame += 1
 
     # FrogPilot variables
+    if not self.doors_locked and CS.out.gearShifter != PARK:
+      if frogpilot_toggles.lock_doors:
+        can_sends.append(CanData(0x750, LOCK_CMD, 0))
+      self.doors_locked = True
+    elif self.doors_locked and CS.out.gearShifter == PARK:
+      if frogpilot_toggles.unlock_doors:
+        can_sends.append(CanData(0x750, UNLOCK_CMD, 0))
+      self.doors_locked = False
 
     return new_actuators, can_sends

opendbc_repo/opendbc/safety/modes/toyota.h

@@ -346,7 +346,9 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
   if (msg->addr == 0x750U) {
     // this address is sub-addressed. only allow tester present to radar (0xF)
     bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
-    if (invalid_uds_msg) {
+    // AleSato added some more hack'sss
+    bool valid_uds_msgs = (GET_BYTES(msg, 0, 4) == 0x11300540U);  // automatic door locking and unlocking
+    if (invalid_uds_msg && !valid_uds_msgs) {
       tx = 0;
     }
   }