Experimental Mode activation via steering wheel / onroad UI

Added toggle to enable or disable Experimental Mode from the steering wheel or onroad UI.

selfdrive/car/gm/carstate.py

@@ -152,6 +152,20 @@ class CarState(CarStateBase):
         ret.leftBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
         ret.rightBlindspot = cam_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
 
+    # Toggle Experimental Mode from steering wheel function
+    if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available:
+      if self.CP.carFingerprint in SDGM_CAR:
+        lkas_pressed = cam_cp.vl["ASCMSteeringButton"]["LKAButton"]
+      else:
+        lkas_pressed = pt_cp.vl["ASCMSteeringButton"]["LKAButton"]
+
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if frogpilot_variables.conditional_experimental_mode:
+          self.fpf.update_cestatus_lkas()
+        else:
+          self.fpf.update_experimental_mode()
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   @staticmethod

selfdrive/car/honda/carstate.py

@@ -263,6 +263,16 @@ class CarState(CarStateBase):
       ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
       ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
 
+    # Toggle Experimental Mode from steering wheel function
+    if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available:
+      lkas_pressed = self.cruise_setting == 1
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if frogpilot_variables.conditional_experimental_mode:
+          self.fpf.update_cestatus_lkas()
+        else:
+          self.fpf.update_experimental_mode()
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def get_can_parser(self, CP):

selfdrive/car/hyundai/carstate.py

@@ -170,6 +170,16 @@ class CarState(CarStateBase):
     if self.prev_main_buttons == 0 and self.main_buttons[-1] != 0:
       self.main_enabled = not self.main_enabled
 
+    # Toggle Experimental Mode from steering wheel function
+    if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.flags & HyundaiFlags.CAN_LFA_BTN:
+      lkas_pressed = cp.vl["BCM_PO_11"]["LFA_Pressed"]
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if frogpilot_variables.conditional_experimental_mode:
+          self.fpf.update_cestatus_lkas()
+        else:
+          self.fpf.update_experimental_mode()
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def update_canfd(self, cp, cp_cam, frogpilot_variables):
@@ -255,6 +265,13 @@ class CarState(CarStateBase):
       self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING
                                           else cp_cam.vl["CAM_0x2a4"])
 
+    # Toggle Experimental Mode from steering wheel function
+    if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available:
+      lkas_pressed = cp.vl[self.cruise_btns_msg_canfd]["LKAS_BTN"]
+      if lkas_pressed and not self.lkas_previously_pressed:
+        self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode)
+      self.lkas_previously_pressed = lkas_pressed
+
     return ret
 
   def get_can_parser(self, CP):
@@ -304,6 +321,9 @@ class CarState(CarStateBase):
     else:
       messages.append(("LVR12", 100))
 
+    if CP.flags & HyundaiFlags.CAN_LFA_BTN:
+      messages.append(("BCM_PO_11", 50))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
 
   @staticmethod

selfdrive/car/hyundai/interface.py

@@ -316,6 +316,10 @@ class CarInterface(CarInterfaceBase):
       if candidate in CAMERA_SCC_CAR:
         ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC
 
+      if 0x391 in fingerprint[0]:
+        ret.flags |= HyundaiFlags.CAN_LFA_BTN.value
+        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LFA_BTN
+
     if ret.openpilotLongitudinalControl:
       ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_HYUNDAI_LONG
     if ret.flags & HyundaiFlags.HYBRID:

selfdrive/car/hyundai/values.py

@@ -67,6 +67,7 @@ class HyundaiFlags(IntFlag):
   CANFD_HDA2_ALT_STEERING = 512
   HYBRID = 1024
   EV = 2048
+  CAN_LFA_BTN = 4096
 
 
 class CAR(StrEnum):

selfdrive/car/interfaces.py

@@ -373,6 +373,12 @@ class CarStateBase(ABC):
     # FrogPilot variables
     self.fpf = FrogPilotFunctions()
 
+    self.distance_button = False
+    self.distance_previously_pressed = False
+    self.lkas_previously_pressed = False
+
+    self.distance_pressed_counter = 0
+
   def update_speed_kf(self, v_ego_raw):
     if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0:  # Prevent large accelerations when car starts at non zero speed
       self.v_ego_kf.set_x([[v_ego_raw], [0.0]])

selfdrive/car/toyota/carstate.py

@@ -10,6 +10,7 @@ from opendbc.can.parser import CANParser
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS
 
 SteerControlType = car.CarParams.SteerControlType
 
@@ -166,6 +167,34 @@ class CarState(CarStateBase):
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
 
+    # Experimental Mode via double clicking the LKAS button function
+    if frogpilot_variables.experimental_mode_via_lkas and ret.cruiseState.available and self.CP.carFingerprint != CAR.PRIUS_V:
+      message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
+      lkas_pressed = any(self.lkas_hud.get(key) == 1 for key in message_keys)
+
+      if lkas_pressed and not self.lkas_previously_pressed:
+        if frogpilot_variables.conditional_experimental_mode:
+          self.fpf.update_cestatus_distance()
+        else:
+          self.fpf.update_experimental_mode()
+
+      self.lkas_previously_pressed = lkas_pressed
+
+    # Experimental Mode via long pressing the distance button function
+    if ret.cruiseState.available:
+      if distance_pressed:
+        self.distance_pressed_counter += 1
+      elif self.distance_previously_pressed:
+        self.distance_pressed_counter = 0
+
+      if self.distance_pressed_counter == CRUISE_LONG_PRESS and frogpilot_variables.experimental_mode_via_distance:
+        if frogpilot_variables.conditional_experimental_mode:
+          self.fpf.update_cestatus_lkas()
+        else:
+          self.fpf.update_experimental_mode()
+
+      self.distance_previously_pressed = distance_pressed
+
     return ret
 
   @staticmethod