From 16bc06cb9885c015fd3fbe68bdb0d143ccbea55a Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Thu, 16 May 2024 02:18:42 -0700
Subject: [PATCH] Controls - Experimental Mode Activation - Long Press Distance

Enable/disable 'Experimental Mode' by holding down the 'distance' button on your steering wheel for 0.5 seconds.
---
 selfdrive/car/honda/carstate.py   |  3 +++
 selfdrive/car/hyundai/carstate.py |  6 ++++++
 selfdrive/car/interfaces.py       | 27 +++++++++++++++++++++++++++
 selfdrive/controls/controlsd.py   |  2 +-
 4 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 9604938..9029f43 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -274,6 +274,9 @@ class CarState(CarStateBase):
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
     # FrogPilot carstate functions
+    self.prev_distance_button = self.distance_button
+    self.distance_button = self.cruise_setting == 3
+
     self.lkas_previously_enabled = self.lkas_enabled
     self.lkas_enabled = self.cruise_setting == 1
 
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index e0cc6bb..851e43f 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -172,6 +172,9 @@ class CarState(CarStateBase):
       self.main_enabled = not self.main_enabled
 
     # FrogPilot carstate functions
+    self.prev_distance_button = self.distance_button
+    self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST
+
     if self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
       self.lkas_previously_enabled = self.lkas_enabled
       self.lkas_enabled = cp.vl["BCM_PO_11"]["LFA_Pressed"]
@@ -263,6 +266,9 @@ class CarState(CarStateBase):
                                           else cp_cam.vl["CAM_0x2a4"])
 
     # FrogPilot carstate functions
+    self.prev_distance_button = self.distance_button
+    self.distance_button = self.cruise_buttons[-1] == Buttons.GAP_DIST and self.prev_cruise_buttons == 0
+
     self.lkas_previously_enabled = self.lkas_enabled
     self.lkas_enabled = cp.vl[self.cruise_btns_msg_canfd]["LFA_BTN"]
 
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 51546a9..b476612 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -105,8 +105,11 @@ class CarInterfaceBase(ABC):
     self.belowSteerSpeed_shown = False
     self.disable_belowSteerSpeed = False
     self.disable_resumeRequired = False
+    self.prev_distance_button = False
     self.resumeRequired_shown = False
 
+    self.gap_counter = 0
+
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return ACCEL_MIN, ACCEL_MAX
@@ -253,6 +256,11 @@ class CarInterfaceBase(ABC):
     if ret.cruiseState.speedCluster == 0:
       ret.cruiseState.speedCluster = ret.cruiseState.speed
 
+    if self.CP.carName != "mock":
+      distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed")
+      fp_ret.distanceLongPressed = self.frogpilot_distance_functions(distance_button, self.prev_distance_button, frogpilot_variables)
+      self.prev_distance_button = distance_button
+
     # copy back for next iteration
     reader = ret.as_reader()
     frogpilot_reader = fp_ret.as_reader()
@@ -338,6 +346,22 @@ class CarInterfaceBase(ABC):
 
     return events
 
+  def frogpilot_distance_functions(self, distance_button, prev_distance_button, frogpilot_variables):
+    if distance_button:
+      self.gap_counter += 1
+    elif not prev_distance_button:
+      self.gap_counter = 0
+
+    if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance:
+      if frogpilot_variables.conditional_experimental_mode:
+        conditional_status = self.params_memory.get_int("CEStatus")
+        override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2
+        self.params_memory.put_int("CEStatus", override_value)
+      else:
+        experimental_mode = self.params.get_bool("ExperimentalMode")
+        self.params.put_bool("ExperimentalMode", not experimental_mode)
+
+    return self.gap_counter >= CRUISE_LONG_PRESS
 
 class RadarInterfaceBase(ABC):
   def __init__(self, CP):
@@ -381,6 +405,9 @@ class CarStateBase(ABC):
     self.lkas_enabled = False
     self.lkas_previously_enabled = False
 
+    self.prev_distance_button = 0
+    self.distance_button = 0
+
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
       self.v_ego_kf.set_x([[v_ego_raw], [0.0]])
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a802fae..eebb443 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -684,7 +684,7 @@ class Controls:
     if self.CP.openpilotLongitudinalControl:
       if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents) or self.onroad_distance_pressed:
         menu_open = self.display_timer > 0 or self.CP.carName != "gm" or not self.sm['frogpilotCarState'].hasCamera
-        if not self.params_memory.get_bool("OnroadDistanceButtonPressed") and menu_open:
+        if not (self.sm['frogpilotCarState'].distanceLongPressed or self.params_memory.get_bool("OnroadDistanceButtonPressed")) and menu_open:
           self.personality = (self.personality - 1) % 3
           self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
         self.display_timer = 350