From 8ccdb47c88c3555bafcd254e26aac53a1da11429 Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Wed, 15 Apr 2026 03:15:07 +0000 Subject: [PATCH] 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) --- common/params.cc | 9 ++- selfdrive/car/hyundai/carstate.py | 2 + selfdrive/clearpilot/bench_onroad.py | 17 ++++- selfdrive/clearpilot/speed_logic.py | 52 +++++++++++++ selfdrive/controls/controlsd.py | 21 +++++- selfdrive/manager/manager.py | 6 ++ selfdrive/ui/qt/onroad.cc | 108 +++++++++++++++++++++++---- selfdrive/ui/qt/onroad.h | 9 +++ 8 files changed, 203 insertions(+), 21 deletions(-) create mode 100644 selfdrive/clearpilot/speed_logic.py diff --git a/common/params.cc b/common/params.cc index c6cc978..3759de8 100755 --- a/common/params.cc +++ b/common/params.cc @@ -250,7 +250,14 @@ std::unordered_map keys = { {"CarSpeedLimitWarning", PERSISTENT}, {"CarSpeedLimitLiteral", PERSISTENT}, - + {"CarIsMetric", PERSISTENT}, + + {"ClearpilotSpeedDisplay", PERSISTENT}, + {"ClearpilotSpeedLimitDisplay", PERSISTENT}, + {"ClearpilotHasSpeed", PERSISTENT}, + {"ClearpilotIsMetric", PERSISTENT}, + {"ClearpilotSpeedUnit", PERSISTENT}, + // {"SpeedLimitLatDesired", PERSISTENT}, // {"SpeedLimitVTSC", PERSISTENT}, diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 7483ecb..bad8159 100755 --- 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 diff --git a/selfdrive/clearpilot/bench_onroad.py b/selfdrive/clearpilot/bench_onroad.py index e8d1245..34883b3 100644 --- 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 diff --git a/selfdrive/clearpilot/speed_logic.py b/selfdrive/clearpilot/speed_logic.py new file mode 100644 index 0000000..f9e65b0 --- /dev/null +++ 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") diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b71e90d..a3e5d99 100755 --- 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(): diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 12574f2..4bc4ca3 100755 --- 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(): diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 5f1e885..55dce76 100755 --- 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(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}); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 8c9f1a0..0cdec82 100755 --- 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;