From ac520a882dafc67a1caa9bae36e8f5ce6194b317 Mon Sep 17 00:00:00 2001
From: Rick Lan <ricklan@gmail.com>
Date: Wed, 19 Jun 2024 14:03:19 +0800
Subject: [PATCH] toyota door lock controller

---
 common/params.cc                      | 2 ++
 selfdrive/car/toyota/carcontroller.py | 8 ++++++++
 system/manager/manager.py             | 2 ++
 3 files changed, 12 insertions(+)

diff --git a/common/params.cc b/common/params.cc
index 8b2622a0c..0dc29a43d 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -224,6 +224,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"dp_nav_free_map", PERSISTENT},
     {"dp_nav_name", PERSISTENT},
     {"dp_nav_traffic", PERSISTENT},
+    {"dp_toyota_auto_lock", PERSISTENT},
+    {"dp_toyota_auto_unlock", PERSISTENT},
 };
 
 } // namespace
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f9b7a478e..a1165660a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -8,6 +8,8 @@ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
+from dp_ext.selfdrive.car.toyota.door_lock_controller import DoorLockController
+
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -41,6 +43,8 @@ class CarController(CarControllerBase):
     self.packer = CANPacker(dbc_name)
     self.gas = 0
     self.accel = 0
+    # dp
+    self.dlc = DoorLockController()
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
@@ -51,6 +55,10 @@ class CarController(CarControllerBase):
     # *** control msgs ***
     can_sends = []
 
+    result = self.dlc.process(CS.out.gearShifter, CS.out.vEgo, CS.out.doorOpen)
+    if result:
+      can_sends.append(make_can_msg(result[0], result[1], result[2]))
+
     # *** steer torque ***
     new_steer = int(round(actuators.steer * self.params.STEER_MAX))
     apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
diff --git a/system/manager/manager.py b/system/manager/manager.py
index f5336942c..6336b29e7 100755
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@@ -56,6 +56,8 @@ def manager_init() -> None:
     ("dp_nav_free_map", "0"),
     ("dp_nav_name", "0"),
     ("dp_nav_traffic", "0"),
+    ("dp_toyota_auto_lock", "0"),
+    ("dp_toyota_auto_unlock", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))