controlsd: re-wire UI hooks on top of restored baseline

Adds back the UI plumbing that the baseline controlsd doesn't have, so
the existing UI features keep working without changing driving logic:

- ScreenDisplayMode 5-state machine (auto-normal, auto-nightrider,
  manual normal, screen-off, manual nightrider) driven by the LFA
  debug button + gear edges. Replaces baseline's simple 0..2 cycle.
  Pure params write — no actuator effect.
- Speed/cruise-warning overlay tick at ~2Hz feeding SpeedState, which
  writes ClearpilotSpeedDisplay / ClearpilotSpeedLimitDisplay /
  ClearpilotCruiseWarning for the onroad UI. Reads gpsLocation,
  CarSpeedLimit, and CS.cruiseState — no actuator effect.
- frogpilot_variables.no_lat_lane_change is now populated alongside
  the existing Params write, so the perf-optimized carcontroller (which
  takes the bit as an argument instead of re-reading Params on the
  100Hz hot path) still sees the lane-change suppression signal.
- ScreenDisplayMode init switched from put_bool to put_int (UI reads
  it as int).
- gpsLocation added to SubMaster (ignore_alive/avg_freq/valid set,
  since gpsd is a ClearPilot addition not present everywhere).

No changes to controlsd's lateral or longitudinal control paths.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

selfdrive/controls/controlsd.py

@@ -39,6 +39,9 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUIS
 from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
+# CLEARPILOT: UI plumbing for ScreenDisplayMode and the speed/cruise-warning overlay
+from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
+
 SOFT_DISABLE_TIME = 3  # seconds
 LDW_MIN_SPEED = 31 * CV.MPH_TO_MS
 LANE_DEPARTURE_THRESHOLD = 0.1
@@ -77,7 +80,14 @@ class Controls:
     self.params_storage = Params("/persist/params")
 
     self.params_memory.put_bool("CPTLkasButtonAction", False)
-    self.params_memory.put_bool("ScreenDisplayMode", 0)
+    # CLEARPILOT: ScreenDisplayMode is an int (5-state machine: 0..4); UI reads it via getInt
+    self.params_memory.put_int("ScreenDisplayMode", 0)
+
+    # CLEARPILOT: speed/cruise-warning overlay state, ticked at ~2Hz from clearpilot_state_control
+    self.speed_state = SpeedState()
+    self.speed_state_frame = 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
 
@@ -107,8 +117,8 @@ class Controls:
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
-                                   'testJoystick', 'frogpilotPlan'] + self.camera_packets + self.sensor_packets,
+                                   'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
-                                  ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
+                                  ignore_alive=ignore + ['gpsLocation'], ignore_avg_freq=ignore+['radarState', 'testJoystick', 'gpsLocation'], ignore_valid=['testJoystick', 'gpsLocation'],
                                   frequency=int(1/DT_CTRL))
 
     self.joystick_mode = self.params.get_bool("JoystickDebugMode")
@@ -677,8 +687,11 @@ class Controls:
     if model_v2.meta.laneChangeState == LaneChangeState.laneChangeStarting and clearpilot_disable_lat_on_lane_change:
       CC.latActive = False
       self.params_memory.put_bool("no_lat_lane_change", True)
+      # CLEARPILOT: hyundai carcontroller reads this off frogpilot_variables (not Params) — keep both in sync
+      self.frogpilot_variables.no_lat_lane_change = True
     else:
       self.params_memory.put_bool("no_lat_lane_change", False)
+      self.frogpilot_variables.no_lat_lane_change = False
 
     if CS.leftBlinker or CS.rightBlinker:
       self.last_blinker_frame = self.sm.frame
@@ -1241,24 +1254,44 @@ class Controls:
       # 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):
+    # CLEARPILOT: pure UI plumbing — does not modify CC/actuators. Maintains
-      
+    # ScreenDisplayMode (5-state machine driven by the LFA/debug button + gear
-      # Uncomment for debug testing.
+    # edges) and ticks the speed/cruise-warning overlay at ~2Hz.
-      # if self.params_memory.get_bool("CPTLkasButtonAction"):
+    driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park,
-      #   self.params_memory.put_bool("CPTLkasButtonAction", False)
+                                          GearShifter.reverse, GearShifter.unknown)
-      # 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)
 
-      # Leftovers
+    # Auto-wake screen when shifting into drive from screen-off
-      # self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
+    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)
+
+    # Speed/cruise-warning overlay tick (~2Hz at 100Hz loop)
+    self.speed_state_frame += 1
+    if self.speed_state_frame % 50 == 0:
+      gps = self.sm['gpsLocation']
+      has_gps = self.sm.valid['gpsLocation'] and gps.hasFix
+      speed_ms = gps.speed if has_gps else 0.0
+      speed_limit_ms = self.params_memory.get_float("CarSpeedLimit") or 0.0
+      is_metric = self.is_metric
+      cruise_speed_ms = CS.cruiseState.speed
+      cruise_active = CS.cruiseState.enabled
+      cruise_standstill = CS.cruiseState.standstill
+      self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric,
+                              cruise_speed_ms, cruise_active, cruise_standstill)
     return CC
 
 def main():