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:
@@ -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():
|
||||
|
||||
Reference in New Issue
Block a user