Merge branch 'long-distance-btn-exp-mode' into master-dev-c3-new

cereal/log.capnp

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

selfdrive/selfdrived/events.py

@@ -1063,6 +1063,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.experimental_switcher import ExperimentalSwitcher
 
 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(ExperimentalSwitcher):
   def __init__(self, CP=None):
     self.params = Params()
 
@@ -140,6 +141,8 @@ class SelfdriveD:
 
     self.car_events_sp = CarSpecificEventsSP(self.CP, self.params)
 
+    ExperimentalSwitcher.__init__(self, self.params)
+
   def update_events(self, CS):
     """Compute onroadEvents from carState"""
 
@@ -367,12 +370,18 @@ class SelfdriveD:
       if self.sm['modelV2'].frameDropPerc > 20:
         self.events.add(EventName.modeldLagging)
 
+    # toggle experimental mode once on distance button hold
+    if self.CP.openpilotLongitudinalControl:
+      ExperimentalSwitcher.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/experimental_switcher.py

@@ -0,0 +1,42 @@
+"""
+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
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventName = log.OnroadEvent.EventName
+
+DISTANCE_LONG_PRESS = 50
+
+
+class ExperimentalSwitcher:
+  def __init__(self, params):
+    self.params = params
+
+    self.button_timers = {ButtonType.gapAdjustCruise: 0}
+    self.experimental_mode_switched = False
+
+  def update(self, CS, events, experimental_mode):
+    if CS.cruiseState.available:
+      self.update_button_timers(CS)
+      self.update_mode(events, experimental_mode)
+
+  def update_button_timers(self, CS) -> None:
+    for k in self.button_timers:
+      if self.button_timers[k] > 0:
+        self.button_timers[k] += 1
+
+    for b in CS.buttonEvents:
+      if b.type.raw in self.button_timers:
+        self.button_timers[b.type.raw] = 1 if b.pressed else 0
+
+  def update_mode(self, events, experimental_mode) -> None:
+    if self.button_timers[ButtonType.gapAdjustCruise] >= DISTANCE_LONG_PRESS and not self.experimental_mode_switched:
+      experimental_mode = not experimental_mode
+      self.params.put_bool_nonblocking("ExperimentalMode", experimental_mode)
+      events.add(EventName.experimentalModeSwitched)
+      self.experimental_mode_switched = True