From 0e02fc2449cd3b57da4a1faa163d5d68c6485b45 Mon Sep 17 00:00:00 2001 From: James Vecellio Date: Sun, 2 Nov 2025 18:53:53 -0800 Subject: [PATCH] commacon stuff --- selfdrive/ui/sunnypilot/qt/onroad/hud.cc | 87 +++++++++++++++++++- selfdrive/ui/sunnypilot/qt/onroad/hud.h | 9 ++ selfdrive/ui/sunnypilot/ui.cc | 2 +- sunnypilot/selfdrive/assets/nav_left.png | 3 + sunnypilot/selfdrive/assets/nav_right.png | 3 + sunnypilot/selfdrive/assets/nav_straight.png | 3 + 6 files changed, 104 insertions(+), 3 deletions(-) create mode 100644 sunnypilot/selfdrive/assets/nav_left.png create mode 100644 sunnypilot/selfdrive/assets/nav_right.png create mode 100644 sunnypilot/selfdrive/assets/nav_straight.png diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc index 2fc9eb98fc..33a5b8bcad 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc @@ -15,6 +15,10 @@ HudRendererSP::HudRendererSP() { plus_arrow_up_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_plus_arrow_up", {90, 90}); minus_arrow_down_img = loadPixmap("../../sunnypilot/selfdrive/assets/img_minus_arrow_down", {90, 90}); + nav_left_img = loadPixmap("../../sunnypilot/selfdrive/assets/nav_left", {176, 176}); + nav_right_img = loadPixmap("../../sunnypilot/selfdrive/assets/nav_right", {176, 176}); + nav_straight_img = loadPixmap("../../sunnypilot/selfdrive/assets/nav_straight", {176, 176}); + int size = e2e_alert_size * 2 - 40; green_light_alert_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/green_light.png", {size, size}); lead_depart_alert_img = loadPixmap("../../sunnypilot/selfdrive/assets/images/lead_depart.png", {size, size}); @@ -154,6 +158,47 @@ void HudRendererSP::updateState(const UIState &s) { allow_e2e_alerts = sm["selfdriveState"].getSelfdriveState().getAlertSize() == cereal::SelfdriveState::AlertSize::NONE && sm.rcv_frame("driverStateV2") > s.scene.started_frame && !reversing; + + // Navigationd + if (sm.updated("navigationd")) { + auto nav = sm["navigationd"].getNavigationd(); + navigationValid = nav.getValid(); + if (navigationValid && nav.getAllManeuvers().size() > 0) { + auto maneuver = nav.getAllManeuvers()[-1]; + QString modifier = QString::fromStdString(maneuver.getModifier()); + if (modifier == "left") navigationArrowType = "left"; + else if (modifier == "right") navigationArrowType = "right"; + else navigationArrowType = "straight"; + + float dist = maneuver.getDistance(); + if (is_metric) { + if (dist < 1000) { + navigationDistance = QString::number((int)dist) + " m"; + } else { + navigationDistance = QString::number(dist / 1000, 'f', 1) + " km"; + } + } else { + float dist_ft = dist * 3.28084f; + if (dist_ft < 528) { + navigationDistance = QString::number((int)dist_ft) + " ft"; + } else { + navigationDistance = QString::number(dist_ft / 5280, 'f', 1) + " mi"; + } + } + + QString instruction = QString::fromStdString(maneuver.getInstruction()); + QStringList parts = instruction.split(" onto "); + if (parts.size() > 1) { + navigationStreet = parts[1].trimmed(); + } else { + navigationStreet = instruction; + } + } else { + navigationStreet = "---"; + navigationDistance = "---"; + navigationArrowType = "straight"; + } + } } void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { @@ -287,6 +332,8 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) { } } + drawNavigationHUD(p, surface_rect); + p.restore(); } @@ -795,11 +842,15 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const Q void HudRendererSP::drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect) { QString speedStr = QString::number(std::nearbyint(speed)); + int speedX = surface_rect.left() + 120; + int speedY = surface_rect.center().y() - 50; + int unitsY = surface_rect.center().y(); + p.setFont(InterFont(176, QFont::Bold)); - HudRenderer::drawText(p, surface_rect.center().x(), 210, speedStr); + HudRenderer::drawText(p, speedX, speedY, speedStr); p.setFont(InterFont(66)); - HudRenderer::drawText(p, surface_rect.center().x(), 290, is_metric ? tr("km/h") : tr("mph"), 200); + HudRenderer::drawText(p, speedX, unitsY, is_metric ? tr("km/h") : tr("mph"), 200); } void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) { @@ -885,3 +936,35 @@ void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) { p.restore(); } + +void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) { + int x = surface_rect.center().x(); + int y = 50; + int arrowSize = 176; + int cy_offset = (navigationArrowType == "left" || navigationArrowType == "right") ? -20 : 0; + int cx = x - 100; + int cy = y + arrowSize/2 + cy_offset; + + QPixmap arrow_img; + if (navigationArrowType == "left") { + arrow_img = nav_left_img; + } else if (navigationArrowType == "right") { + arrow_img = nav_right_img; + } else { + arrow_img = nav_straight_img; + } + + p.drawPixmap(cx - arrowSize/2, cy - arrowSize/2, arrowSize, arrowSize, arrow_img); + int gap = (navigationArrowType == "left") ? -10 : 20; + int textX = cx + arrowSize/2 + gap; + int textY = y + arrowSize/2; + + // Draw distance text + p.setFont(InterFont(80, QFont::Bold)); + p.setPen(Qt::white); + p.drawText(textX, textY, navigationDistance); + + p.setFont(InterFont(50)); + int streetY = textY + 60; // draw street name below distance text + p.drawText(textX, streetY, navigationStreet); +} diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h index f021a949e4..47df291a40 100644 --- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h +++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h @@ -39,6 +39,7 @@ private: void drawE2eAlert(QPainter &p, const QRect &surface_rect, const QString &alert_alt_text = ""); void drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect); void drawBlinker(QPainter &p, const QRect &surface_rect); + void drawNavigationHUD(QPainter &p, const QRect &surface_rect); bool lead_status; float lead_d_rel; @@ -120,4 +121,12 @@ private: float speedCluster = 0; int icbm_active_counter = 0; bool pcmCruiseSpeed = true; + + bool navigationValid; + QString navigationStreet; + QString navigationDistance; + QString navigationArrowType; + QPixmap nav_left_img; + QPixmap nav_right_img; + QPixmap nav_straight_img; }; diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc index df7b3ce5ca..4665b4ad5a 100644 --- a/selfdrive/ui/sunnypilot/ui.cc +++ b/selfdrive/ui/sunnypilot/ui.cc @@ -29,7 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) { "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP", "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters", - "carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP" + "carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd" }); // update timer diff --git a/sunnypilot/selfdrive/assets/nav_left.png b/sunnypilot/selfdrive/assets/nav_left.png new file mode 100644 index 0000000000..1622c7b389 --- /dev/null +++ b/sunnypilot/selfdrive/assets/nav_left.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:946954b61ee5c89f01481cb030a6364f5ee447b4c8408f64214510e30b53715d +size 780 diff --git a/sunnypilot/selfdrive/assets/nav_right.png b/sunnypilot/selfdrive/assets/nav_right.png new file mode 100644 index 0000000000..36ff2ff4e6 --- /dev/null +++ b/sunnypilot/selfdrive/assets/nav_right.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a5ef8830e879dec513eee7bdab7dbb035b09fbccd8bcce61f0c8bf2c731380b8 +size 777 diff --git a/sunnypilot/selfdrive/assets/nav_straight.png b/sunnypilot/selfdrive/assets/nav_straight.png new file mode 100644 index 0000000000..83f5c52a79 --- /dev/null +++ b/sunnypilot/selfdrive/assets/nav_straight.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:74d5e2d8b69a997e2f8661145581277972718f17ead82c2401a62de0911731f5 +size 839