Custom steering wheel button functions

frogpilot/controls/frogpilot_card.py

@@ -1,11 +1,12 @@
 #!/usr/bin/env python3
 from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from openpilot.common.params import Params
-from openpilot.selfdrive.car.cruise import ButtonType
+from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType
 from openpilot.selfdrive.selfdrived.events import ET
 
 from openpilot.frogpilot.common.frogpilot_utilities import is_FrogsGoMoo
 from openpilot.frogpilot.common.frogpilot_variables import NON_DRIVING_GEARS
+from openpilot.frogpilot.controls.lib.conditional_experimental_mode import CEStatus
 
 class FrogPilotCard:
   def __init__(self, CP, FPCP):
@@ -17,10 +18,33 @@ class FrogPilotCard:
     self.accel_pressed = False
     self.always_on_lateral_allowed = False
     self.decel_pressed = False
+    self.distancePressed_previously = False
+
+    self.gap_counter = 0
 
     self.always_on_lateral_set = bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
     self.frogs_go_moo = is_FrogsGoMoo()
 
+    self.long_press_threshold = CRUISE_LONG_PRESS * (1.5 if self.CP.brand == "gm" else 1)
+    self.very_long_press_threshold = CRUISE_LONG_PRESS * 5
+
+  def handle_button_event(self, key, sm, frogpilot_toggles):
+    if sm["carControl"].longActive and getattr(frogpilot_toggles, f"experimental_mode_via_{key}"):
+      self.handle_experimental_mode(sm, frogpilot_toggles)
+
+  def handle_experimental_mode(self, sm, frogpilot_toggles):
+    if frogpilot_toggles.conditional_experimental_mode:
+      if self.params_memory.get("CEStatus") in (CEStatus["USER_DISABLED"], CEStatus["USER_OVERRIDDEN"]):
+        override_value = CEStatus["OFF"]
+      elif sm["selfdriveState"].experimentalMode:
+        override_value = CEStatus["USER_DISABLED"]
+      else:
+        override_value = CEStatus["USER_OVERRIDDEN"]
+
+      self.params_memory.put("CEStatus", override_value)
+    else:
+      self.params.put_bool_nonblocking("ExperimentalMode", not sm["selfdriveState"].experimentalMode)
+
   def update(self, carState, frogpilotCarState, sm, frogpilot_toggles):
     if self.CP.brand == "hyundai":
       for be in carState.buttonEvents:
@@ -44,8 +68,28 @@ class FrogPilotCard:
     if sm.updated["frogpilotPlan"] or any(be.type == ButtonType.decelCruise for be in carState.buttonEvents):
       self.decel_pressed = any(be.type == ButtonType.decelCruise for be in carState.buttonEvents)
 
+    if frogpilotCarState.distancePressed:
+      self.gap_counter += 1
+    elif not self.distancePressed_previously:
+      self.gap_counter = 0
+
+    self.distancePressed_previously = frogpilotCarState.distancePressed
+
+    if not frogpilotCarState.distancePressed and 1 <= self.gap_counter < self.long_press_threshold:
+      self.handle_button_event("distance", sm, frogpilot_toggles)
+    elif self.gap_counter == self.long_press_threshold:
+      self.handle_button_event("distance_long", sm, frogpilot_toggles)
+    elif self.gap_counter == self.very_long_press_threshold:
+      self.handle_button_event("distance_long", sm, frogpilot_toggles)
+      self.handle_button_event("distance_very_long", sm, frogpilot_toggles)
+
+    if any(be.pressed and be.type == ButtonType.lkas for be in carState.buttonEvents):
+      self.handle_button_event("lkas", sm, frogpilot_toggles)
+
     frogpilotCarState.accelPressed = self.accel_pressed
     frogpilotCarState.alwaysOnLateralEnabled = self.always_on_lateral_enabled
     frogpilotCarState.decelPressed = self.decel_pressed
+    frogpilotCarState.distanceLongPressed = self.very_long_press_threshold > self.gap_counter >= self.long_press_threshold
+    frogpilotCarState.distanceVeryLongPressed = self.gap_counter >= self.very_long_press_threshold
 
     return frogpilotCarState

opendbc_repo/opendbc/car/chrysler/carstate.py

@@ -29,6 +29,7 @@ class CarState(CarStateBase):
     self.button_message = "CRUISE_BUTTONS_ALT" if FPCP.flags & ChryslerFrogPilotFlags.RAM_HD_ALT_BUTTONS else "CRUISE_BUTTONS"
 
     # FrogPilot variables
+    self.lkas_button = 0
 
   def update(self, can_parsers, frogpilot_toggles) -> structs.CarState:
     cp = can_parsers[Bus.pt]
@@ -106,6 +107,16 @@ class CarState(CarStateBase):
     # FrogPilot variables
     fp_ret = custom.FrogPilotCarState.new_message()
 
+    self.prev_lkas_button = self.lkas_button
+    if self.CP.carFingerprint in RAM_CARS:
+      self.lkas_button = cp.vl["Center_Stack_1"]["LKAS_Button"] or cp.vl["Center_Stack_2"]["LKAS_Button"]
+    else:
+      self.lkas_button = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] == 1
+
+    buttonEvents += [
+      *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}),
+    ]
+
     ret.buttonEvents = buttonEvents
 
     return ret, fp_ret

opendbc_repo/opendbc/car/interfaces.py

@@ -10,7 +10,7 @@ from functools import cache
 from types import SimpleNamespace
 
 from cereal import custom
-from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
+from opendbc.car import DT_CTRL, apply_hysteresis, create_button_events, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
 from opendbc.car import structs
 from opendbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
 from opendbc.car.chrysler.values import CAR as CHRYSLER, ChryslerFrogPilotFlags
@@ -126,6 +126,8 @@ class CarInterfaceBase(ABC):
 
     self.params_memory = Params(memory=True)
 
+    self.distance_button = 0
+
   def apply(self, c: structs.CarControl, now_nanos: int | None = None, frogpilot_toggles: SimpleNamespace = None) -> tuple[structs.CarControl.Actuators, list[CanData]]:
     if now_nanos is None:
       now_nanos = int(time.monotonic() * 1e9)
@@ -317,6 +319,9 @@ class CarInterfaceBase(ABC):
     self.CS.out = ret
 
     # FrogPilot variables
+    self.distance_button = bool(self.CS.distance_button)
+
+    fp_ret.distancePressed = self.distance_button
     fp_ret.ecoGear |= ret.gearShifter == GearShifter.eco
     fp_ret.sportGear |= ret.gearShifter == GearShifter.sport
 
@@ -353,6 +358,8 @@ class CarStateBase(ABC):
 
     self.CC: structs.CarControl = structs.CarControl.new_message()
 
+    self.distance_button = False
+
   @abstractmethod
   def update(self, can_parsers, frogpilot_toggles) -> structs.CarState:
     pass

opendbc_repo/opendbc/car/nissan/carstate.py

@@ -26,6 +26,7 @@ class CarState(CarStateBase):
     self.distance_button = 0
 
     # FrogPilot variables
+    self.lkas_button = 0
 
   def update(self, can_parsers, frogpilot_toggles) -> structs.CarState:
     cp = can_parsers[Bus.pt]
@@ -134,6 +135,13 @@ class CarState(CarStateBase):
     # FrogPilot variables
     fp_ret = custom.FrogPilotCarState.new_message()
 
+    self.prev_lkas_button = self.lkas_button
+    self.lkas_button = ret.invalidLkasSetting
+
+    buttonEvents += [
+      *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas, 0: ButtonType.lkas}),
+    ]
+
     ret.buttonEvents = buttonEvents
 
     return ret, fp_ret

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -204,6 +204,11 @@ class CarState(CarStateBase):
     # FrogPilot variables
     fp_ret = custom.FrogPilotCarState.new_message()
 
+    buttonEvents += [
+      *create_button_events(self.pcm_acc_status == 9, False, {1: ButtonType.accelCruise}),
+      *create_button_events(self.pcm_acc_status == 10, False, {1: ButtonType.decelCruise}),
+    ]
+
     if not self.CP.flags & ToyotaFlags.SECOC.value:
       fp_ret.ecoGear = cp.vl["GEAR_PACKET"]["ECON_ON"] == 1
       fp_ret.sportGear = cp.vl["GEAR_PACKET"]["SPORT_ON_2" if self.CP.flags & ToyotaFlags.NO_DSU else "SPORT_ON"] == 1

selfdrive/selfdrived/selfdrived.py

@@ -155,6 +155,8 @@ class SelfdriveD:
     self.frogpilot_AM = AlertManager()
     self.frogpilot_events = Events(frogpilot=True)
 
+    self.distance_pressed_previously = False
+
     self.frogpilot_events_prev = []
 
     self.FPCP = messaging.log_from_bytes(self.params.get("FrogPilotCarParams", block=True), custom.FrogPilotCarParams)
@@ -426,11 +428,25 @@ class SelfdriveD:
 
     # 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):
+      distance_pressed = False
+
+      if self.frogpilot_toggles.personality_profile_via_distance:
+        distance_pressed |= any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents)
+        distance_pressed &= not (self.sm['frogpilotCarState'].distanceLongPressed or self.sm['frogpilotCarState'].distanceVeryLongPressed)
+      if self.frogpilot_toggles.personality_profile_via_distance_long:
+        distance_pressed |= self.sm['frogpilotCarState'].distanceLongPressed
+      if self.frogpilot_toggles.personality_profile_via_distance_very_long:
+        distance_pressed |= self.sm['frogpilotCarState'].distanceVeryLongPressed
+      if self.frogpilot_toggles.personality_profile_via_lkas:
+        distance_pressed |= any(not be.pressed and be.type == ButtonType.lkas for be in CS.buttonEvents)
+
+      if not distance_pressed and self.distance_pressed_previously:
         self.personality = (self.personality - 1) % 3
         self.params.put_nonblocking('LongitudinalPersonality', self.personality)
         self.events.add(EventName.personalityChanged)
 
+      self.distance_pressed_previously = distance_pressed
+
     # FrogPilot variables
     self.frogpilot_events.add_from_msg(self.sm['frogpilotPlan'].frogpilotEvents)