ui: Speed Limit Information (#995)

* less for now * always on and fix conv * update api * missed * fix convert * only draw when decreasing, seems like a bug from mapd * use frame check, otherwise too jumpy * only update when mapd updates * count up and down * introduce toggle * more ui * slightly wider * desc * rename * slight cleanup * fix offset position * more vienna fix * fix ahead alignment * road name in another PR * cleanup * single place * adjust ahead distance display * cleanup * Near * bump it * cleanup logic --------- Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
2026-02-18 13:04:01 +08:00 · 2025-09-19 22:01:08 -07:00
parent ddf63701e8
commit c55f40e77d
9 changed files with 281 additions and 12 deletions
--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -227,6 +227,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}},

    // Speed Limit
+    {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}},
    {"SpeedLimitOffsetType", {PERSISTENT | BACKUP, INT, "0"}},
    {"SpeedLimitPolicy", {PERSISTENT | BACKUP, INT, "3"}},
    {"SpeedLimitValueOffset", {PERSISTENT | BACKUP, INT, "0"}},
--- a/common/util.h
+++ b/common/util.h
@@ -36,6 +36,7 @@ const double MS_TO_KPH = 3.6;
 const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
 const double METER_TO_MILE = KM_TO_MILE / 1000.0;
 const double METER_TO_FOOT = 3.28084;
+const double METER_TO_KM = 1. / 1000.0;

 #define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1))

--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h
@@ -33,3 +33,13 @@ inline const QString SpeedLimitSourcePolicyTexts[]{
  QObject::tr("Map\nFirst"),
  QObject::tr("Combined\nData")
 };
+
+enum class SpeedLimitMode {
+  OFF,
+  INFORMATION,
+};
+
+inline const QString SpeedLimitModeTexts[]{
+  QObject::tr("Off"),
+  QObject::tr("Information"),
+};
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.cc
@@ -20,16 +20,26 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)

  subPanelLayout->addSpacing(20);

-  ListWidgetSP *list = new ListWidgetSP(this);
-
-  auto *speedLimitBtnFrame = new QFrame(this);
-  auto *speedLimitBtnFrameLayout = new QGridLayout();
-  speedLimitBtnFrame->setLayout(speedLimitBtnFrameLayout);
-  speedLimitBtnFrameLayout->setContentsMargins(0, 40, 0, 40);
-  speedLimitBtnFrameLayout->setSpacing(0);
+  ListWidgetSP *list = new ListWidgetSP(this, false);

  speedLimitPolicyScreen = new SpeedLimitPolicy(this);

+  std::vector<QString> speed_limit_mode_texts{
+    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::OFF)],
+    SpeedLimitModeTexts[static_cast<int>(SpeedLimitMode::INFORMATION)],
+  };
+  speed_limit_mode_settings = new ButtonParamControlSP(
+    "SpeedLimitMode",
+    tr("Speed Limit Mode"),
+    "",
+    "",
+    speed_limit_mode_texts,
+    385);
+  list->addItem(speed_limit_mode_settings);
+
+  list->addItem(horizontal_line());
+  list->addItem(vertical_space());
+
  speedLimitSource = new PushButtonSP(tr("Customize Source"));
  connect(speedLimitSource, &QPushButton::clicked, [&]() {
    setCurrentWidget(speedLimitPolicyScreen);
@@ -41,8 +51,10 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)
  });

  speedLimitSource->setFixedWidth(720);
-  speedLimitBtnFrameLayout->addWidget(speedLimitSource, 0, 0, Qt::AlignLeft);
-  list->addItem(speedLimitBtnFrame);
+  list->addItem(speedLimitSource);
+
+  list->addItem(vertical_space(0));
+  list->addItem(horizontal_line());

  QFrame *offsetFrame = new QFrame(this);
  QVBoxLayout *offsetLayout = new QVBoxLayout(offsetFrame);
@@ -73,6 +85,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)

  list->addItem(offsetFrame);

+  connect(speed_limit_mode_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
  connect(speed_limit_offset, &OptionControlSP::updateLabels, this, &SpeedLimitSettings::refresh);
  connect(speed_limit_offset_settings, &ButtonParamControlSP::showDescriptionEvent, speed_limit_offset, &OptionControlSP::showDescription);
  connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh);
@@ -86,9 +99,11 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent)

 void SpeedLimitSettings::refresh() {
  bool is_metric_param = params.getBool("IsMetric");
+  SpeedLimitMode speed_limit_mode_param = static_cast<SpeedLimitMode>(std::atoi(params.get("SpeedLimitMode").c_str()));
  SpeedLimitOffsetType offset_type_param = static_cast<SpeedLimitOffsetType>(std::atoi(params.get("SpeedLimitOffsetType").c_str()));
  QString offsetLabel = QString::fromStdString(params.get("SpeedLimitValueOffset"));

+  speed_limit_mode_settings->setDescription(modeDescription(speed_limit_mode_param));
  speed_limit_offset->setDescription(offsetDescription(offset_type_param));

  if (offset_type_param == SpeedLimitOffsetType::PERCENT) {
@@ -104,9 +119,11 @@ void SpeedLimitSettings::refresh() {
    speed_limit_offset->setLabel(offsetLabel);
    speed_limit_offset->showDescription();
  }
+
+  speed_limit_mode_settings->showDescription();
+  speed_limit_offset->showDescription();
 }

 void SpeedLimitSettings::showEvent(QShowEvent *event) {
  refresh();
-  speed_limit_offset->showDescription();
 }
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h
@@ -27,6 +27,7 @@ signals:
 private:
  Params params;
  QFrame *subPanelFrame;
+  ButtonParamControlSP *speed_limit_mode_settings;
  PushButtonSP *speedLimitSource;
  SpeedLimitPolicy *speedLimitPolicyScreen;
  ButtonParamControlSP *speed_limit_offset_settings;
@@ -50,4 +51,19 @@ private:
        .arg(fixed_str)
        .arg(percent_str);
  }
+
+  static QString modeDescription(SpeedLimitMode mode = SpeedLimitMode::OFF) {
+    QString off_str = tr("⦿ Off: Disables the Speed Limit functions.");
+    QString info_str = tr("⦿ Information: Displays the current road's speed limit.");
+
+    if (mode == SpeedLimitMode::INFORMATION) {
+      info_str = "<font color='white'><b>" + info_str + "</b></font>";
+    } else {
+      off_str = "<font color='white'><b>" + off_str + "</b></font>";
+    }
+
+    return QString("%1<br>%2")
+        .arg(off_str)
+        .arg(info_str);
+  }
 };
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
@@ -27,6 +27,24 @@ void HudRendererSP::updateState(const UIState &s) {
  const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
  const auto car_params = sm["carParams"].getCarParams();
  const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();
+  const auto lmd = sm["liveMapDataSP"].getLiveMapDataSP();
+
+  is_metric = s.scene.is_metric;
+  float speedConv = is_metric ? MS_TO_KPH : MS_TO_MPH;
+  speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv;
+  speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv;
+  speedLimitMode = static_cast<SpeedLimitMode>(s.scene.speed_limit_mode);
+  if (sm.updated("liveMapDataSP")) {
+    speedLimitAheadValid = lmd.getSpeedLimitAheadValid();
+    speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv;
+    speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance();
+    if (speedLimitAheadDistance < speedLimitAheadDistancePrev && speedLimitAheadValidFrame < 2) {
+      speedLimitAheadValidFrame++;
+    } else if (speedLimitAheadDistance > speedLimitAheadDistancePrev && speedLimitAheadValidFrame > 0) {
+      speedLimitAheadValidFrame--;
+    }
+  }
+  speedLimitAheadDistancePrev = speedLimitAheadDistance;

  static int reverse_delay = 0;
  bool reverse_allowed = false;
@@ -41,7 +59,6 @@ void HudRendererSP::updateState(const UIState &s) {
  }

  reversing = reverse_allowed;
-  is_metric = s.scene.is_metric;

  // Handle older routes where vEgoCluster is not set
  v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0;
@@ -123,6 +140,12 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
    if (standstillTimer) {
      drawStandstillTimer(p, surface_rect.right() / 12 * 10, surface_rect.bottom() / 12 * 1.53);
    }
+
+    // Speed Limit
+    if (speedLimitMode != SpeedLimitMode::OFF) {
+      drawSpeedLimitSigns(p);
+      drawUpcomingSpeedLimit(p);
+    }
  }
 }

@@ -311,3 +334,191 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) {
    standstillElapsedTime = 0.0;
  }
 }
+
+void HudRendererSP::drawSpeedLimitSigns(QPainter &p) {
+  QString speedLimitStr = speedLimit > 0 ? QString::number(std::nearbyint(speedLimit)) : "---";
+
+  // Offset display text
+  QString speedLimitSubText = "";
+  if (speedLimitOffset != 0) {
+    speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset));
+  }
+
+  // Position next to MAX speed box
+  const int sign_width = is_metric ? 200 : 172;
+  const int sign_x = is_metric ? 280 : 272;
+  const int sign_y = 45;
+  const int sign_height = 204;
+  QRect sign_rect(sign_x, sign_y, sign_width, sign_height);
+
+  int alpha = 255;
+
+  if (is_metric) {
+    // EU Vienna Convention style circular sign
+    QRect vienna_rect = sign_rect;
+    int circle_size = std::min(vienna_rect.width(), vienna_rect.height());
+    QRect circle_rect(vienna_rect.x(), vienna_rect.y(), circle_size, circle_size);
+
+    if (vienna_rect.width() > vienna_rect.height()) {
+      circle_rect.moveLeft(vienna_rect.x() + (vienna_rect.width() - circle_size) / 2);
+    } else if (vienna_rect.height() > vienna_rect.width()) {
+      circle_rect.moveTop(vienna_rect.y() + (vienna_rect.height() - circle_size) / 2);
+    }
+
+    // White background circle
+    p.setPen(Qt::NoPen);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawEllipse(circle_rect);
+
+    // Red border ring with color coding
+    QRect red_ring = circle_rect;
+    QColor ring_color = QColor(255, 0, 0, alpha);
+
+    p.setBrush(ring_color);
+    p.drawEllipse(red_ring);
+
+    // Center white circle for text
+    int ring_size = circle_size * 0.12;
+    QRect center_circle = red_ring.adjusted(ring_size, ring_size, -ring_size, -ring_size);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawEllipse(center_circle);
+
+    // Speed value, smaller font for 3+ digits
+    int font_size = (speedLimitStr.size() >= 3) ? 70 : 85;
+    p.setFont(InterFont(font_size, QFont::Bold));
+    QColor speed_color = QColor(0, 0, 0, alpha);
+
+    p.setPen(speed_color);
+    p.drawText(center_circle, Qt::AlignCenter, speedLimitStr);
+
+    // Offset value in small circular box
+    if (!speedLimitSubText.isEmpty()) {
+      int offset_circle_size = circle_size * 0.4;
+      int overlap = offset_circle_size * 0.25;
+      QRect offset_circle_rect(
+        circle_rect.right() - offset_circle_size/1.25 + overlap,
+        circle_rect.top() - offset_circle_size/1.75 + overlap,
+        offset_circle_size,
+        offset_circle_size
+      );
+
+      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
+      p.setBrush(QColor(0, 0, 0, alpha));
+      p.drawEllipse(offset_circle_rect);
+
+      p.setFont(InterFont(offset_circle_size * 0.45, QFont::Bold));
+      p.setPen(QColor(255, 255, 255, alpha));
+      p.drawText(offset_circle_rect, Qt::AlignCenter, speedLimitSubText);
+    }
+  } else {
+    // US/Canada MUTCD style sign
+    p.setPen(Qt::NoPen);
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawRoundedRect(sign_rect, 32, 32);
+
+    // Inner border with violation color coding
+    QRect inner_rect = sign_rect.adjusted(10, 10, -10, -10);
+    QColor border_color = QColor(0, 0, 0, alpha);
+
+    p.setPen(QPen(border_color, 4));
+    p.setBrush(QColor(255, 255, 255, alpha));
+    p.drawRoundedRect(inner_rect, 22, 22);
+
+    // "SPEED LIMIT" text
+    p.setFont(InterFont(40, QFont::DemiBold));
+    p.setPen(QColor(0, 0, 0, alpha));
+    p.drawText(inner_rect.adjusted(0, 10, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED"));
+    p.drawText(inner_rect.adjusted(0, 50, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT"));
+
+    // Speed value with color coding
+    p.setFont(InterFont(90, QFont::Bold));
+    QColor speed_color = QColor(0, 0, 0, alpha);
+
+    p.setPen(speed_color);
+    p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr);
+
+    // Offset value in small box
+    if (!speedLimitSubText.isEmpty()) {
+      int offset_box_size = sign_rect.width() * 0.4;
+      int overlap = offset_box_size * 0.25;
+      QRect offset_box_rect(
+        sign_rect.right() - offset_box_size/1.5 + overlap,
+        sign_rect.top() - offset_box_size/1.25 + overlap,
+        offset_box_size,
+        offset_box_size
+      );
+
+      int corner_radius = offset_box_size * 0.2;
+      p.setPen(QPen(QColor(77, 77, 77, 255), 6));
+      p.setBrush(QColor(0, 0, 0, alpha));
+      p.drawRoundedRect(offset_box_rect, corner_radius, corner_radius);
+
+      p.setFont(InterFont(offset_box_size * 0.45, QFont::Bold));
+      p.setPen(QColor(255, 255, 255, alpha));
+      p.drawText(offset_box_rect, Qt::AlignCenter, speedLimitSubText);
+    }
+  }
+}
+
+void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) {
+  bool speed_limit_ahead = speedLimitAheadValid && speedLimitAhead > 0 && speedLimitAhead != speedLimit && speedLimitAheadValidFrame > 0;
+  if (!speed_limit_ahead) {
+    return;
+  }
+
+  auto roundToInterval = [&](float distance, int interval, int threshold) {
+    int base = static_cast<int>(distance / interval) * interval;
+    return (distance - base >= threshold) ? base + interval : base;
+  };
+
+  auto outputDistance = [&] {
+    if (is_metric) {
+      if (speedLimitAheadDistance < 50) return tr("Near");
+      if (speedLimitAheadDistance >= 1000) return QString::number(speedLimitAheadDistance * METER_TO_KM, 'f', 1) + tr("km");
+
+      int rounded = (speedLimitAheadDistance < 200) ? std::max(10, roundToInterval(speedLimitAheadDistance, 10, 5)) : roundToInterval(speedLimitAheadDistance, 100, 50);
+      return QString::number(rounded) + tr("m");
+    } else {
+      float distance_ft = speedLimitAheadDistance * METER_TO_FOOT;
+      if (distance_ft < 100) return tr("Near");
+      if (distance_ft >= 900) return QString::number(speedLimitAheadDistance * METER_TO_MILE, 'f', 1) + tr("mi");
+
+      int rounded = (distance_ft < 500) ? std::max(50, roundToInterval(distance_ft, 50, 25)) : roundToInterval(distance_ft, 100, 50);
+      return QString::number(rounded) + tr("ft");
+    }
+  };
+
+  QString speedStr = QString::number(std::nearbyint(speedLimitAhead));
+  QString distanceStr = outputDistance();
+
+  // Position below current speed limit sign
+  const int sign_width = is_metric ? 200 : 172;
+  const int sign_x = is_metric ? 280 : 272;
+  const int sign_y = 45;
+  const int sign_height = 204;
+
+  const int ahead_width = 170;
+  const int ahead_height = 160;
+  const int ahead_x = sign_x + (sign_width - ahead_width) / 2;
+  const int ahead_y = sign_y + sign_height + 10;
+
+  QRect ahead_rect(ahead_x, ahead_y, ahead_width, ahead_height);
+  p.setPen(QPen(QColor(255, 255, 255, 100), 3));
+  p.setBrush(QColor(0, 0, 0, 180));
+  p.drawRoundedRect(ahead_rect, 16, 16);
+
+  // "AHEAD" label
+  p.setFont(InterFont(40, QFont::DemiBold));
+  p.setPen(QColor(200, 200, 200, 255));
+  p.drawText(ahead_rect.adjusted(0, 4, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("AHEAD"));
+
+  // Speed value
+  p.setFont(InterFont(70, QFont::Bold));
+  p.setPen(QColor(255, 255, 255, 255));
+  p.drawText(ahead_rect.adjusted(0, 38, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedStr);
+
+  // Distance
+  p.setFont(InterFont(40, QFont::Normal));
+  p.setPen(QColor(180, 180, 180, 255));
+  p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr);
+}
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
@@ -8,6 +8,7 @@
 #pragma once

 #include "selfdrive/ui/qt/onroad/hud.h"
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h"
 #include "selfdrive/ui/sunnypilot/qt/onroad/developer_ui/developer_ui.h"

 class HudRendererSP : public HudRenderer {
@@ -28,6 +29,8 @@ private:
  void drawStandstillTimer(QPainter &p, int x, int y);
  bool pulseElement(int frame);
  void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);
+  void drawSpeedLimitSigns(QPainter &p);
+  void drawUpcomingSpeedLimit(QPainter &p);

  bool lead_status;
  float lead_d_rel;
@@ -63,4 +66,12 @@ private:
  bool smartCruiseControlVisionEnabled;
  bool smartCruiseControlVisionActive;
  int smartCruiseControlVisionFrame;
+  float speedLimit;
+  float speedLimitOffset;
+  bool speedLimitAheadValid;
+  float speedLimitAhead;
+  float speedLimitAheadDistance;
+  float speedLimitAheadDistancePrev;
+  int speedLimitAheadValidFrame;
+  SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF;
 };
--- a/selfdrive/ui/sunnypilot/ui.cc
+++ b/selfdrive/ui/sunnypilot/ui.cc
@@ -20,7 +20,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
    "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
    "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
    "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
-    "carStateSP", "liveParameters"
+    "carStateSP", "liveParameters", "liveMapDataSP"
  });

  // update timer
@@ -53,6 +53,7 @@ void ui_update_params_sp(UIStateSP *s) {
  auto params = Params();
  s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str());
  s->scene.standstill_timer = params.getBool("StandstillTimer");
+  s->scene.speed_limit_mode = params.getBool("SpeedLimitMode");
 }

 DeviceSP::DeviceSP(QObject *parent) : Device(parent) {
--- a/selfdrive/ui/sunnypilot/ui_scene.h
+++ b/selfdrive/ui/sunnypilot/ui_scene.h
@@ -10,4 +10,5 @@
 typedef struct UISceneSP : UIScene {
  int dev_ui_info = 0;
  bool standstill_timer = false;
+  int speed_limit_mode = 0;
 } UISceneSP;