Toyota: Automatically Lock/Unlock Doors

reword params: fix not defined values fix
Edison-CBS · Aug 4, 2024 · ffe93f1 · ffe93f1
1 parent 7215a17
commit ffe93f1
Show file tree

Hide file tree

Showing 16 changed files with 119 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
+    {"ToyotaDoors", PERSISTENT},
 };
 
 } // 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/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts
@@ -1078,6 +1078,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts
@@ -1062,6 +1062,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts
@@ -1062,6 +1062,14 @@ Esto puede tardar un minuto.</translation>
         <source>Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode.</source>
         <translation>Activar el control longitudinal (fase experimental) para permitir el modo Experimental.</translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts
@@ -1062,6 +1062,14 @@ Cela peut prendre jusqu&apos;à une minute.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts
@@ -1056,6 +1056,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts
@@ -1058,6 +1058,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts
@@ -1062,6 +1062,14 @@ Isso pode levar até um minuto.</translation>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado.</translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts
@@ -1058,6 +1058,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts
@@ -1056,6 +1056,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation type="unfinished"></translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts
@@ -1058,6 +1058,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活时也启用驾驶员监控。</translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts
@@ -1058,6 +1058,14 @@ This may take up to a minute.</source>
         <source>Enable driver monitoring even when openpilot is not engaged.</source>
         <translation>即使在openpilot未激活時也啟用駕駛監控。</translation>
     </message>
+    <message>
+        <source>Automatically Lock/Unlock Doors</source>
+        <translation type="unfinished"></translation>
+    </message>
+    <message>
+        <source>Automatically lock the doors when in drive and unlock when in park.</source>
+        <translation type="unfinished"></translation>
+    </message>
 </context>
 <context>
     <name>Updater</name>

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')))