Lock doors after ignition off for Toyota/Lexus

frogpilot/common/frogpilot_utilities.py

@@ -15,6 +15,8 @@ from pathlib import Path
 import openpilot.system.sentry as sentry
 
 from cereal import log, messaging
+from opendbc.can.parser import CANParser
+from opendbc.car.toyota.carcontroller import LOCK_CMD
 from openpilot.common.realtime import DT_DMON, DT_HW
 from panda import Panda
 
@@ -160,6 +162,11 @@ def flash_panda(params_memory):
   params_memory.remove("FlashPanda")
 
 
+def get_lock_status(can_parser, can_sock):
+  update_can_parser(can_parser, can_sock)
+  return can_parser.vl["DOOR_LOCKS"]["LOCK_STATUS"]
+
+
 @cache
 def is_FrogsGoMoo():
   return FROGS_GO_MOO_PATH.is_file()
@@ -203,6 +210,37 @@ def load_json_file(path):
   return {}
 
 
+def lock_doors(lock_doors_timer, sm, params):
+  wait_for_no_driver(params, sm, door_checks=True, time_threshold=lock_doors_timer)
+
+  sm.update()
+  if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
+    return
+
+  can_parser = CANParser("toyota_nodsu_pt_generated", [("DOOR_LOCKS", 3)], bus=0)
+  can_sock = messaging.sub_sock("can", timeout=100)
+
+  pm = messaging.PubMaster(["sendcan"])
+
+  while True:
+    sm.update()
+
+    if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown):
+      break
+
+    sendcan_send = messaging.new_message("sendcan", 1)
+    sendcan_send.sendcan[0].address = 0x750
+    sendcan_send.sendcan[0].dat = LOCK_CMD
+    sendcan_send.sendcan[0].src = 0
+    pm.send("sendcan", sendcan_send)
+
+    time.sleep(1)
+
+    lock_status = get_lock_status(can_parser, can_sock)
+    if lock_status == 0:
+      break
+
+
 def run_cmd(cmd, success_message, fail_message, env=None, report=True):
   try:
     result = subprocess.run(cmd, capture_output=True, check=True, env=env, text=True)
@@ -242,7 +280,10 @@ def use_konik_server():
   return KONIK_PATH.is_file()
 
 
-def wait_for_no_driver(params, sm, time_threshold=60):
+def wait_for_no_driver(params, sm, door_checks=False, time_threshold=60):
+  can_parser = CANParser("toyota_nodsu_pt_generated", [("BODY_CONTROL_STATE", 3)], bus=0)
+  can_sock = messaging.sub_sock("can", timeout=100)
+
   while sm["deviceState"].screenBrightnessPercent != 0 or any(proc.name == "dmonitoringd" and proc.running for proc in sm["managerState"].processes):
     sm.update()
 
@@ -272,6 +313,14 @@ def wait_for_no_driver(params, sm, time_threshold=60):
     if sm["driverMonitoringState"].faceDetected or not sm.alive["driverMonitoringState"]:
       start_time = time.monotonic()
 
+    if door_checks:
+      update_can_parser(can_parser, can_sock)
+
+      door_open = any([can_parser.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], can_parser.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
+                       can_parser.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], can_parser.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
+      if door_open:
+        start_time = time.monotonic()
+
     time.sleep(DT_DMON)
 
   params.remove("IsDriverViewEnabled")

frogpilot/frogpilot_process.py

@@ -11,7 +11,7 @@ from openpilot.common.time_helpers import system_time_valid
 from openpilot.frogpilot.assets.theme_manager import THEME_COMPONENT_PARAMS, ThemeManager
 from openpilot.frogpilot.common.frogpilot_backups import backup_toggles
 from openpilot.frogpilot.common.frogpilot_functions import update_openpilot
-from openpilot.frogpilot.common.frogpilot_utilities import ThreadManager, flash_panda, is_url_pingable
+from openpilot.frogpilot.common.frogpilot_utilities import ThreadManager, flash_panda, is_url_pingable, lock_doors
 from openpilot.frogpilot.common.frogpilot_variables import ERROR_LOGS_PATH, FrogPilotVariables
 from openpilot.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
 from openpilot.frogpilot.system.frogpilot_stats import send_stats
@@ -31,6 +31,9 @@ def check_assets(theme_manager, thread_manager, params_memory, frogpilot_toggles
 def transition_offroad(frogpilot_planner, thread_manager, time_validated, sm, params, frogpilot_toggles):
   params.put("LastGPSPosition", json.dumps(frogpilot_planner.gps_position))
 
+  if frogpilot_toggles.lock_doors_timer != 0:
+    thread_manager.run_with_lock(lock_doors, (frogpilot_toggles.lock_doors_timer, sm, params), report=False)
+
   if time_validated:
     thread_manager.run_with_lock(send_stats, (params, frogpilot_toggles))
 

selfdrive/pandad/pandad.cc

@@ -44,6 +44,7 @@
 ExitHandler do_exit;
 
 // FrogPilot variables
+static uint64_t last_door_lock_command_time = 0;
 
 bool check_all_connected(const std::vector<Panda *> &pandas) {
   for (const auto& panda : pandas) {
@@ -111,6 +112,17 @@ void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
     }
 
     // FrogPilot variables
+    for (const cereal::CanData::Reader &can : event.getSendcan()) {
+      if (can.getAddress() == 0x750) {
+        last_door_lock_command_time = nanos_since_boot();
+        Panda *internal_panda = pandas[0];
+        const std::optional<health_t> state = internal_panda->get_state();
+        if (state && state->safety_mode_pkt == (uint8_t)cereal::CarParams::SafetyModel::NO_OUTPUT) {
+          internal_panda->set_safety_model(cereal::CarParams::SafetyModel::TOYOTA);
+        }
+        break;
+      }
+    }
   }
 }
 
@@ -259,7 +271,7 @@ std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *>
     }
 
     // set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect
-    bool should_close_relay = !ignition_local || !is_onroad;
+    bool should_close_relay = (!ignition_local || !is_onroad) && (nanos_since_boot() - last_door_lock_command_time >= 2e9);
     if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
       panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
     }