From 2931f4932f1431fa06b24f3b2d3f7d936f86471e Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Fri, 7 Jun 2024 03:37:21 -0700
Subject: [PATCH] FrogPilot setup - FrogPilot Process

---
 selfdrive/frogpilot/frogpilot_process.py | 70 ++++++++++++++++++++++++
 system/manager/process_config.py         |  3 +
 2 files changed, 73 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..ca9003096
--- /dev/null
+++ b/selfdrive/frogpilot/frogpilot_process.py
@@ -0,0 +1,70 @@
+import datetime
+import http.client
+import socket
+import urllib.error
+import urllib.request
+
+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 FrogPilotFunctions
+
+WIFI = log.DeviceState.NetworkType.wifi
+
+def github_pinged(url="https://github.com", timeout=5):
+  try:
+    urllib.request.urlopen(url, timeout=timeout)
+    return True
+  except (urllib.error.URLError, socket.timeout, http.client.RemoteDisconnected):
+    return False
+
+def time_checks(deviceState, now, params, params_memory):
+  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_functions = FrogPilotFunctions()
+  frogpilot_planner = FrogPilotPlanner()
+
+  time_validated = system_time_valid()
+
+  pm = messaging.PubMaster(['frogpilotPlan'])
+  sm = messaging.SubMaster(['carState', 'controlsState', 'deviceState', 'frogpilotCarControl', 'frogpilotCarState', 'frogpilotNavigation',
+                            'frogpilotPlan', 'longitudinalPlan', 'modelV2', 'radarState'],
+                            poll='modelV2', ignore_avg_freq=['radarState'])
+
+  while True:
+    sm.update()
+
+    now = datetime.datetime.now()
+    deviceState = sm['deviceState']
+    started = deviceState.started
+
+    if started and sm.updated['modelV2']:
+      frogpilot_planner.update(sm['carState'], sm['controlsState'], sm['frogpilotCarControl'], sm['frogpilotCarState'],
+                               sm['frogpilotNavigation'], sm['modelV2'], sm['radarState'])
+      frogpilot_planner.publish(sm, pm)
+
+    if now.second == 0 or not time_validated:
+      if not started:
+        if github_pinged():
+          time_checks(deviceState, now, params, params_memory)
+
+      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}