From 4371a6c1fb593efe9350137d484cfee15f4aa720 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 10 Jun 2024 06:39:00 -0700
Subject: [PATCH] Vehicles - Toyota - Automatically Lock/Unlock Doors

Automatically lock the doors when in drive and unlock when in park.

Credit goes to AlexandreSato!

https: //github.com/AlexandreSato/openpilot
Co-Authored-By: Alexandre Nobuharu Sato <66435071+alexandresato@users.noreply.github.com>
---
 panda/board/safety/safety_toyota.h    |  5 ++++-
 selfdrive/car/toyota/carcontroller.py | 17 +++++++++++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h
index 0b30bc12c..c25280b4a 100644
--- a/panda/board/safety/safety_toyota.h
+++ b/panda/board/safety/safety_toyota.h
@@ -51,6 +51,7 @@ const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 805;
 // Stock longitudinal
 #define TOYOTA_COMMON_TX_MSGS                                                                                     \
   {0x2E4, 0, 5}, {0x191, 0, 8}, {0x412, 0, 8}, {0x343, 0, 8}, {0x1D2, 0, 8},  /* LKAS + LTA + ACC & PCM cancel cmds */  \
+  {0x750, 0, 8},  /* white list 0x750 for Enhanced Diagnostic Request */                                                \
 
 #define TOYOTA_COMMON_LONG_TX_MSGS                                                                                                          \
   TOYOTA_COMMON_TX_MSGS                                                                                                                     \
@@ -362,7 +363,9 @@ static bool toyota_tx_hook(const CANPacket_t *to_send) {
   if (addr == 0x750) {
     // this address is sub-addressed. only allow tester present to radar (0xF)
     bool invalid_uds_msg = (GET_BYTES(to_send, 0, 4) != 0x003E020FU) || (GET_BYTES(to_send, 4, 4) != 0x0U);
-    if (invalid_uds_msg) {
+    // AleSato added some more hack'sss
+    bool valid_uds_msgs = (GET_BYTES(to_send, 0, 4) == 0x11300540U);  // automatic door locking and unlocking
+    if (invalid_uds_msg && !valid_uds_msgs) {
       tx = 0;
     }
   }
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index a37d9dc30..06cf175de 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -25,6 +25,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(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
@@ -46,6 +51,8 @@ class CarController(CarControllerBase):
     # FrogPilot variables
     params = Params()
 
+    self.doors_locked = False
+
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -205,5 +212,15 @@ class CarController(CarControllerBase):
     new_actuators.accel = self.accel
     new_actuators.gas = self.gas
 
+    # Lock doors when in drive / unlock doors when in park
+    if not self.doors_locked and CS.out.gearShifter != PARK:
+      if frogpilot_toggles.lock_doors:
+        can_sends.append(make_can_msg(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(make_can_msg(0x750, UNLOCK_CMD, 0))
+      self.doors_locked = False
+
     self.frame += 1
     return new_actuators, can_sends