From 1e930ec10ba8e858f7fe9800c3ce4a6d16c94b01 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 31 Jul 2024 19:39:35 -0700
Subject: [PATCH] FrogPilot features - Force device onroad/offroad

---
 selfdrive/ui/qt/offroad/settings.cc | 23 ++++++++++++++++++++++-
 selfdrive/ui/qt/offroad/settings.h  |  5 +++++
 selfdrive/ui/ui.cc                  |  3 +++
 system/hardware/hardwared.py        |  6 ++++++
 4 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index deee70cdf..990305182 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -272,7 +272,9 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
       }
     }
     for (FrogPilotButtonsControl *btn : findChildren<FrogPilotButtonsControl *>()) {
-      btn->setEnabled(offroad);
+      if (btn != forceStartedBtn) {
+        btn->setEnabled(offroad);
+      }
     }
   });
 
@@ -459,6 +461,25 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
   });
   addItem(toggleBackupBtn);
 
+  // Force offroad/onroad
+  std::vector<QString> forceStartedOptions{tr("OFFROAD"), tr("ONROAD"), tr("OFF")};
+  forceStartedBtn = new FrogPilotButtonsControl(tr("Force Started State"), tr("Force openpilot either offroad or onroad."), "", forceStartedOptions, true);
+  connect(forceStartedBtn, &FrogPilotButtonsControl::buttonClicked, [=](int id) {
+    if (id == 0) {
+      paramsMemory.putBool("ForceOffroad", true);
+      paramsMemory.putBool("ForceOnroad", false);
+    } else if (id == 1) {
+      paramsMemory.putBool("ForceOffroad", false);
+      paramsMemory.putBool("ForceOnroad", true);
+    } else if (id == 2) {
+      paramsMemory.putBool("ForceOffroad", false);
+      paramsMemory.putBool("ForceOnroad", false);
+    }
+    forceStartedBtn->updateButtonStyles(id);
+  });
+  forceStartedBtn->updateButtonStyles(2);
+  addItem(forceStartedBtn);
+
   // power buttons
   QHBoxLayout *power_layout = new QHBoxLayout();
   power_layout->setSpacing(30);
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index 9b680723f..93e24214d 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -72,6 +72,11 @@ private slots:
 private:
   Params params;
   ButtonControl *pair_device;
+
+  // FrogPilot variables
+  Params paramsMemory{"/dev/shm/params"};
+
+  FrogPilotButtonsControl *forceStartedBtn;
 };
 
 class TogglesPanel : public ListWidget {
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index dfcaa2295..df00bf26b 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -286,6 +286,9 @@ void UIState::updateStatus() {
     }
   }
 
+  scene.started |= paramsMemory.getBool("ForceOnroad");
+  scene.started &= !paramsMemory.getBool("ForceOffroad");
+
   // Handle onroad/offroad transition
   if (scene.started != started_prev || sm->frame == 1) {
     if (scene.started) {
diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py
index 574a872fb..36d8584e0 100755
--- a/system/hardware/hardwared.py
+++ b/system/hardware/hardwared.py
@@ -209,6 +209,8 @@ def hardware_thread(end_event, hw_queue) -> None:
   # FrogPilot variables
   frogpilot_toggles = FrogPilotVariables.toggles
 
+  params_memory = Params("/dev/shm/params")
+
   update_toggles = False
 
   while not end_event.is_set():
@@ -343,6 +345,10 @@ def hardware_thread(end_event, hw_queue) -> None:
     if started_ts is None:
       should_start = should_start and all(startup_conditions.values())
 
+    # Handle force offroad/onroad
+    should_start |= params_memory.get_bool("ForceOnroad")
+    should_start &= not params_memory.get_bool("ForceOffroad")
+
     if should_start != should_start_prev or (count == 0):
       params.put_bool("IsEngaged", False)
       engaged_prev = False