From 3f5172b58bbd5dcfb66ef82261a42535b7225d41 Mon Sep 17 00:00:00 2001
From: Brian Hanson <brian@hanson.xyz>
Date: Sun, 3 May 2026 23:06:57 -0500
Subject: [PATCH] controlsd: 5-state ScreenDisplayMode machine + park->drive
 auto-wake
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Replaces the 3-state cycle in clearpilot_state_control with broken's
5-state machine:
- Onroad (drive gear): 0→4, 1→2, 2→3, 3→4, 4→2 (auto modes never
  reached via the button — gpsd's day/night calc is the only path back)
- Not in drive: anything except 3 → 3 (screen off), state 3 → 0 (auto)

Also tracks park→drive edge to auto-wake from screen-off (mode 3 → 0)
when shifting into drive, regardless of how the screen got turned off.

Speed/cruise overlay state intentionally left out — owned by
speed_logicd, not controlsd.

events.py: remove clp_debug_notice + EventName.clpDebug binding
(no more "Clearpilot Debug Function Executed (N)" on-screen message).
The dead commented-out events.add(EventName.clpDebug) and stray print
of buttonEvents in controlsd are also cleaned up.
---
 selfdrive/controls/controlsd.py  | 49 ++++++++++++++++++--------------
 selfdrive/controls/lib/events.py | 14 ---------
 2 files changed, 27 insertions(+), 36 deletions(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 63124e0..b7b8d2e 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -78,6 +78,8 @@ class Controls:
 
     self.params_memory.put_bool("CPTLkasButtonAction", False)
     self.params_memory.put_int("ScreenDisplayMode", 0)
+    # CLEARPILOT: edge tracking for park→drive auto-wake of screen
+    self.was_driving_gear = False
 
     self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
 
@@ -1233,32 +1235,35 @@ class Controls:
     self.frogpilot_variables.use_ev_tables = self.params.get_bool("EVTable")
 
   def update_clearpilot_events(self, CS):
-    if (len(CS.buttonEvents) > 0): 
+    if (len(CS.buttonEvents) > 0):
       print (CS.buttonEvents)
 
-    # Uncomment to alert when lkas button pressed
-    # if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
-      # self.events.add(EventName.clpDebug)
-
   def clearpilot_state_control(self, CC, CS):
-    if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
-      
-      # Uncomment for debug testing.
-      # if self.params_memory.get_bool("CPTLkasButtonAction"):
-      #   self.params_memory.put_bool("CPTLkasButtonAction", False)
-      # else:
-      #   self.params_memory.put_bool("CPTLkasButtonAction", True)
-  
-      # Rotate display mode.  These are mostly used in the frontend ui app.
-      max_display_mode = 2
-      current_display_mode = self.params_memory.get_int("ScreenDisplayMode")
-      current_display_mode = current_display_mode + 1
-      if current_display_mode > max_display_mode:
-        current_display_mode = 0
-      self.params_memory.put_int("ScreenDisplayMode", current_display_mode)
+    # CLEARPILOT: pure UI plumbing — does not modify CC/actuators. Maintains
+    # ScreenDisplayMode (5-state machine driven by the LFA/debug button + gear
+    # edges). Speed/cruise overlay state is owned by speed_logicd, not here.
+    driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park,
+                                          GearShifter.reverse, GearShifter.unknown)
+
+    # Auto-wake screen when shifting into drive from screen-off
+    if driving_gear and not self.was_driving_gear:
+      if self.params_memory.get_int("ScreenDisplayMode") == 3:
+        self.params_memory.put_int("ScreenDisplayMode", 0)
+    self.was_driving_gear = driving_gear
+
+    # LFA/debug button cycles ScreenDisplayMode. Onroad and offroad use
+    # different transition tables.
+    if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
+      current = self.params_memory.get_int("ScreenDisplayMode")
+      if driving_gear:
+        # Onroad: 0→4, 1→2, 2→3, 3→4, 4→2 (never back to auto via button)
+        transitions = {0: 4, 1: 2, 2: 3, 3: 4, 4: 2}
+        new_mode = transitions.get(current, 0)
+      else:
+        # Not in drive: anything except 3 → 3 (screen off), state 3 → 0 (auto)
+        new_mode = 0 if current == 3 else 3
+      self.params_memory.put_int("ScreenDisplayMode", new_mode)
 
-      # Leftovers
-      # self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
     return CC
 
 def main():
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index 6d23406..2aa5ef3 100755
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -262,16 +262,6 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag
     AlertStatus.normal, AlertSize.mid,
     Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)
 
-def clp_debug_notice(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
-  if not hasattr(clp_debug_notice, "counter"):
-    clp_debug_notice.counter = 0  
-  clp_debug_notice.counter += 1 
-  return Alert(
-    f"Clearpilot Debug Function Executed ({clp_debug_notice.counter})",
-    "",
-    AlertStatus.normal, AlertSize.small,
-    Priority.LOWER, VisualAlert.none, AudibleAlert.none, 3.0)
-
 
 def no_gps_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
   # Clearpilot todo:
@@ -778,10 +768,6 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.SOFT_DISABLE: soft_disable_alert("Sensor Data Invalid"),
   },
 
-  EventName.clpDebug: {
-    ET.PERMANENT: clp_debug_notice,
-  },
-
   EventName.noGps: {
     ET.PERMANENT: no_gps_alert,
   },