From 69006ee3f289b0eb87df1424700d219a48b64193 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 15 May 2024 20:29:27 -0700
Subject: [PATCH] Controls - Experimental Mode Activation

Toggle Experimental Mode with either buttons on the steering wheel or the screen. Overrides 'Conditional Experimental Mode'.
---
 .../lib/conditional_experimental_mode.py      |  9 ++++++--
 selfdrive/ui/qt/onroad.cc                     | 21 ++++++++++++++++++-
 selfdrive/ui/ui.h                             |  2 ++
 3 files changed, 29 insertions(+), 3 deletions(-)

diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
index e2916ac..9d8fca3 100644
--- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
+++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py
@@ -29,12 +29,17 @@ class ConditionalExperimentalMode:
     self.stop_light_mac = MovingAverageCalculator()
 
   def update(self, carState, enabled, frogpilotNavigation, lead_distance, lead, modelData, road_curvature, slower_lead, v_ego, v_lead, frogpilot_toggles):
+    if frogpilot_toggles.experimental_mode_via_press and enabled:
+      overridden = self.params_memory.get_int("CEStatus")
+    else:
+      overridden = 0
+
     self.update_conditions(lead_distance, lead.status, modelData, road_curvature, slower_lead, carState.standstill, v_ego, v_lead, frogpilot_toggles)
 
     condition_met = self.check_conditions(carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles) and enabled
-    self.experimental_mode = condition_met
+    self.experimental_mode = condition_met and overridden not in {1, 3, 5} or overridden in {2, 4, 6}
 
-    self.params_memory.put_int("CEStatus", self.status_value if condition_met else 0)
+    self.params_memory.put_int("CEStatus", overridden if overridden in {1, 2, 3, 4, 5, 6} else self.status_value if condition_met else 0)
 
   def check_conditions(self, carState, frogpilotNavigation, lead, modelData, v_ego, frogpilot_toggles):
     if carState.standstill:
diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 41bbc22..8faf2dc 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -230,7 +230,12 @@ void ExperimentalButton::changeMode() {
   const auto cp = (*uiState()->sm)["carParams"].getCarParams();
   bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
   if (can_change) {
-    params.putBool("ExperimentalMode", !experimental_mode);
+    if (scene.conditional_experimental) {
+      int override_value = (scene.conditional_status >= 1 && scene.conditional_status <= 6) ? 0 : scene.conditional_status >= 7 ? 5 : 6;
+      paramsMemory.putIntNonBlocking("CEStatus", override_value);
+    } else {
+      params.putBool("ExperimentalMode", !experimental_mode);
+    }
   }
 }
 
@@ -901,6 +906,20 @@ void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
     newStatus = conditionalStatusMap[status != STATUS_DISENGAGED ? conditionalStatus : 0];
   }
 
+  QString distanceSuffix = tr(". Long press the \"distance\" button to revert");
+  QString lkasSuffix = tr(". Double press the \"LKAS\" button to revert");
+  QString screenSuffix = tr(". Double tap the screen to revert");
+
+  if (!alwaysOnLateralActive && !mapOpen && status != STATUS_DISENGAGED && !newStatus.isEmpty()) {
+    if (conditionalStatus == 1 || conditionalStatus == 2) {
+      newStatus += distanceSuffix;
+    } else if (conditionalStatus == 3 || conditionalStatus == 4) {
+      newStatus += lkasSuffix;
+    } else if (conditionalStatus == 5 || conditionalStatus == 6) {
+      newStatus += screenSuffix;
+    }
+  }
+
   if (newStatus != lastShownStatus) {
     displayStatusText = true;
     lastShownStatus = newStatus;
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 0997c2c..8d71ec7 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -116,6 +116,7 @@ typedef enum UIStatus {
 
   // FrogPilot statuses
   STATUS_ALWAYS_ON_LATERAL_ACTIVE,
+  STATUS_CONDITIONAL_OVERRIDDEN,
   STATUS_EXPERIMENTAL_MODE_ACTIVE,
   STATUS_NAVIGATION_ACTIVE,
 } UIStatus;
@@ -137,6 +138,7 @@ const QColor bg_colors [] = {
 
   // FrogPilot colors
   [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
+  [STATUS_CONDITIONAL_OVERRIDDEN] = QColor(0xff, 0xff, 0x00, 0xf1),
   [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1),
   [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1),
 };