From 6e7117b177b2c3fac47d261ed0c1f0155124cd31 Mon Sep 17 00:00:00 2001 From: Brian Hanson Date: Wed, 15 Apr 2026 03:33:27 +0000 Subject: [PATCH] feat: cruise warning signs and speed limit sign sizing Cruise warning sign appears above speed limit sign when cruise set speed is too far from the speed limit: - Red (over): cruise >= limit + 10 (if limit >= 50) or + 5 (if < 50) - Green (under): cruise <= limit - 5 - Only when cruise active (not paused/disabled) and limit >= 20 - Nightrider mode: colored text/border on black background Speed limit sign enlarged 5%. 20px gap between signs. Bench mode gains cruiseactive command (0=disabled, 1=active, 2=paused). Co-Authored-By: Claude Opus 4.6 (1M context) --- common/params.cc | 3 + selfdrive/clearpilot/bench_cmd.py | 2 + selfdrive/clearpilot/bench_onroad.py | 12 ++- selfdrive/clearpilot/speed_logic.py | 27 +++++- selfdrive/controls/controlsd.py | 6 +- selfdrive/manager/manager.py | 2 + selfdrive/ui/qt/onroad.cc | 121 ++++++++++++++++++++++----- selfdrive/ui/qt/onroad.h | 3 + 8 files changed, 154 insertions(+), 22 deletions(-) diff --git a/common/params.cc b/common/params.cc index 3759de8..e692693 100755 --- a/common/params.cc +++ b/common/params.cc @@ -217,6 +217,7 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationPath", PERSISTENT}, + {"BenchCruiseActive", CLEAR_ON_MANAGER_START}, {"BenchCruiseSpeed", CLEAR_ON_MANAGER_START}, {"ClpUiState", CLEAR_ON_MANAGER_START}, {"BenchEngaged", CLEAR_ON_MANAGER_START}, @@ -257,6 +258,8 @@ std::unordered_map keys = { {"ClearpilotHasSpeed", PERSISTENT}, {"ClearpilotIsMetric", PERSISTENT}, {"ClearpilotSpeedUnit", PERSISTENT}, + {"ClearpilotCruiseWarning", PERSISTENT}, + {"ClearpilotCruiseWarningSpeed", PERSISTENT}, // {"SpeedLimitLatDesired", PERSISTENT}, // {"SpeedLimitVTSC", PERSISTENT}, diff --git a/selfdrive/clearpilot/bench_cmd.py b/selfdrive/clearpilot/bench_cmd.py index 5c63b8f..839ad03 100644 --- a/selfdrive/clearpilot/bench_cmd.py +++ b/selfdrive/clearpilot/bench_cmd.py @@ -7,6 +7,7 @@ Usage: python3 -m selfdrive.clearpilot.bench_cmd speed 20 python3 -m selfdrive.clearpilot.bench_cmd speedlimit 45 python3 -m selfdrive.clearpilot.bench_cmd cruise 55 + python3 -m selfdrive.clearpilot.bench_cmd cruiseactive 0|1|2 (0=disabled, 1=active, 2=paused) python3 -m selfdrive.clearpilot.bench_cmd engaged 1 python3 -m selfdrive.clearpilot.bench_cmd debugbutton (simulate LKAS debug button press) python3 -m selfdrive.clearpilot.bench_cmd dump @@ -89,6 +90,7 @@ def main(): "speed": "BenchSpeed", "speedlimit": "BenchSpeedLimit", "cruise": "BenchCruiseSpeed", + "cruiseactive": "BenchCruiseActive", "engaged": "BenchEngaged", } diff --git a/selfdrive/clearpilot/bench_onroad.py b/selfdrive/clearpilot/bench_onroad.py index 34883b3..1764182 100644 --- a/selfdrive/clearpilot/bench_onroad.py +++ b/selfdrive/clearpilot/bench_onroad.py @@ -8,6 +8,7 @@ configurable vehicle state. Control values via params in /dev/shm/params: BenchSpeed - vehicle speed in mph (default: 0) BenchSpeedLimit - speed limit in mph (default: 0, 0=hidden) BenchCruiseSpeed - cruise set speed in mph (default: 0, 0=not set) + BenchCruiseActive - 0=disabled, 1=active, 2=paused/standstill (default: 0) BenchGear - P, D, R, N (default: P) BenchEngaged - 0 or 1, cruise engaged (default: 0) @@ -33,6 +34,7 @@ def main(): params_mem.put("BenchSpeed", "0") params_mem.put("BenchSpeedLimit", "0") params_mem.put("BenchCruiseSpeed", "0") + params_mem.put("BenchCruiseActive", "0") params_mem.put("BenchGear", "P") params_mem.put("BenchEngaged", "0") @@ -64,16 +66,24 @@ def main(): speed_limit_mph = float((params_mem.get("BenchSpeedLimit", encoding="utf-8") or "0").strip()) cruise_mph = float((params_mem.get("BenchCruiseSpeed", encoding="utf-8") or "0").strip()) gear_str = (params_mem.get("BenchGear", encoding="utf-8") or "P").strip().upper() + cruise_active_str = (params_mem.get("BenchCruiseActive", encoding="utf-8") or "0").strip() engaged = (params_mem.get("BenchEngaged", encoding="utf-8") or "0").strip() == "1" speed_ms = speed_mph * CV.MPH_TO_MS speed_limit_ms = speed_limit_mph * CV.MPH_TO_MS + cruise_ms = cruise_mph * CV.MPH_TO_MS gear = gear_map.get(gear_str, car.CarState.GearShifter.park) + # Cruise state: 0=disabled, 1=active, 2=paused + cruise_active = cruise_active_str == "1" + cruise_standstill = cruise_active_str == "2" + # 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) + speed_state.update(speed_ms, has_speed, speed_limit_ms, is_metric=False, + cruise_speed_ms=cruise_ms, cruise_active=cruise_active or cruise_standstill, + cruise_standstill=cruise_standstill) # pandaStates — 10 Hz (thermald reads ignition from this) if frame % 1 == 0: diff --git a/selfdrive/clearpilot/speed_logic.py b/selfdrive/clearpilot/speed_logic.py index f9e65b0..d4d869f 100644 --- a/selfdrive/clearpilot/speed_logic.py +++ b/selfdrive/clearpilot/speed_logic.py @@ -18,7 +18,8 @@ class SpeedState: 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): + def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool, + cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False): """ Convert raw m/s values to display-ready strings and write to params_memory. @@ -27,18 +28,24 @@ class SpeedState: 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) + cruise_speed_ms: cruise control set speed in m/s + cruise_active: True if cruise is engaged and not paused + cruise_standstill: True if cruise is paused at standstill """ if is_metric: speed_display = speed_ms * CV.MS_TO_KPH speed_limit_display = speed_limit_ms * CV.MS_TO_KPH + cruise_display = cruise_speed_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 + cruise_display = cruise_speed_ms * CV.MS_TO_MPH unit = "mph" speed_int = int(math.floor(speed_display)) speed_limit_int = int(math.floor(speed_limit_display)) + cruise_int = int(round(cruise_display)) # Detect speed limit changes (groundwork for future chime) if speed_limit_int != self.prev_speed_limit: @@ -50,3 +57,21 @@ class SpeedState: 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") + + # Cruise warning logic + # Only evaluate when speed limit >= 20 and cruise is active (not paused, not disabled) + warning = "" + warning_speed = "" + cruise_engaged = cruise_active and not cruise_standstill + + if speed_limit_int >= 20 and cruise_engaged and cruise_int > 0: + over_threshold = 10 if speed_limit_int >= 50 else 5 + if cruise_int >= speed_limit_int + over_threshold: + warning = "over" + warning_speed = str(cruise_int) + elif cruise_int <= speed_limit_int - 5: + warning = "under" + warning_speed = str(cruise_int) + + self.params_memory.put("ClearpilotCruiseWarning", warning) + self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a3e5d99..16234b8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1284,7 +1284,11 @@ class Controls: 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) + cruise_speed_ms = CS.cruiseState.speed + cruise_active = CS.cruiseState.enabled + cruise_standstill = CS.cruiseState.standstill + self.speed_state.update(speed_ms, has_gps, speed_limit_ms, is_metric, + cruise_speed_ms, cruise_active, cruise_standstill) return CC diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 4bc4ca3..5646886 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -95,6 +95,8 @@ def manager_init(frogpilot_functions) -> None: params_memory.put("ClearpilotHasSpeed", "0") params_memory.put("ClearpilotIsMetric", "0") params_memory.put("ClearpilotSpeedUnit", "mph") + params_memory.put("ClearpilotCruiseWarning", "") + params_memory.put("ClearpilotCruiseWarningSpeed", "") 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 55dce76..c82c99d 100755 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -362,6 +362,8 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { clpSpeedDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedDisplay")); clpSpeedLimitDisplay = QString::fromStdString(paramsMemory.get("ClearpilotSpeedLimitDisplay")); clpSpeedUnit = QString::fromStdString(paramsMemory.get("ClearpilotSpeedUnit")); + clpCruiseWarning = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarning")); + clpCruiseWarningSpeed = QString::fromStdString(paramsMemory.get("ClearpilotCruiseWarningSpeed")); } // auto speed_limit_sign = nav_instruction.getSpeedLimitSign(); @@ -462,8 +464,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { drawText(p, rect().center().x(), 290, clpSpeedUnit, 200); } - // CLEARPILOT: speed limit sign in lower-left + // CLEARPILOT: speed limit sign in lower-left, cruise warning above it drawSpeedLimitSign(p); + drawCruiseWarningSign(p); // Draw FrogPilot widgets paintFrogPilotWidgets(p); @@ -564,13 +567,13 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) { p.save(); - const int signW = 150; - const int signH = 190; + const int signW = 189; + const int signH = 239; const int margin = 20; - const int borderW = 5; - const int innerBorderW = 3; - const int innerMargin = 8; - const int cornerR = 12; + const int borderW = 6; + const int innerBorderW = 4; + const int innerMargin = 10; + const int cornerR = 15; // Position: 20px from lower-left corner QRect signRect(margin, height() - signH - margin, signW, signH); @@ -593,16 +596,16 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) { // "SPEED" text p.setPen(textColor); - p.setFont(InterFont(24, QFont::Bold)); - p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED"); + p.setFont(InterFont(30, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 15, 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"); + p.setFont(InterFont(30, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 48, 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.setFont(InterFont(90, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay); } else { // Normal: white background, black border and text QColor borderColor(0, 0, 0); @@ -621,16 +624,96 @@ void AnnotatedCameraWidget::drawSpeedLimitSign(QPainter &p) { // "SPEED" text p.setPen(textColor); - p.setFont(InterFont(24, QFont::Bold)); - p.drawText(innerRect.adjusted(0, 12, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SPEED"); + p.setFont(InterFont(30, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 15, 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"); + p.setFont(InterFont(30, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 48, 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.setFont(InterFont(90, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpSpeedLimitDisplay); + } + + p.restore(); +} + +void AnnotatedCameraWidget::drawCruiseWarningSign(QPainter &p) { + // Only show when there's an active warning and the speed limit sign is visible + if (clpCruiseWarning.isEmpty() || clpCruiseWarningSpeed.isEmpty()) return; + if (clpSpeedLimitDisplay.isEmpty() || clpSpeedLimitDisplay == "0") return; + + bool isOver = (clpCruiseWarning == "over"); + if (!isOver && clpCruiseWarning != "under") return; + + p.save(); + + // Same dimensions as speed limit sign + const int signW = 189; + const int signH = 239; + const int margin = 20; + const int borderW = 6; + const int innerBorderW = 4; + const int innerMargin = 10; + const int cornerR = 15; + const int gap = 20; + + // Position: directly above the speed limit sign + int speedLimitY = height() - signH - margin; + QRect signRect(margin, speedLimitY - signH - gap, signW, signH); + + if (nightriderMode) { + // Nightrider: black background with colored border/text + QColor accentColor = isOver ? QColor(220, 50, 50) : QColor(50, 180, 80); + + p.setPen(QPen(accentColor, borderW)); + p.setBrush(QColor(0, 0, 0, 220)); + p.drawRoundedRect(signRect, cornerR, cornerR); + + QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin); + p.setPen(QPen(accentColor, innerBorderW)); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4); + + // "CRUISE" text + p.setPen(accentColor); + p.setFont(InterFont(26, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE"); + + // "SET" text + p.setFont(InterFont(26, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET"); + + // Cruise speed number + p.setFont(InterFont(90, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed); + } else { + // Normal: colored background with white border/text + QColor bgColor = isOver ? QColor(200, 30, 30, 240) : QColor(40, 160, 60, 240); + QColor fgColor(255, 255, 255); + + p.setPen(QPen(fgColor, borderW)); + p.setBrush(bgColor); + p.drawRoundedRect(signRect, cornerR, cornerR); + + QRect innerRect = signRect.adjusted(innerMargin, innerMargin, -innerMargin, -innerMargin); + p.setPen(QPen(fgColor, innerBorderW)); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(innerRect, cornerR - 4, cornerR - 4); + + // "CRUISE" text + p.setPen(fgColor); + p.setFont(InterFont(26, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 15, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "CRUISE"); + + // "SET" text + p.setFont(InterFont(26, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 45, 0, 0), Qt::AlignTop | Qt::AlignHCenter, "SET"); + + // Cruise speed number + p.setFont(InterFont(90, QFont::Bold)); + p.drawText(innerRect.adjusted(0, 42, 0, 0), Qt::AlignCenter, clpCruiseWarningSpeed); } p.restore(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 0cdec82..bdb87e1 100755 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -52,6 +52,7 @@ 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); + void drawCruiseWarningSign(QPainter &p); QVBoxLayout *main_layout; QPixmap dm_img; @@ -66,6 +67,8 @@ private: QString clpSpeedDisplay; QString clpSpeedLimitDisplay; QString clpSpeedUnit; + QString clpCruiseWarning; + QString clpCruiseWarningSpeed; int clpParamFrame = 0; float setSpeed;