From c55f40e77d90f2128b8d2fcf14e08c2728339021 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Fri, 19 Sep 2025 22:01:08 -0700 Subject: [PATCH 01/12] 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 --- common/params_keys.h | 1 + common/util.h | 1 + .../longitudinal/speed_limit/helpers.h | 10 + .../speed_limit/speed_limit_settings.cc | 37 ++- .../speed_limit/speed_limit_settings.h | 16 ++ selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 213 +++++++++++++++++- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 11 + selfdrive/ui/sunnypilot/ui.cc | 3 +- selfdrive/ui/sunnypilot/ui_scene.h | 1 + 9 files changed, 281 insertions(+), 12 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index a9408037a0..6d60e2cfdd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -227,6 +227,7 @@ inline static std::unordered_map 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"}}, diff --git a/common/util.h b/common/util.h index f46db4d9fa..6d28e3ccbc 100644 --- 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)) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h index 84979a53af..55764372c4 100644 --- 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"), +}; diff --git 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 index 7508ea134c..2bacbace7c 100644 --- 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 speed_limit_mode_texts{ + SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], + SpeedLimitModeTexts[static_cast(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(std::atoi(params.get("SpeedLimitMode").c_str())); SpeedLimitOffsetType offset_type_param = static_cast(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(); } diff --git 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 index 40ffead700..ebffcf65e4 100644 --- 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 = "" + info_str + ""; + } else { + off_str = "" + off_str + ""; + } + + return QString("%1
%2") + .arg(off_str) + .arg(info_str); + } }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index fb2a69c24b..923eb3bb81 100644 --- 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(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(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); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index 4c92835957..d76eb06060 100644 --- 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; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 7b10929bc6..7f83e5ea79 100644 --- 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) { diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index c941be675c..69cebd6d79 100644 --- 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; From bdb83b6be17f6663b9de97ebccb403ad8fd4751f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 01:40:51 -0400 Subject: [PATCH 02/12] ui: remove redundant code (#1262) --- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 8 -------- 1 file changed, 8 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 923eb3bb81..47ba0590d1 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -17,7 +17,6 @@ void HudRendererSP::updateState(const UIState &s) { HudRenderer::updateState(s); const SubMaster &sm = *(s.sm); - const bool cs_alive = sm.alive("controlsState"); const auto cs = sm["controlsState"].getControlsState(); const auto car_state = sm["carState"].getCarState(); const auto car_control = sm["carControl"].getCarControl(); @@ -29,7 +28,6 @@ void HudRendererSP::updateState(const UIState &s) { 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; @@ -60,12 +58,6 @@ void HudRendererSP::updateState(const UIState &s) { reversing = reverse_allowed; - // Handle older routes where vEgoCluster is not set - v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; - float v_ego = v_ego_cluster_seen ? car_state.getVEgoCluster() : car_state.getVEgo(); - speed = cs_alive ? std::max(0.0, v_ego) : 0.0; - speed *= is_metric ? MS_TO_KPH : MS_TO_MPH; - latActive = car_control.getLatActive(); steerOverride = car_state.getSteeringPressed(); From c248f307f86350e16734bb427a6a1c7750618b8a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 01:59:07 -0400 Subject: [PATCH 03/12] ui: Speed Limit Warning (#1263) * ui: Speed Limit Warning * missed * behind toggle * missed * reorder * fix type --- .../settings/longitudinal/speed_limit/helpers.h | 2 ++ .../longitudinal/speed_limit/speed_limit_settings.cc | 1 + .../longitudinal/speed_limit/speed_limit_settings.h | 10 +++++++--- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 12 +++++++----- selfdrive/ui/sunnypilot/ui.cc | 2 +- 5 files changed, 18 insertions(+), 9 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h index 55764372c4..22cdd8c631 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h @@ -37,9 +37,11 @@ inline const QString SpeedLimitSourcePolicyTexts[]{ enum class SpeedLimitMode { OFF, INFORMATION, + WARNING, }; inline const QString SpeedLimitModeTexts[]{ QObject::tr("Off"), QObject::tr("Information"), + QObject::tr("Warning"), }; diff --git 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 index 2bacbace7c..6f48833611 100644 --- 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 @@ -27,6 +27,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) std::vector speed_limit_mode_texts{ SpeedLimitModeTexts[static_cast(SpeedLimitMode::OFF)], SpeedLimitModeTexts[static_cast(SpeedLimitMode::INFORMATION)], + SpeedLimitModeTexts[static_cast(SpeedLimitMode::WARNING)], }; speed_limit_mode_settings = new ButtonParamControlSP( "SpeedLimitMode", diff --git 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 index ebffcf65e4..f83f450557 100644 --- 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 @@ -55,15 +55,19 @@ private: 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."); + QString warning_str = tr("⦿ Warning: Provides a warning when exceeding the current road's speed limit."); - if (mode == SpeedLimitMode::INFORMATION) { + if (mode == SpeedLimitMode::WARNING) { + warning_str = "" + warning_str + ""; + } else if (mode == SpeedLimitMode::INFORMATION) { info_str = "" + info_str + ""; } else { off_str = "" + off_str + ""; } - return QString("%1
%2") + return QString("%1
%2
%3") .arg(off_str) - .arg(info_str); + .arg(info_str) + .arg(warning_str); } }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 47ba0590d1..71bdd92cab 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -328,7 +328,10 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { } void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { - QString speedLimitStr = speedLimit > 0 ? QString::number(std::nearbyint(speedLimit)) : "---"; + int speedLimitRounded = std::nearbyint(speedLimit); + bool overspeed = speedLimitRounded < std::nearbyint(speed) && speedLimitRounded > 0; + bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING; + QString speedLimitStr = speedLimit > 0 ? QString::number(speedLimitRounded) : "---"; // Offset display text QString speedLimitSubText = ""; @@ -344,6 +347,8 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { QRect sign_rect(sign_x, sign_y, sign_width, sign_height); int alpha = 255; + QColor red_color = QColor(255, 0, 0, alpha); + QColor speed_color = (speedLimitWarningEnabled && overspeed) ? red_color : QColor(0, 0, 0, alpha); if (is_metric) { // EU Vienna Convention style circular sign @@ -364,9 +369,8 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { // Red border ring with color coding QRect red_ring = circle_rect; - QColor ring_color = QColor(255, 0, 0, alpha); - p.setBrush(ring_color); + p.setBrush(red_color); p.drawEllipse(red_ring); // Center white circle for text @@ -378,7 +382,6 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { // 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); @@ -424,7 +427,6 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { // 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); diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 7f83e5ea79..7b582a8341 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -53,7 +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"); + s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str()); } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { From 55b7529ca4b6b4a7704dd3822109a54d9397b571 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 05:23:02 -0400 Subject: [PATCH 04/12] ui: Road Name (#1265) --- .../qt/offroad/settings/visuals_panel.cc | 13 +++++-- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 35 +++++++++++++++++++ selfdrive/ui/sunnypilot/qt/onroad/hud.h | 3 ++ selfdrive/ui/sunnypilot/ui.cc | 1 + selfdrive/ui/sunnypilot/ui_scene.h | 1 + 5 files changed, 50 insertions(+), 3 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc index e19760f2e1..ca58282a3d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc @@ -25,21 +25,28 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) { "BlindSpot", tr("Show Blind Spot Warnings"), tr("Enabling this will display warnings when a vehicle is detected in your blind spot as long as your car has BSM supported."), - "../assets/offroad/icon_monitoring.png", + "", false, }, { "RainbowMode", tr("Enable Tesla Rainbow Mode"), RainbowizeWords(tr("A beautiful rainbow effect on the path the model wants to take.")) + "
" + tr("It")+ " " + tr("does not") + " " + tr("affect driving in any way.") + "", - "../assets/offroad/icon_monitoring.png", + "", false, }, { "StandstillTimer", tr("Enable Standstill Timer"), tr("Show a timer on the HUD when the car is at a standstill."), - "../assets/offroad/icon_monitoring.png", + "", + false, + }, + { + "RoadName", + tr("Display Road Name"), + tr("Displays the name of the road the car is traveling on. The OpenStreetMap database of the location must be downloaded from the OSM panel to fetch the road name."), + "", false, }, }; diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 71bdd92cab..991710bbfa 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -32,7 +32,9 @@ void HudRendererSP::updateState(const UIState &s) { speedLimit = lp_sp.getSpeedLimit().getResolver().getSpeedLimit() * speedConv; speedLimitOffset = lp_sp.getSpeedLimit().getResolver().getSpeedLimitOffset() * speedConv; speedLimitMode = static_cast(s.scene.speed_limit_mode); + roadName = s.scene.road_name; if (sm.updated("liveMapDataSP")) { + roadNameStr = QString::fromStdString(lmd.getRoadName()); speedLimitAheadValid = lmd.getSpeedLimitAheadValid(); speedLimitAhead = lmd.getSpeedLimitAhead() * speedConv; speedLimitAheadDistance = lmd.getSpeedLimitAheadDistance(); @@ -138,6 +140,9 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { drawSpeedLimitSigns(p); drawUpcomingSpeedLimit(p); } + + // Road Name + drawRoadName(p, surface_rect); } } @@ -516,3 +521,33 @@ void HudRendererSP::drawUpcomingSpeedLimit(QPainter &p) { p.setPen(QColor(180, 180, 180, 255)); p.drawText(ahead_rect.adjusted(0, 110, 0, 0), Qt::AlignTop | Qt::AlignHCenter, distanceStr); } + +void HudRendererSP::drawRoadName(QPainter &p, const QRect &surface_rect) { + if (!roadName || roadNameStr.isEmpty()) return; + + // Measure text to size container + p.setFont(InterFont(40, QFont::Normal)); + QFontMetrics fm(p.font()); + + int text_width = fm.horizontalAdvance(roadNameStr); + int padding = 40; + int rect_width = text_width + padding; + + // Constrain to reasonable bounds + int min_width = 200; + int max_width = surface_rect.width() - 40; + rect_width = std::max(min_width, std::min(rect_width, max_width)); + + // Center at top of screen + QRect road_rect(surface_rect.width() / 2 - rect_width / 2, -6, rect_width, 60); + + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 120)); + p.drawRoundedRect(road_rect, 12, 12); + + p.setPen(QColor(255, 255, 255, 200)); + + // Truncate if still too long + QString truncated = fm.elidedText(roadNameStr, Qt::ElideRight, road_rect.width() - 20); + p.drawText(road_rect, Qt::AlignCenter, truncated); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index d76eb06060..073780bc10 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -31,6 +31,7 @@ private: 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); + void drawRoadName(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -74,4 +75,6 @@ private: float speedLimitAheadDistancePrev; int speedLimitAheadValidFrame; SpeedLimitMode speedLimitMode = SpeedLimitMode::OFF; + bool roadName; + QString roadNameStr; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index 7b582a8341..9acf844088 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -54,6 +54,7 @@ void ui_update_params_sp(UIStateSP *s) { s->scene.dev_ui_info = std::atoi(params.get("DevUIInfo").c_str()); s->scene.standstill_timer = params.getBool("StandstillTimer"); s->scene.speed_limit_mode = std::atoi(params.get("SpeedLimitMode").c_str()); + s->scene.road_name = params.getBool("RoadName"); } DeviceSP::DeviceSP(QObject *parent) : Device(parent) { diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h index 69cebd6d79..768cc5d7a1 100644 --- a/selfdrive/ui/sunnypilot/ui_scene.h +++ b/selfdrive/ui/sunnypilot/ui_scene.h @@ -11,4 +11,5 @@ typedef struct UISceneSP : UIScene { int dev_ui_info = 0; bool standstill_timer = false; int speed_limit_mode = 0; + bool road_name = false; } UISceneSP; From 5f3821c1f9ce589d4bd37ae7fbc5ce6ad94fa825 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 12:21:13 -0400 Subject: [PATCH 05/12] Longitudinal planner: expose custom vTarget and aTarget (#1267) --- cereal/custom.capnp | 2 ++ .../selfdrive/controls/lib/longitudinal_planner.py | 10 +++++++--- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 6527838097..c4360ae311 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -146,6 +146,8 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { longitudinalPlanSource @1 :LongitudinalPlanSource; smartCruiseControl @2 :SmartCruiseControl; speedLimit @3 :SpeedLimit; + vTarget @4 :Float32; + aTarget @5 :Float32; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index b4f4fe6d7c..3e4dbd9ab2 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -24,6 +24,9 @@ class LongitudinalPlannerSP: self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None self.source = Source.cruise + self.output_v_target = 0. + self.output_a_target = 0. + @property def mlsim(self) -> bool: # If we don't have a generation set, we assume it's default model. Which as of today are mlsim. @@ -47,9 +50,8 @@ class LongitudinalPlannerSP: } self.source = min(targets, key=lambda k: targets[k][0]) - v_target, a_target = targets[self.source] - - return v_target, a_target + self.output_v_target, self.output_a_target = targets[self.source] + return self.output_v_target, self.output_a_target def update(self, sm: messaging.SubMaster) -> None: self.dec.update(sm) @@ -61,6 +63,8 @@ class LongitudinalPlannerSP: longitudinalPlanSP = plan_sp_send.longitudinalPlanSP longitudinalPlanSP.longitudinalPlanSource = self.source + longitudinalPlanSP.vTarget = float(self.output_v_target) + longitudinalPlanSP.aTarget = float(self.output_a_target) # Dynamic Experimental Control dec = longitudinalPlanSP.dec From 632b416f2ac130d0754efeebdea5588d89da5210 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 14:22:10 -0400 Subject: [PATCH 06/12] ICBM: use `longitudinalPlanSP.vTarget` directly for evaluation (#1266) * ICBM: add SCC-V to v_targets list * Longitudinal planner: expose custom vTarget and aTarget * use the source directly --- selfdrive/selfdrived/selfdrived.py | 4 ++-- .../controller.py | 16 ++++------------ 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 69dc51360b..170e184629 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -96,7 +96,7 @@ class SelfdriveD(CruiseHelper): 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback', - 'modelDataV2SP'] + \ + 'modelDataV2SP', 'longitudinalPlanSP'] + \ self.camera_packets + self.sensor_packets + self.gps_packets, ignore_alive=ignore, ignore_avg_freq=ignore, ignore_valid=ignore, frequency=int(1/DT_CTRL)) @@ -444,7 +444,7 @@ class SelfdriveD(CruiseHelper): self.events.add(EventName.personalityChanged) self.experimental_mode_switched = False - self.icbm.run(CS, self.sm['carControl'], self.is_metric) + self.icbm.run(CS, self.sm['carControl'], self.sm['longitudinalPlanSP'], self.is_metric) def data_sample(self): _car_state = messaging.recv_one(self.car_state_sock) diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py index 7ee7f3ea64..468e6f55b5 100644 --- a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py @@ -49,19 +49,11 @@ class IntelligentCruiseButtonManagement: def v_cruise_equal(self) -> bool: return self.v_target == self.v_cruise_cluster - def update_calculations(self, CS: car.CarState) -> None: + def update_calculations(self, CS: car.CarState, LP_SP: custom.LongitudinalPlanSP) -> None: speed_conv = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH ms_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS - v_cruise_ms = CS.vCruise * CV.KPH_TO_MS - # all targets in m/s - v_targets = { - LongitudinalPlanSource.cruise: v_cruise_ms - } - source = min(v_targets, key=lambda k: v_targets[k]) - v_target_ms = v_targets[source] - - self.v_target_ms_last = apply_hysteresis(v_target_ms, self.v_target_ms_last, HYST_GAP * ms_conv) + self.v_target_ms_last = apply_hysteresis(LP_SP.vTarget, self.v_target_ms_last, HYST_GAP * ms_conv) self.v_target = round(self.v_target_ms_last * speed_conv) self.v_cruise_min = get_minimum_set_speed(self.is_metric) @@ -123,13 +115,13 @@ class IntelligentCruiseButtonManagement: self.is_ready = ready and not button_pressed - def run(self, CS: car.CarState, CC: car.CarControl, is_metric: bool) -> None: + def run(self, CS: car.CarState, CC: car.CarControl, LP_SP: custom.LongitudinalPlanSP, is_metric: bool) -> None: if self.CP_SP.pcmCruiseSpeed: return self.is_metric = is_metric - self.update_calculations(CS) + self.update_calculations(CS, LP_SP) self.update_readiness(CS, CC) self.cruise_button = self.update_state_machine() From 2892dc05c8585ba316c9e5ac49744d75c9eced78 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 16:20:22 -0400 Subject: [PATCH 07/12] mapd: use SubMaster polling to validate GPS status (#1268) --- sunnypilot/mapd/live_map_data/base_map_data.py | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/sunnypilot/mapd/live_map_data/base_map_data.py b/sunnypilot/mapd/live_map_data/base_map_data.py index 536d7720b7..6c7679420a 100644 --- a/sunnypilot/mapd/live_map_data/base_map_data.py +++ b/sunnypilot/mapd/live_map_data/base_map_data.py @@ -4,12 +4,12 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -import time from abc import abstractmethod, ABC from cereal import messaging from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.navd.helpers import Coordinate, coordinate_from_param @@ -18,7 +18,9 @@ class BaseMapData(ABC): self.params = Params() self.gps_location_service = get_gps_location_service(self.params) - self.sm = messaging.SubMaster(['livePose', 'carControl'] + [self.gps_location_service]) + gps_packets = [self.gps_location_service] + self.sm = messaging.SubMaster(['livePose'] + gps_packets, ignore_alive=gps_packets, ignore_avg_freq=gps_packets, + ignore_valid=gps_packets, poll='livePose') self.pm = messaging.PubMaster(['liveMapDataSP']) self.last_position = coordinate_from_param("LastGPSPosition", self.params) @@ -44,20 +46,22 @@ class BaseMapData(ABC): gps = self.sm[self.gps_location_service] # ignore the message if the fix is invalid - gps_ok = self.sm.updated[self.gps_location_service] or (time.monotonic() - self.sm.logMonoTime[self.gps_location_service] / 1e9) > 2.0 + gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_MDL < 2.0 if not gps_ok and self.sm['livePose'].inputsOK: - return None + return # livePose has these data, but aren't on cereal self.last_position = Coordinate(gps.latitude, gps.longitude) self.last_altitude = gps.altitude + return + def publish(self) -> None: speed_limit = self.get_current_speed_limit() next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance() mapd_sp_send = messaging.new_message('liveMapDataSP') - mapd_sp_send.valid = self.sm.all_checks(service_list=[self.gps_location_service, 'livePose']) + mapd_sp_send.valid = self.sm.all_checks(['livePose']) live_map_data = mapd_sp_send.liveMapDataSP live_map_data.speedLimitValid = bool(speed_limit > 0) From 629cfd845f1b6fd8723c915f025ba0c18e58a2a0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 17:23:24 -0400 Subject: [PATCH 08/12] ui: fix scrolling behavior in Speed Limit settings (#1269) * rename * ui: fix scrolling behavior in Speed Limit settings --- .../longitudinal/speed_limit/speed_limit_policy.cc | 5 ++++- .../settings/longitudinal/speed_limit/speed_limit_policy.h | 2 ++ .../longitudinal/speed_limit/speed_limit_settings.cc | 7 +++++-- .../longitudinal/speed_limit/speed_limit_settings.h | 2 ++ 4 files changed, 13 insertions(+), 3 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc index 46f361a153..764a8e0208 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.cc @@ -4,6 +4,7 @@ * This file is part of sunnypilot and is licensed under the MIT License. * See the LICENSE.md file in the root directory for more details. */ +#include #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" @@ -38,8 +39,9 @@ SpeedLimitPolicy::SpeedLimitPolicy(QWidget *parent) : QWidget(parent) { list->addItem(speed_limit_policy); connect(speed_limit_policy, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitPolicy::refresh); + speedLimitPolicyScroller = new ScrollViewSP(list, this); + main_layout->addWidget(speedLimitPolicyScroller); refresh(); - main_layout->addWidget(list); }; void SpeedLimitPolicy::refresh() { @@ -48,6 +50,7 @@ void SpeedLimitPolicy::refresh() { } void SpeedLimitPolicy::showEvent(QShowEvent *event) { + speedLimitPolicyScroller->verticalScrollBar()->setValue(0); refresh(); speed_limit_policy->showDescription(); } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h index adeb115db9..219aa53c6e 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h @@ -10,6 +10,7 @@ #include "selfdrive/ui/sunnypilot/ui.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" class SpeedLimitPolicy : public QWidget { Q_OBJECT @@ -25,6 +26,7 @@ signals: private: Params params; ButtonParamControlSP *speed_limit_policy; + ScrollViewSP *speedLimitPolicyScroller; static QString sourceDescription(SpeedLimitSourcePolicy type = SpeedLimitSourcePolicy::CAR_ONLY) { QString car_only = tr("⦿ Car Only: Use Speed Limit data only from Car"); diff --git 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 index 6f48833611..eed0306ca0 100644 --- 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 @@ -31,7 +31,7 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) }; speed_limit_mode_settings = new ButtonParamControlSP( "SpeedLimitMode", - tr("Speed Limit Mode"), + tr("Speed Limit"), "", "", speed_limit_mode_texts, @@ -43,10 +43,12 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) speedLimitSource = new PushButtonSP(tr("Customize Source")); connect(speedLimitSource, &QPushButton::clicked, [&]() { + speedLimitScroller->setLastScrollPosition(); setCurrentWidget(speedLimitPolicyScreen); speedLimitPolicyScreen->refresh(); }); connect(speedLimitPolicyScreen, &SpeedLimitPolicy::backPress, [&]() { + speedLimitScroller->restoreScrollPosition(); setCurrentWidget(subPanelFrame); showEvent(new QShowEvent()); }); @@ -92,7 +94,8 @@ SpeedLimitSettings::SpeedLimitSettings(QWidget *parent) : QStackedWidget(parent) connect(speed_limit_offset_settings, &ButtonParamControlSP::buttonClicked, this, &SpeedLimitSettings::refresh); refresh(); - subPanelLayout->addWidget(list); + speedLimitScroller = new ScrollViewSP(list, this); + subPanelLayout->addWidget(speedLimitScroller); addWidget(subPanelFrame); addWidget(speedLimitPolicyScreen); setCurrentWidget(subPanelFrame); diff --git 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 index f83f450557..61c86f9208 100644 --- 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 @@ -12,6 +12,7 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/helpers.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_policy.h" #include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" class SpeedLimitSettings : public QStackedWidget { Q_OBJECT @@ -26,6 +27,7 @@ signals: private: Params params; + ScrollViewSP *speedLimitScroller; QFrame *subPanelFrame; ButtonParamControlSP *speed_limit_mode_settings; PushButtonSP *speedLimitSource; From 569a9216db77e650ec55a3062724d7c881cab60e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 17:36:13 -0400 Subject: [PATCH 09/12] ui: only draw speed limit offset when speed limit is valid (#1270) --- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 991710bbfa..6805422393 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -333,15 +333,16 @@ void HudRendererSP::drawStandstillTimer(QPainter &p, int x, int y) { } void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { + bool speedLimitValid = speedLimit > 0; int speedLimitRounded = std::nearbyint(speedLimit); bool overspeed = speedLimitRounded < std::nearbyint(speed) && speedLimitRounded > 0; bool speedLimitWarningEnabled = speedLimitMode == SpeedLimitMode::WARNING; - QString speedLimitStr = speedLimit > 0 ? QString::number(speedLimitRounded) : "---"; + QString speedLimitStr = speedLimitValid ? QString::number(speedLimitRounded) : "---"; // Offset display text QString speedLimitSubText = ""; if (speedLimitOffset != 0) { - speedLimitSubText = (speedLimitOffset > 0 ? "+" : "") + QString::number(std::nearbyint(speedLimitOffset)); + speedLimitSubText = (speedLimitOffset > 0 ? "+" : "-") + QString::number(std::nearbyint(speedLimitOffset)); } // Position next to MAX speed box @@ -392,7 +393,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { p.drawText(center_circle, Qt::AlignCenter, speedLimitStr); // Offset value in small circular box - if (!speedLimitSubText.isEmpty()) { + if (!speedLimitSubText.isEmpty() && speedLimitValid) { int offset_circle_size = circle_size * 0.4; int overlap = offset_circle_size * 0.25; QRect offset_circle_rect( @@ -437,7 +438,7 @@ void HudRendererSP::drawSpeedLimitSigns(QPainter &p) { p.drawText(inner_rect.adjusted(0, 80, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); // Offset value in small box - if (!speedLimitSubText.isEmpty()) { + if (!speedLimitSubText.isEmpty() && speedLimitValid) { int offset_box_size = sign_rect.width() * 0.4; int overlap = offset_box_size * 0.25; QRect offset_box_rect( From 2efe78a4ef16aab116a26a673f7764f421d686d4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 20 Sep 2025 17:48:13 -0400 Subject: [PATCH 10/12] ICBM: allow button commands at all speeds (#1271) * ICBM: allow button commands at all speeds * use openpilot state --- .../car/intelligent_cruise_button_management/controller.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py index 468e6f55b5..1f491e0f58 100644 --- a/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py +++ b/sunnypilot/selfdrive/car/intelligent_cruise_button_management/controller.py @@ -108,9 +108,7 @@ class IntelligentCruiseButtonManagement: def update_readiness(self, CS: car.CarState, CC: car.CarControl) -> None: update_manual_button_timers(CS, self.cruise_button_timers) - allowed_speed = CS.vEgo > ALLOWED_SPEED_THRESHOLD - ready = CS.cruiseState.enabled and allowed_speed and not CC.cruiseControl.override and not CC.cruiseControl.cancel and \ - not CC.cruiseControl.resume + ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume button_pressed = any(self.cruise_button_timers[k] > 0 for k in self.cruise_button_timers) self.is_ready = ready and not button_pressed From 563ae654438d49abf711d89410430b98836fe64b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Sep 2025 00:33:43 -0400 Subject: [PATCH 11/12] ui: Road Name param should be persistent (#1273) --- common/params_keys.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/params_keys.h b/common/params_keys.h index 6d60e2cfdd..e9ffcfc1fd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -224,7 +224,7 @@ inline static std::unordered_map keys = { {"OsmStateName", {PERSISTENT, STRING, "All"}}, {"OsmStateTitle", {PERSISTENT, STRING}}, {"OsmWayTest", {PERSISTENT, STRING}}, - {"RoadName", {CLEAR_ON_ONROAD_TRANSITION, STRING}}, + {"RoadName", {PERSISTENT, STRING}}, // Speed Limit {"SpeedLimitMode", {PERSISTENT | BACKUP, INT, "1"}}, From d5a873ed8617cefd691509c9cc5dc4ec7ea42ff4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 21 Sep 2025 02:53:03 -0400 Subject: [PATCH 12/12] ui: generic check with ICBM param (#1274) --- .../qt/offroad/settings/longitudinal_panel.cc | 10 ++++++---- .../qt/offroad/settings/longitudinal_panel.h | 3 ++- selfdrive/ui/sunnypilot/qt/util.cc | 4 ++++ selfdrive/ui/sunnypilot/qt/util.h | 1 + 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 205f64a0b2..8faa907a6d 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -80,6 +80,7 @@ void LongitudinalPanel::showEvent(QShowEvent *event) { } void LongitudinalPanel::refresh(bool _offroad) { + auto icbm_available = false; auto cp_bytes = params.get("CarParamsPersistent"); auto cp_sp_bytes = params.get("CarParamsSPPersistent"); if (!cp_bytes.empty() && !cp_sp_bytes.empty()) { @@ -92,11 +93,12 @@ void LongitudinalPanel::refresh(bool _offroad) { has_longitudinal_control = hasLongitudinalControl(CP); is_pcm_cruise = CP.getPcmCruise(); - intelligent_cruise_button_management_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + icbm_available = CP_SP.getIntelligentCruiseButtonManagementAvailable(); + has_intelligent_cruise_button_management = hasIntelligentCruiseButtonManagement(CP_SP); } else { has_longitudinal_control = false; is_pcm_cruise = false; - intelligent_cruise_button_management_available = false; + has_intelligent_cruise_button_management = false; } QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); @@ -108,7 +110,7 @@ void LongitudinalPanel::refresh(bool _offroad) { customAccIncrement->setDescription(onroadOnlyDescription); customAccIncrement->showDescription(); } else { - if (has_longitudinal_control || intelligent_cruise_button_management_available) { + if (has_longitudinal_control || icbm_available) { if (is_pcm_cruise) { customAccIncrement->setDescription(accPcmCruiseDisabledDescription); customAccIncrement->showDescription(); @@ -125,7 +127,7 @@ void LongitudinalPanel::refresh(bool _offroad) { } } - bool icbm_allowed = intelligent_cruise_button_management_available && !has_longitudinal_control; + bool icbm_allowed = has_intelligent_cruise_button_management && !has_longitudinal_control; intelligentCruiseButtonManagement->setEnabled(icbm_allowed && offroad); // enable toggle when long is available and is not PCM cruise diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index 8464150656..1e26ea22fc 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -7,6 +7,7 @@ #pragma once +#include "selfdrive/ui/sunnypilot/qt/util.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/speed_limit/speed_limit_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" @@ -24,7 +25,7 @@ private: Params params; bool has_longitudinal_control = false; bool is_pcm_cruise = false; - bool intelligent_cruise_button_management_available = false;; + bool has_intelligent_cruise_button_management = false;; bool offroad = false; QStackedLayout *main_layout = nullptr; diff --git a/selfdrive/ui/sunnypilot/qt/util.cc b/selfdrive/ui/sunnypilot/qt/util.cc index 2e066e88b5..eaf05942a9 100644 --- a/selfdrive/ui/sunnypilot/qt/util.cc +++ b/selfdrive/ui/sunnypilot/qt/util.cc @@ -123,3 +123,7 @@ std::optional loadCerealEvent(Params& params, const std:: return std::nullopt; } } + +bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp) { + return car_params_sp.getIntelligentCruiseButtonManagementAvailable() && Params().getBool("IntelligentCruiseButtonManagement"); +} diff --git a/selfdrive/ui/sunnypilot/qt/util.h b/selfdrive/ui/sunnypilot/qt/util.h index 4b9d615ce5..60a73615ba 100644 --- a/selfdrive/ui/sunnypilot/qt/util.h +++ b/selfdrive/ui/sunnypilot/qt/util.h @@ -23,3 +23,4 @@ std::optional getParamIgnoringDefault(const std::string ¶m_name, co QMap loadPlatformList(); QStringList searchFromList(const QString &query, const QStringList &list); std::optional loadCerealEvent(Params& params, const std::string& _param); +bool hasIntelligentCruiseButtonManagement(const cereal::CarParamsSP::Reader &car_params_sp);