From fa9d5c3ee232960eee5484ef9f88bb69f4c5d3fe Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 22 Jul 2024 23:15:44 -0700
Subject: [PATCH] FrogPilot setup - FrogPilot Process

---
 selfdrive/frogpilot/frogpilot_process.py | 86 ++++++++++++++++++++++++
 system/manager/process_config.py         |  3 +
 2 files changed, 89 insertions(+)
 create mode 100644 selfdrive/frogpilot/frogpilot_process.py

diff --git a/selfdrive/frogpilot/frogpilot_process.py b/selfdrive/frogpilot/frogpilot_process.py
new file mode 100644
index 000000000..91b91f8d4
--- /dev/null
+++ b/selfdrive/frogpilot/frogpilot_process.py
@@ -0,0 +1,86 @@
+import datetime
+import threading
+
+from cereal import log, messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import Priority, config_realtime_process
+from openpilot.common.time import system_time_valid
+
+from openpilot.selfdrive.frogpilot.controls.frogpilot_planner import FrogPilotPlanner
+from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import is_url_pingable
+
+OFFLINE = log.DeviceState.NetworkType.none
+
+locks = {
+  "time_checks": threading.Lock(),
+}
+
+running_threads = {}
+
+def run_thread_with_lock(name, lock, target, args):
+  if name not in running_threads or not running_threads[name].is_alive():
+    with lock:
+      thread = threading.Thread(target=target, args=args)
+      thread.start()
+      running_threads[name] = thread
+
+def time_checks(deviceState, now, started, params, params_memory):
+  if deviceState.networkType == OFFLINE:
+    return
+
+  if not is_url_pingable("https://github.com"):
+    return
+
+  screen_off = deviceState.screenBrightnessPercent == 0
+  wifi_connection = deviceState.networkType == WIFI
+
+def frogpilot_thread():
+  config_realtime_process(5, Priority.CTRL_LOW)
+
+  params = Params()
+  params_memory = Params("/dev/shm/params")
+
+  frogpilot_planner = FrogPilotPlanner()
+
+  run_time_checks = False
+  started_previously = False
+  time_validated = system_time_valid()
+
+  pm = messaging.PubMaster(['frogpilotPlan'])
+  sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl',
+                            'frogpilotCarState', 'frogpilotNavigation', 'modelV2', 'radarState'],
+                            poll='modelV2', ignore_avg_freq=['radarState'])
+
+  while True:
+    sm.update()
+
+    now = datetime.datetime.now()
+    deviceState = sm['deviceState']
+    started = deviceState.started
+
+    if not started and started_previously:
+      frogpilot_planner = FrogPilotPlanner()
+
+    if started and sm.updated['modelV2']:
+      frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'],
+                               sm['frogpilotNavigation'], sm['modelV2'], sm['radarState'], frogpilot_toggles)
+      frogpilot_planner.publish(sm, pm, frogpilot_toggles)
+
+    started_previously = started
+
+    if now.second == 0:
+      run_time_checks = True
+    elif run_time_checks or not time_validated:
+      run_thread_with_lock("time_checks", locks["time_checks"], time_checks, (deviceState, now, started, params, params_memory))
+      run_time_checks = False
+
+      if not time_validated:
+        time_validated = system_time_valid()
+        if not time_validated:
+          continue
+
+def main():
+  frogpilot_thread()
+
+if __name__ == "__main__":
+  main()
diff --git a/system/manager/process_config.py b/system/manager/process_config.py
index ced31077c..2cb2fc8b1 100644
--- a/system/manager/process_config.py
+++ b/system/manager/process_config.py
@@ -86,6 +86,9 @@ procs = [
   NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar),
   PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar),
   PythonProcess("webjoystick", "tools.bodyteleop.web", notcar),
+
+  # FrogPilot processes
+  PythonProcess("frogpilot_process", "selfdrive.frogpilot.frogpilot_process", always_run),
 ]
 
 managed_processes = {p.name: p for p in procs}