diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index 3f45cb22..814a77c6 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/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") diff --git a/frogpilot/frogpilot_process.py b/frogpilot/frogpilot_process.py index 24423cb8..ac9b74aa 100644 --- a/frogpilot/frogpilot_process.py +++ b/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)) diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index fe5a1b6d..70b80ffa 100644 --- a/selfdrive/pandad/pandad.cc +++ b/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 &pandas) { for (const auto& panda : pandas) { @@ -111,6 +112,17 @@ void can_send_thread(std::vector 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 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 send_panda_states(PubMaster *pm, const std::vector } // 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); }