Toyota: Automatically Lock/Unlock Doors

reword
Edison-CBS · Aug 3, 2024 · d8086ee · d8086ee
1 parent 7215a17
commit d8086ee
Show file tree

Hide file tree

Showing 5 changed files with 31 additions and 1 deletion.
diff --git a/common/params.cc b/common/params.cc
@@ -200,6 +200,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
+
+    // extra parameters
+    {"LockDoors", PERSISTENT | FROGPILOT_STORAGE | FROGPILOT_VEHICLES},
 };
 
 } // namespace

diff --git a/panda b/panda
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
@@ -26,6 +26,12 @@
 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
+
 # Time values for hysteresis
 RESUME_HYSTERESIS_TIME = 1.5  # seconds
 
@@ -52,6 +58,8 @@ def __init__(self, dbc_name, CP, VM):
     self.gas = 0
     self.accel = 0
 
+    self.doors_locked = False
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -209,5 +217,15 @@ def update(self, CC, CS, now_nanos):
     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 self.params.get_bool("ToyotaDoors") and self.CP.carName == "toyota":
+        can_sends.append(make_can_msg(0x750, LOCK_CMD, 0))
+        self.doors_locked = True
+    elif self.doors_locked and CS.out.gearShifter == PARK:
+      if self.params.get_bool("ToyotaDoors") and self.CP.carName == "toyota":
+        can_sends.append(make_can_msg(0x750, UNLOCK_CMD, 0))
+        self.doors_locked = False
+
     self.frame += 1
     return new_actuators, can_sends
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
@@ -69,6 +69,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
       tr("Display speed in km/h instead of mph."),
       "../assets/offroad/icon_metric.png",
     },
+    {
+      "ToyotaDoors",
+      tr("Automatically Lock/Unlock Doors"),
+      tr("Automatically lock the doors when in drive and unlock when in park."),
+      "",
+    },
   };
 
 

diff --git a/system/manager/manager.py b/system/manager/manager.py
@@ -40,6 +40,9 @@ def manager_init() -> None:
     ("LanguageSetting", "main_en"),
     ("OpenpilotEnabledToggle", "1"),
     ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)),
+
+    # Default extra parameters
+    ("ToyotaDoors", "1"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))