Longitudinal: Distance button hold to toggle Chill/Experimental Mode (#576)

* Longitudinal: Distance button hold to toggle Chill/Experimental Mode

* unused

* fix

* no need

* Refactor: Introduce ButtonHoldTracker to manage button hold durations (#593)

Add ButtonHoldTracker for button hold logic and tests

Introduce a new `ButtonHoldTracker` class to manage button hold durations, replacing manual timer handling in `ExperimentalSwitcher`. Updated `ExperimentalSwitcher` to leverage this implementation for cleaner and more modular code. Added comprehensive unit tests for both `ButtonHoldTracker` and `ExperimentalSwitcher` to ensure functionality and edge case coverage.

* Revert "Refactor: Introduce ButtonHoldTracker to manage button hold durations (#593)"

This reverts commit 7ee7e73ce7.

* less in selfdrived

* pass carparams into child

* tests for cruisehelper

* rename these bad bois (happy now @devtekve? xD)

* Apply suggestions from code review

Co-authored-by: DevTekVE <devtekve@gmail.com>

---------

Co-authored-by: DevTekVE <devtekve@gmail.com>

cereal/log.capnp

@@ -199,6 +199,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
     silentParkBrake @162;
     controlsMismatchLateral @163;
     hyundaiRadarTracksConfirmed @164;
+    experimentalModeSwitched @165;
 
     soundsUnavailableDEPRECATED @47;
   }

selfdrive/selfdrived/events.py

@@ -1066,6 +1066,10 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
 
   EventName.hyundaiRadarTracksConfirmed: {
     ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize")
+  },
+
+  EventName.experimentalModeSwitched: {
+    ET.WARNING: NormalPermanentAlert("Experimental Mode Switched", duration=1.5)
   }
 
 }

selfdrive/selfdrived/selfdrived.py

@@ -25,6 +25,7 @@ from openpilot.system.version import get_build_metadata
 
 from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem
 from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP
+from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper
 
 REPLAY = "REPLAY" in os.environ
 SIMULATION = "SIMULATION" in os.environ
@@ -44,7 +45,7 @@ SafetyModel = car.CarParams.SafetyModel
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
-class SelfdriveD:
+class SelfdriveD(CruiseHelper):
   def __init__(self, CP=None):
     self.params = Params()
 
@@ -140,6 +141,8 @@ class SelfdriveD:
 
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
 
+    CruiseHelper.__init__(self, self.CP)
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
@@ -367,12 +370,16 @@ class SelfdriveD:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    CruiseHelper.update(self, CS, self.events, self.experimental_mode)
+
     # decrement personality on distance button press
     if self.CP.openpilotLongitudinalControl:
       if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
-        self.personality = (self.personality - 1) % 3
-        self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
-        self.events.add(EventName.personalityChanged)
+        if not self.experimental_mode_switched:
+          self.personality = (self.personality - 1) % 3
+          self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+          self.events.add(EventName.personalityChanged)
+        self.experimental_mode_switched = False
 
   def data_sample(self):
     car_state = messaging.recv_one(self.car_state_sock)

sunnypilot/selfdrive/car/cruise_helpers.py

@@ -0,0 +1,50 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from cereal import car, log
+from opendbc.car import structs
+from openpilot.common.params import Params
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventName = log.OnroadEvent.EventName
+
+DISTANCE_LONG_PRESS = 50
+
+
+class CruiseHelper:
+  def __init__(self, CP: structs.CarParams):
+    self.CP = CP
+    self.params = Params()
+
+    self.button_frame_counts = {ButtonType.gapAdjustCruise: 0}
+    self._experimental_mode = False
+    self.experimental_mode_switched = False
+
+  def update(self, CS, events, experimental_mode) -> None:
+    if self.CP.openpilotLongitudinalControl:
+      if CS.cruiseState.available:
+        self.update_button_frame_counts(CS)
+
+        # toggle experimental mode once on distance button hold
+        self.update_experimental_mode(events, experimental_mode)
+
+  def update_button_frame_counts(self, CS) -> None:
+    for button in self.button_frame_counts:
+      if self.button_frame_counts[button] > 0:
+        self.button_frame_counts[button] += 1
+
+    for button_event in CS.buttonEvents:
+      button = button_event.type.raw
+      if button in self.button_frame_counts:
+        self.button_frame_counts[button] = int(button_event.pressed)
+
+  def update_experimental_mode(self, events, experimental_mode) -> None:
+    if self.button_frame_counts[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
+      self._experimental_mode = not experimental_mode
+      self.params.put_bool_nonblocking("ExperimentalMode", self._experimental_mode)
+      events.add(EventName.experimentalModeSwitched)
+      self.experimental_mode_switched = True

sunnypilot/selfdrive/car/tests/test_cruise_mode.py

@@ -0,0 +1,66 @@
+from parameterized import parameterized_class
+from cereal import car
+from openpilot.selfdrive.selfdrived.events import Events
+from openpilot.sunnypilot.selfdrive.car.cruise_helpers import CruiseHelper, DISTANCE_LONG_PRESS
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+
+
+@parameterized_class(('openpilot_longitudinal',), [(True,)])
+class TestCruiseHelper:
+  def setup_method(self):
+    self.CP = car.CarParams(openpilotLongitudinalControl=self.openpilot_longitudinal)
+    self.cruise_helper = CruiseHelper(self.CP)
+    self.cruise_helper.experimental_mode_switched = False
+    self.events = Events()
+
+  def reset(self):
+    for _ in range(2):
+      CS = car.CarState(cruiseState={"available": False})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=False)]
+      self.cruise_helper._experimental_mode = False
+      self.cruise_helper.experimental_mode_switched = False
+      self.cruise_helper.update(CS, self.events, False)
+
+
+  def test_gap_adjust_cruise_long_press_toggle_mode(self) -> None:
+    for pressed in (True, False):
+      for experimental_mode in (True, False):
+        self.reset()
+        self.cruise_helper._experimental_mode = experimental_mode
+        toggled_mode = not experimental_mode if pressed else experimental_mode
+
+        for i in range(DISTANCE_LONG_PRESS):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
+          self.cruise_helper.update(CS, self.events, experimental_mode)
+
+        # mode should be toggled
+        assert self.cruise_helper._experimental_mode == toggled_mode
+        assert self.cruise_helper.experimental_mode_switched is pressed
+
+        # keep holding button after switching mode
+        for _ in range(DISTANCE_LONG_PRESS):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)]
+          self.cruise_helper.update(CS, self.events, toggled_mode)
+
+        # mode should not be toggled
+        assert self.cruise_helper._experimental_mode == toggled_mode
+        assert self.cruise_helper.experimental_mode_switched is pressed
+
+  def test_gap_adjust_cruise_short_press_toggle_mode(self) -> None:
+    for pressed in (True, False):
+      for experimental_mode in (True, False):
+        self.reset()
+        self.cruise_helper._experimental_mode = experimental_mode
+
+        for i in range(DISTANCE_LONG_PRESS - 1):
+          CS = car.CarState(cruiseState={"available": True})
+          CS.buttonEvents = [ButtonEvent(type=ButtonType.gapAdjustCruise, pressed=pressed)] if i == 0 else []
+          self.cruise_helper.update(CS, self.events, experimental_mode)
+
+        # mode should not be toggled
+        assert self.cruise_helper._experimental_mode == experimental_mode
+        assert self.cruise_helper.experimental_mode_switched is False