controlsd: re-apply QoL hooks on top of reverted baseline

Re-adds the non-self-drive controlsd integrations that the UI and
memory-param pipeline need:

- import SpeedState from clearpilot.speed_logic
- self.speed_state, speed_state_frame, was_driving_gear init
- subscribe to gpsLocation (ignored in alive/freq_ok/valid gates — OK if
  the GPS daemon isn't publishing)
- clearpilot_state_control:
  - auto-reset ScreenDisplayMode 3→0 on park→drive transition
  - ~2Hz speed-state update driving the speed-limit sign and cruise
    over/under warning sign via ClearpilotCruiseWarning /
    ClearpilotSpeedLimitDisplay memory params

The debug-button (LFA) ScreenDisplayMode cycling already lived in the
reverted baseline (it was in the first commit), so it's preserved.

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

selfdrive/controls/controlsd.py

@@ -38,6 +38,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import CRUIS
 
 from openpilot.selfdrive.frogpilot.controls.lib.model_manager import RADARLESS_MODELS
 from openpilot.selfdrive.clearpilot.telemetry import tlog
+from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
 from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 SOFT_DISABLE_TIME = 3  # seconds
@@ -80,6 +81,11 @@ class Controls:
     self.params_memory.put_bool("CPTLkasButtonAction", False)
     self.params_memory.put_int("ScreenDisplayMode", 0)
 
+    # CLEARPILOT: speed-limit/cruise-warning state machine + park→drive auto-reset
+    self.speed_state = SpeedState()
+    self.speed_state_frame = 0
+    self.was_driving_gear = False
+
     self.radarless_model = self.params.get("Model", encoding='utf-8') in RADARLESS_MODELS
 
     with car.CarParams.from_bytes(self.params.get("CarParams", block=True)) as msg:
@@ -108,8 +114,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,
-                                  ignore_alive=ignore, ignore_avg_freq=ignore+['radarState', 'testJoystick'], ignore_valid=['testJoystick', ],
+                                   'testJoystick', 'frogpilotPlan', 'gpsLocation'] + self.camera_packets + self.sensor_packets,
+                                  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")
@@ -1257,6 +1263,12 @@ class Controls:
       self.events.add(EventName.clpDebug)
 
   def clearpilot_state_control(self, CC, CS):
+    # CLEARPILOT: auto-reset display when shifting into drive from screen-off
+    if self.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 = self.driving_gear
+
     if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
       current = self.params_memory.get_int("ScreenDisplayMode")
 
@@ -1269,6 +1281,21 @@ class Controls:
         new_mode = 0 if current == 3 else 3
 
       self.params_memory.put_int("ScreenDisplayMode", new_mode)
+
+    # ClearPilot speed processing (~2 Hz at 100 Hz loop) — drives speed-limit sign
+    # and cruise over/under warning sign via memory params read by the UI.
+    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")
+      is_metric = (self.params_memory.get("CarIsMetric", encoding="utf-8") or "0") == "1"
+      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():