Longitudinal: Refactor Experimental Mode toggle with distance button …

…hold (commaai#409)

* init

* deprecated

* in controlsd directly

* update

* unused

* combine

* only once

* fix

* better

* cleanup

* more

* Update CHANGELOGS.md

CHANGELOGS.md

@@ -40,6 +40,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
   * Auto Unlock by Shift to P: All doors are automatically unlocked when shifting the shift lever to P
 * FIXED: Driving Personality:
   * Maniac mode now correctly enforced when selected
+* FIXED: Experimental Model Distance Button Hold
+  * Experimental Model toggle with distance button hold no longer changes Personality
+  * Personality setting remains consistent when switching between Chill and Experimental Mode
 * UI Updates
   * Display Metrics Below Chevron
     * NEW❗: Time to Lead Car

selfdrive/car/chrysler/interface.py

@@ -135,7 +135,7 @@ def _update(self, c):
         self.CS.accEnabled = False
       self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/ford/interface.py

@@ -105,7 +105,7 @@ def _update(self, c):
         self.CS.accEnabled = False
       self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/gm/interface.py

@@ -204,8 +204,6 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_loopback)
 
-    distance_button = 0
-
     # Don't add event if transitioning from INIT, unless it's to an actual button
     if self.CS.cruise_buttons != CruiseButtons.UNPRESS or self.CS.prev_cruise_buttons != CruiseButtons.INIT:
       self.CS.button_events = [
@@ -214,7 +212,6 @@ def _update(self, c):
         *create_button_events(self.CS.distance_button, self.CS.prev_distance_button,
                               {1: ButtonType.gapAdjustCruise})
       ]
-      distance_button = self.CS.distance_button
 
     self.CS.button_events = [
       *self.CS.button_events,
@@ -251,7 +248,7 @@ def _update(self, c):
         self.CS.accEnabled = False
       self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/honda/interface.py

@@ -300,7 +300,7 @@ def _update(self, c):
       elif not ret.cruiseState.enabled:
         self.CS.accEnabled = False
 
-    ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_setting == 3))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/hyundai/interface.py

@@ -261,7 +261,7 @@ def _update(self, c):
       self.get_sp_cancel_cruise_state()
       ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=(self.CS.cruise_buttons[-1] == 3))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/interfaces.py

@@ -244,8 +244,6 @@ def __init__(self, CP, CarController, CarState):
     self.cruise_cancelled_btn = True
     self.prev_acc_mads_combo = False
     self.mads_event_lock = True
-    self.gap_button_counter = 0
-    self.experimental_mode_hold = False
     self.last_mads_init = 0.
     self.madsEnabledInit = False
     self.madsEnabledInitPrev = False
@@ -616,7 +614,7 @@ def get_sp_started_mads(self, cs_out, mads_enabled):
     else:
       return mads_enabled
 
-  def get_sp_common_state(self, cs_out, gear_allowed=True, gap_button=False):
+  def get_sp_common_state(self, cs_out, gear_allowed=True):
     cs_out.cruiseState.enabled = self.CS.accEnabled if not self.CP.pcmCruise or not self.CP.pcmCruiseSpeed else cs_out.cruiseState.enabled
 
     if not self.enable_mads:
@@ -625,9 +623,6 @@ def get_sp_common_state(self, cs_out, gear_allowed=True, gap_button=False):
       elif not cs_out.cruiseState.enabled and self.CS.out.cruiseState.enabled:
         self.CS.madsEnabled = False
 
-    if self.CP.openpilotLongitudinalControl:
-      self.toggle_exp_mode(gap_button)  # TODO-SP: use buttonEvents to handle this, then remove gap_button
-
     lane_change_speed_min = get_min_lateral_speed(self.CS.params_list.pause_lateral_speed, self.CS.params_list.is_metric)
 
     cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.CS.params_list.below_speed_pause
@@ -655,19 +650,6 @@ def get_sp_common_state(self, cs_out, gear_allowed=True, gap_button=False):
 
     return cs_out
 
-  # TODO: SP: use upstream's buttonEvents counter checks from controlsd
-  def toggle_exp_mode(self, gap_pressed):
-    if gap_pressed:
-      if not self.experimental_mode_hold:
-        self.gap_button_counter += 1
-        if self.gap_button_counter > 50:
-          self.gap_button_counter = 0
-          self.experimental_mode_hold = True
-          self.param_s.put_bool_nonblocking("ExperimentalMode", not self.CS.params_list.experimental_mode)
-    else:
-      self.gap_button_counter = 0
-      self.experimental_mode_hold = False
-
   def create_sp_events(self, cs_out, events, main_enabled=False, allow_enable=True, enable_pressed=False,
                        enable_from_brake=False, enable_pressed_long=False,
                        enable_buttons=(ButtonType.accelCruise, ButtonType.decelCruise)):

selfdrive/car/mazda/interface.py

@@ -71,7 +71,7 @@ def _update(self, c):
         self.CS.accEnabled = False
       self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/nissan/interface.py

@@ -50,7 +50,7 @@ def _update(self, c):
       self.get_sp_cancel_cruise_state()
       ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(self.CS.distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/toyota/interface.py

@@ -208,11 +208,8 @@ def init(CP, logcan, sendcan):
   def _update(self, c):
     ret = self.CS.update(self.cp, self.cp_cam)
 
-    distance_button = 0
-
     if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
       self.CS.button_events = create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise})
-      distance_button = self.CS.distance_button
 
     self.CS.mads_enabled = self.get_sp_cruise_main_state(ret)
 
@@ -238,7 +235,7 @@ def _update(self, c):
       if not self.CP.pcmCruise:
         ret.cruiseState.enabled = self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=bool(distance_button))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/car/volkswagen/interface.py

@@ -140,7 +140,7 @@ def _update(self, c):
         self.CS.accEnabled = False
       self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
 
-    ret = self.get_sp_common_state(ret, gap_button=any(b.type == ButtonType.gapAdjustCruise and b.pressed for b in self.CS.button_events))
+    ret = self.get_sp_common_state(ret)
 
     ret.buttonEvents = [
       *self.CS.button_events,

selfdrive/controls/controlsd.py

@@ -20,7 +20,7 @@
 
 from openpilot.selfdrive.car.car_helpers import get_car_interface, get_startup_event
 from openpilot.selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
-from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature
+from openpilot.selfdrive.controls.lib.drive_helpers import VCruiseHelper, clip_curvature, get_lag_adjusted_curvature, CRUISE_LONG_PRESS
 from openpilot.selfdrive.controls.lib.events import Events, ET
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -179,6 +179,7 @@ def __init__(self, CI=None):
     self.mads_disengage_lateral_on_brake = self.params.get_bool("DisengageLateralOnBrake")
     self.mads_ndlob = self.enable_mads and not self.mads_disengage_lateral_on_brake
     self.process_not_running = False
+    self.experimental_mode_update = False
 
     self.custom_model_metadata = CustomModelMetadata(params=self.params, init_only=True)
     self.model_use_lateral_planner = self.custom_model_metadata.valid and \
@@ -717,11 +718,21 @@ def state_control(self, CS):
         cloudlog.error(f"actuators.{p} not finite {actuators.to_dict()}")
         setattr(actuators, p, 0.0)
 
+    # toggle experimental mode once on distance button hold
+    if self.CP.openpilotLongitudinalControl:
+      if self.v_cruise_helper.button_timers[ButtonType.gapAdjustCruise] == CRUISE_LONG_PRESS and \
+            not self.experimental_mode_update:
+        self.experimental_mode = not self.experimental_mode
+        self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
+        self.experimental_mode_update = True
+
     # 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))
+        if not self.experimental_mode_update:
+          self.personality = (self.personality - 1) % 3
+          self.params.put_nonblocking('LongitudinalPersonality', str(self.personality))
+        self.experimental_mode_update = False
 
     return CC, lac_log
 

selfdrive/controls/lib/drive_helpers.py

@@ -75,7 +75,7 @@ def __init__(self, CP):
     self.v_cruise_kph = V_CRUISE_UNSET
     self.v_cruise_cluster_kph = V_CRUISE_UNSET
     self.v_cruise_kph_last = 0
-    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
+    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0, ButtonType.gapAdjustCruise: 0}
     self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
 
     self.is_metric_prev = None
@@ -101,10 +101,10 @@ def update_v_cruise(self, CS, enabled, is_metric, reverse_acc, long_plan_sp):
         self._update_v_cruise_non_pcm(CS, enabled, is_metric, reverse_acc)
         self._update_v_cruise_slc(long_plan_sp)
         self.v_cruise_cluster_kph = self.v_cruise_kph
-        self.update_button_timers(CS, enabled)
       else:
         self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
         self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+      self.update_button_timers(CS, enabled)
     else:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
@@ -137,7 +137,7 @@ def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, reverse_acc):
           long_press = True
           break
 
-    if button_type is None:
+    if button_type is None or button_type == ButtonType.gapAdjustCruise:
       return
 
     resume_button = ButtonType.accelCruise