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>
This commit is contained in:
@@ -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):
|
||||
# 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)
|
||||
|
||||
# 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)
|
||||
|
||||
# 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)
|
||||
|
||||
# Leftovers
|
||||
# self.params_memory.put_int("SpeedLimitLatDesired", CC.actuators.speed * CV.MS_TO_MPH )
|
||||
# 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():
|
||||
|
||||
Reference in New Issue
Block a user