From d848011743954b23a09c364ec47fda9b6fba28d8 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 10 Jun 2024 06:47:01 -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 | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 60d76c8a5..b8c234a9d 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
 
@@ -926,6 +927,15 @@ class Controls:
     if self.frogpilot_toggles.conditional_experimental_mode:
       self.experimental_mode = self.sm['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.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode)
+
     FPCC = custom.FrogPilotCarControl.new_message()
     FPCC.alwaysOnLateral = self.always_on_lateral_active