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>
2026-04-15 03:15:07 +00:00
parent b84c268b6e
commit 8ccdb47c88
8 changed files with 203 additions and 21 deletions
--- a/common/params.cc
+++ b/common/params.cc
@@ -250,7 +250,14 @@ std::unordered_map<std::string, uint32_t> keys = {
    
    {"CarSpeedLimitWarning", PERSISTENT},
    {"CarSpeedLimitLiteral", PERSISTENT},
-    
+    {"CarIsMetric", PERSISTENT},
+
+    {"ClearpilotSpeedDisplay", PERSISTENT},
+    {"ClearpilotSpeedLimitDisplay", PERSISTENT},
+    {"ClearpilotHasSpeed", PERSISTENT},
+    {"ClearpilotIsMetric", PERSISTENT},
+    {"ClearpilotSpeedUnit", PERSISTENT},
+
    // {"SpeedLimitLatDesired", PERSISTENT},
    // {"SpeedLimitVTSC", PERSISTENT},

--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -212,6 +212,7 @@ class CarState(CarStateBase):

    # self.params_memory.put_int("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_conv)
+    self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")
    self.params_memory.put_float("CarCruiseDisplayActual", cp_cruise.vl["SCC11"]["VSetDis"])
    

@@ -420,6 +421,7 @@ class CarState(CarStateBase):
    # print(self.calculate_speed_limit(cp, cp_cam))
    # self.params_memory.put_float("CarSpeedLimitLiteral", self.calculate_speed_limit(cp, cp_cam))
    self.params_memory.put_float("CarSpeedLimit", self.calculate_speed_limit(cp, cp_cam) * speed_factor)
+    self.params_memory.put("CarIsMetric", "1" if self.is_metric else "0")

    # CLEARPILOT: telemetry logging — disabled, re-enable when needed
    # speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam
--- a/selfdrive/clearpilot/bench_onroad.py
+++ b/selfdrive/clearpilot/bench_onroad.py
@@ -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
--- a/selfdrive/clearpilot/speed_logic.py
+++ b/selfdrive/clearpilot/speed_logic.py
@@ -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")
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -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():
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -89,6 +89,12 @@ def manager_init(frogpilot_functions) -> None:
  params_memory.put("VpnEnabled", "1")
  params_memory.put("ModelStandby", "0")
  params_memory.put("ModelStandbyTs", "0")
+  params_memory.put("CarIsMetric", "0")
+  params_memory.put("ClearpilotSpeedDisplay", "")
+  params_memory.put("ClearpilotSpeedLimitDisplay", "0")
+  params_memory.put("ClearpilotHasSpeed", "0")
+  params_memory.put("ClearpilotIsMetric", "0")
+  params_memory.put("ClearpilotSpeedUnit", "mph")
  params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION)
  params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION)
  if is_release_branch():
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -355,14 +355,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
    setSpeed *= KM_TO_MILE;
  }

-  // CLEARPILOT: use GPS speed if available, hide speed display if no GPS fix
-  has_gps_speed = sm.valid("gpsLocation") && sm["gpsLocation"].getGpsLocation().getHasFix();
-  if (has_gps_speed) {
-    float gps_speed_ms = sm["gpsLocation"].getGpsLocation().getSpeed();
-    speed = std::max<float>(0.0, gps_speed_ms);
-    speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH;
-  } else {
-    speed = 0.0;
+  // CLEARPILOT: read speed and speed limit from params (written by speed_logic.py ~2Hz)
+  clpParamFrame++;
+  if (clpParamFrame % 10 == 0) {  // ~2Hz at 20Hz UI update rate
+    clpHasSpeed = paramsMemory.get("ClearpilotHasSpeed") == "1";
+    clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay"));
+    clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay"));
+    clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit"));
  }

  // auto speed_limit_sign = nav_instruction.getSpeedLimitSign();
@@ -431,7 +430,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {

  // QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–";
  // QString speedLimitOffsetStr = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : "");
-  QString speedStr = QString::number(std::nearbyint(speed));
+  // QString speedStr = QString::number(std::nearbyint(speed));
  QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–";

  p.restore();
@@ -455,14 +454,17 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
  // Todo: lead speed
  // Todo: Experimental speed

-  // CLEARPILOT: show GPS speed, hide when no fix
-  if (has_gps_speed && !scene.hide_speed) {
+  // CLEARPILOT: show speed from speed_logic params, hide when no speed or speed=0
+  if (clpHasSpeed && !clpSpeedDisplay.isEmpty() && !scene.hide_speed) {
    p.setFont(InterFont(140, QFont::Bold));
-    drawText(p, rect().center().x(), 210, speedStr);
+    drawText(p, rect().center().x(), 210, clpSpeedDisplay);
    p.setFont(InterFont(50));
-    drawText(p, rect().center().x(), 290, speedUnit, 200);
+    drawText(p, rect().center().x(), 290, clpSpeedUnit, 200);
  }
-  
+
+  // CLEARPILOT: speed limit sign in lower-left
+  drawSpeedLimitSign(p);
+
  // Draw FrogPilot widgets
  paintFrogPilotWidgets(p);
 }
@@ -556,6 +558,84 @@ void AnnotatedCameraWidget::drawSpeedWidget(QPainter &p, int x, int y, const QSt
 }


+void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) {
+  // Hide when no speed limit or speed limit is 0
+  if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return;
+
+  p.save();
+
+  const int signW = 150;
+  const int signH = 190;
+  const int margin = 20;
+  const int borderW = 5;
+  const int innerBorderW = 3;
+  const int innerMargin = 8;
+  const int cornerR = 12;
+
+  // Position: 20px from lower-left corner
+  QRect signRect(margin, height() - signH - margin, signW, signH);
+
+  if (nightriderMode) {
+    // Nightrider: black background, light gray-blue border and text
+    QColor borderColor(160, 180, 210);
+    QColor textColor(160, 180, 210);
+
+    // Outer border
+    p.setPen(QPen(borderColor, borderW));
+    p.setBrush(QColor(0, 0, 0, 220));
+    p.drawRoundedRect(signRect, cornerR, cornerR);
+
+    // Inner border
+    QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
+    p.setPen(QPen(borderColor, innerBorderW));
+    p.setBrush(Qt::NoBrush);
+    p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
+
+    // "SPEED" text
+    p.setPen(textColor);
+    p.setFont(InterFont(24, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
+
+    // "LIMIT" text
+    p.setFont(InterFont(24, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
+
+    // Speed limit number
+    p.setFont(InterFont(72, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 20, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
+  } else {
+    // Normal: white background, black border and text
+    QColor borderColor(0, 0, 0);
+    QColor textColor(0, 0, 0);
+
+    // Outer border
+    p.setPen(QPen(borderColor, borderW));
+    p.setBrush(QColor(255, 255, 255, 240));
+    p.drawRoundedRect(signRect, cornerR, cornerR);
+
+    // Inner border
+    QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin);
+    p.setPen(QPen(borderColor, innerBorderW));
+    p.setBrush(Qt::NoBrush);
+    p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4);
+
+    // "SPEED" text
+    p.setPen(textColor);
+    p.setFont(InterFont(24, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED");
+
+    // "LIMIT" text
+    p.setFont(InterFont(24, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "LIMIT");
+
+    // Speed limit number
+    p.setFont(InterFont(72, QFont::Bold));
+    p.drawText(innerRect.adjusted(0, 20, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay);
+  }
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) {
  QRect real_rect = p.fontMetrics().boundingRect(text);
  real_rect.moveCenter({x, y - real_rect.height() / 2});
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -51,6 +51,7 @@ public:
 private:
  void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255);
  void drawSpeedWidget(QPainter &p, int x, int y, const QString &title, const QString &speedLimitStr, QColor colorSpeed, int width = 176);
+  void drawSpeedLimitSign(QPainter &p);

  QVBoxLayout *main_layout;
  QPixmap dm_img;
@@ -59,6 +60,14 @@ private:
  bool nightriderMode = false;
  int displayMode = 0;
  QString speedUnit;
+
+  // ClearPilot speed state (from params_memory, updated ~2Hz)
+  bool clpHasSpeed = false;
+  QString clpSpeedDisplay;
+  QString clpSpeedLimitDisplay;
+  QString clpSpeedUnit;
+  int clpParamFrame = 0;
+
  float setSpeed;
  float speedLimit;
  bool is_cruise_set = false;