From b899f7b859f7ceaf2994d1ec6a6ff48c8eae22ef Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sun, 21 Jul 2024 18:58:34 -0700
Subject: [PATCH] Controls - Experimental Mode Activation - Double Click LKAS

Enable/disable 'Experimental Mode' by double clicking the 'LKAS' button on your steering wheel.
---
 selfdrive/controls/controlsd.py | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index b17be8ccf..70983a84d 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -52,6 +52,7 @@ LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
 EventName = car.CarEvent.EventName
 ButtonType = car.CarState.ButtonEvent.Type
+FrogPilotButtonType = custom.FrogPilotCarState.ButtonEvent.Type
 GearShifter = car.CarState.GearShifter
 SafetyModel = car.CarParams.SafetyModel
 
@@ -924,6 +925,16 @@ class Controls:
     if self.frogpilot_toggles.conditional_experimental_mode:
       self.experimental_mode = frogpilotPlan.conditionalExperimentalActive
 
+    if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
+      if self.frogpilot_toggles.experimental_mode_via_lkas and self.enabled:
+        if self.frogpilot_toggles.conditional_experimental_mode:
+          conditional_status = self.params_memory.get_int("CEStatus")
+          override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 3 if conditional_status >= 7 else 4
+          self.params_memory.put_int("CEStatus", override_value)
+        else:
+          self.experimental_mode = not self.experimental_mode
+          self.params.put_bool_nonblocking("ExperimentalMode", self.experimental_mode)
+
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateral = self.always_on_lateral_active