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:
@@ -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
|
||||
|
||||
52
selfdrive/clearpilot/speed_logic.py
Normal file
52
selfdrive/clearpilot/speed_logic.py
Normal 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")
|
||||
Reference in New Issue
Block a user