From 0bf65192a2a77b1e04f17bd96b5872d543d17d8e Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Mon, 12 Aug 2024 17:49:53 -0400
Subject: [PATCH] in controlsd directly

---
 selfdrive/controls/controlsd.py         |  9 +++++++--
 selfdrive/controls/lib/drive_helpers.py | 23 +++++++----------------
 2 files changed, 14 insertions(+), 18 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 4850d36b56..a59f734535 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -489,8 +489,7 @@ class Controls:
   def state_transition(self, CS):
     """Compute conditional state transitions and execute actions on state transitions"""
 
-    self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change,
-                                         self.sm['longitudinalPlanSP'], self.experimental_mode)
+    self.v_cruise_helper.update_v_cruise(CS, self.enabled_long, self.is_metric, self.reverse_acc_change, self.sm['longitudinalPlanSP'])
 
     # decrement the soft disable timer at every step, as it's reset on
     # entrance in SOFT_DISABLING state
@@ -724,6 +723,12 @@ class Controls:
         self.personality = (self.personality - 1) % 3
         self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
 
+    # toggle experimental mode on distance button hold
+    if self.CP.openpilotLongitudinalControl:
+      if self.v_cruise_helper.experimental_mode_update and self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == 50:
+        self.experimental_mode = not self.experimental_mode
+        self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
+
     return CC, lac_log
 
   def publish_logs(self, CS, start_time, CC, lac_log):
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 99910e1ebf..f89ebe7081 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -4,7 +4,6 @@ import numpy as np
 from cereal import car, log, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
-from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL, DT_CTRL
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
@@ -86,11 +85,13 @@ class VCruiseHelper:
     self.slc_state_prev = SpeedLimitControlState.inactive
     self.slc_speed_limit_offsetted = 0
 
+    self.experimental_mode_update = False
+
   @property
   def v_cruise_initialized(self):
     return self.v_cruise_kph != V_CRUISE_UNSET
 
-  def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp, experimental_mode):
+  def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp):
     self.v_cruise_kph_last = self.v_cruise_kph
     self.slc_state = long_plan_sp.speedLimitControlState
 
@@ -109,7 +110,7 @@ class VCruiseHelper:
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
 
       if self.CP.openpilotLongitudinalControl:
-        self._update_experimental_mode(CS, experimental_mode)
+        self._update_experimental_mode(CS)
         self.update_button_timers(CS, enabled)
     else:
       self.v_cruise_kph = V_CRUISE_UNSET
@@ -246,27 +247,17 @@ class VCruiseHelper:
     self.is_metric_prev = is_metric
 
   # toggle experimental mode on distance button hold
-  def _update_experimental_mode(self, CS, experimental_mode):
-    long_press = False
-    button_type = None
+  def _update_experimental_mode(self, CS):
+    self.experimental_mode_update = False
 
     for b in CS.buttonEvents:
       if b.type == ButtonType.gapAdjustCruise and not b.pressed:
         if self.button_timers[ButtonType.gapAdjustCruise] > CRUISE_LONG_PRESS:
           return  # end long press
-        button_type = b.type
         break
     else:
       if self.button_timers[ButtonType.gapAdjustCruise] and self.button_timers[ButtonType.gapAdjustCruise] % CRUISE_LONG_PRESS == 0:
-        button_type = ButtonType.gapAdjustCruise
-        long_press = True
-
-    if button_type is None:
-      return
-
-    if long_press and self.button_timers[button_type] == CRUISE_LONG_PRESS:
-      self.experimental_mode_updated = True
-      self.param_s.put_bool_nonblocking("ExperimentalMode", not experimental_mode)
+        self.experimental_mode_update = True
 
 
 def clip_curvature(v_ego, prev_curvature, new_curvature):