feat: speed limit sign UI and speed processing pipeline

New speed_logic.py module converts raw CAN speed limit and GPS speed
into display-ready params. Called from controlsd (live) and
bench_onroad (bench) at ~2Hz. UI reads params to render:
- Current speed (top center, hidden when 0 or no GPS)
- MUTCD speed limit sign (lower-left, normal + nightrider styles)
- Unit-aware display (mph/kph based on CAN DISTANCE_UNIT)

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
2026-04-15 03:15:07 +00:00
parent b84c268b6e
commit 8ccdb47c88
8 changed files with 203 additions and 21 deletions

View File

@@ -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,10 @@ class Controls:
self.params_memory.put_bool("CPTLkasButtonAction", False)
self.params_memory.put_int("ScreenDisplayMode", 0)
# ClearPilot speed processing
self.speed_state = SpeedState()
self.speed_state_frame = 0
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 +113,9 @@ 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")
@@ -1269,6 +1275,17 @@ 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)
self.speed_state_frame += 1
if self.speed_state_frame % 50 == 0:
gps = self.sm['gpsLocation'].getGpsLocation()
has_gps = self.sm.valid('gpsLocation') and gps.getHasFix()
speed_ms = gps.getSpeed() 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"
self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric)
return CC
def main():