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

@@ -21,8 +21,8 @@ import time
import cereal.messaging as messaging
from cereal import log, car
from openpilot.common.params import Params
MS_PER_MPH = 0.44704
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.clearpilot.speed_logic import SpeedState
def main():
@@ -36,6 +36,9 @@ def main():
params_mem.put("BenchGear", "P")
params_mem.put("BenchEngaged", "0")
# ClearPilot speed processing
speed_state = SpeedState()
# thermald handles deviceState (reads our fake pandaStates for ignition)
pm = messaging.PubMaster([
"pandaStates", "carState", "controlsState",
@@ -63,9 +66,15 @@ def main():
gear_str = (params_mem.get("BenchGear", encoding="utf-8") or "P").strip().upper()
engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1"
speed_ms = speed_mph * MS_PER_MPH
speed_ms = speed_mph * CV.MPH_TO_MS
speed_limit_ms = speed_limit_mph * CV.MPH_TO_MS
gear = gear_map.get(gear_str, car.CarState.GearShifter.park)
# ClearPilot speed processing (~2 Hz at 10 Hz loop)
if frame % 5 == 0:
has_speed = speed_mph > 0
speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False)
# pandaStates — 10 Hz (thermald reads ignition from this)
if frame % 1 == 0:
dat = messaging.new_message("pandaStates", 1)
@@ -82,7 +91,7 @@ def main():
cs.standstill = speed_ms < 0.1
cs.cruiseState.available = True
cs.cruiseState.enabled = engaged
cs.cruiseState.speed = cruise_mph * MS_PER_MPH if cruise_mph > 0 else 0
cs.cruiseState.speed = cruise_mph * CV.MPH_TO_MS if cruise_mph > 0 else 0
pm.send("carState", dat)
# controlsState — 10 Hz

View File

@@ -0,0 +1,52 @@
"""
ClearPilot speed processing module.
Shared logic for converting raw speed and speed limit data into display-ready
values. Called from controlsd (live mode) and bench_onroad (bench mode).
Reads raw inputs, converts to display units (mph or kph based on car's CAN
unit setting), detects speed limit changes, and writes results to params_memory
for the onroad UI to read.
"""
import math
from openpilot.common.params import Params
from openpilot.common.conversions import Conversions as CV
class SpeedState:
def __init__(self):
self.params_memory = Params("/dev/shm/params")
self.prev_speed_limit = 0
def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool):
"""
Convert raw m/s values to display-ready strings and write to params_memory.
Args:
speed_ms: current vehicle speed in m/s (from GPS or bench)
has_speed: whether we have a valid speed source
speed_limit_ms: current speed limit in m/s (from CAN or bench)
is_metric: True if car's CAN reports metric units (e.g. Canada)
"""
if is_metric:
speed_display = speed_ms * CV.MS_TO_KPH
speed_limit_display = speed_limit_ms * CV.MS_TO_KPH
unit = "km/h"
else:
speed_display = speed_ms * CV.MS_TO_MPH
speed_limit_display = speed_limit_ms * CV.MS_TO_MPH
unit = "mph"
speed_int = int(math.floor(speed_display))
speed_limit_int = int(math.floor(speed_limit_display))
# Detect speed limit changes (groundwork for future chime)
if speed_limit_int != self.prev_speed_limit:
self.prev_speed_limit = speed_limit_int
# Write display-ready values to params_memory
self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0")
self.params_memory.put("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "")
self.params_memory.put("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0")
self.params_memory.put("ClearpilotSpeedUnit", unit)
self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0")