From f7e602c00b059406203cd5c0d499cbc7465ed6af Mon Sep 17 00:00:00 2001 From: brian Date: Sun, 26 Apr 2026 08:44:52 -0500 Subject: [PATCH] controlsd: re-wire UI hooks on top of restored baseline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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) --- selfdrive/controls/controlsd.py | 73 ++++++++++++++++++++++++--------- 1 file changed, 53 insertions(+), 20 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e050505..3c43a02 100755 --- a/selfdrive/controls/controlsd.py +++ b/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, - 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") @@ -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): - - # 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) and ticks the speed/cruise-warning overlay at ~2Hz. + driving_gear = CS.gearShifter not in (GearShifter.neutral, GearShifter.park, + GearShifter.reverse, GearShifter.unknown) - # Leftovers - # self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH ) + # 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) + + # 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():