diff --git a/frogpilot/common/frogpilot_utilities.py b/frogpilot/common/frogpilot_utilities.py index eae3ee72..7972c90b 100644 --- a/frogpilot/common/frogpilot_utilities.py +++ b/frogpilot/common/frogpilot_utilities.py @@ -6,13 +6,15 @@ import os import requests import subprocess import threading +import time import zipfile from pathlib import Path import openpilot.system.sentry as sentry -from cereal import messaging +from cereal import log, messaging +from openpilot.common.realtime import DT_DMON, DT_HW from openpilot.frogpilot.common.frogpilot_variables import EARTH_RADIUS, FROGS_GO_MOO_PATH @@ -193,3 +195,38 @@ def update_json_file(path, data): os.fsync(file.fileno()) os.replace(temp_path, path) + + +def wait_for_no_driver(params, sm, time_threshold=60): + while sm["deviceState"].screenBrightnessPercent != 0 or any(proc.name == "dmonitoringd" and proc.running for proc in sm["managerState"].processes): + sm.update() + + if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown): + return + + time.sleep(DT_HW) + + params.put_bool("IsDriverViewEnabled", True) + + while not any(proc.name == "dmonitoringd" and proc.running for proc in sm["managerState"].processes): + sm.update() + + time.sleep(DT_HW) + + start_time = time.monotonic() + while True: + sm.update() + + elapsed_time = time.monotonic() - start_time + if elapsed_time >= time_threshold: + break + + if any(ps.ignitionLine or ps.ignitionCan for ps in sm["pandaStates"] if ps.pandaType != log.PandaState.PandaType.unknown): + break + + if sm["driverMonitoringState"].faceDetected or not sm.alive["driverMonitoringState"]: + start_time = time.monotonic() + + time.sleep(DT_DMON) + + params.remove("IsDriverViewEnabled") diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py old mode 100755 new mode 100644 index 571934eb..2a657f7b --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -18,6 +18,8 @@ def dmonitoringd_thread(): # FrogPilot variables sm = sm.extend(['frogpilotCarState']) + driver_view_enabled = params.get_bool("IsDriverViewEnabled") + # 20Hz <- dmonitoringmodeld while True: sm.update() @@ -31,9 +33,11 @@ def dmonitoringd_thread(): elif valid: DM.run_step(sm, demo=demo_mode) # FrogPilot variables + elif driver_view_enabled: + DM.face_detected = sm['driverStateV2'].leftDriverData.faceProb > DM.settings._FACE_THRESHOLD or sm['driverStateV2'].rightDriverData.faceProb > DM.settings._FACE_THRESHOLD # publish - dat = DM.get_state_packet(valid=valid) + dat = DM.get_state_packet(valid=valid or driver_view_enabled) pm.send('driverMonitoringState', dat) # load live always-on toggle