From ae21d40a1994c26c7357cc85262c62b7c1727e01 Mon Sep 17 00:00:00 2001
From: Jason Wen <haibin.wen3@gmail.com>
Date: Wed, 8 Oct 2025 20:33:19 -0400
Subject: [PATCH] ui: Speedometer: Always display true speed (#1335)

* ui: Speedometer: Display True Speed

* update

* use stock one
---
 common/params_keys.h                              |  1 +
 selfdrive/ui/qt/onroad/hud.cc                     |  2 +-
 .../qt/offroad/settings/visuals_panel.cc          |  7 +++++++
 selfdrive/ui/sunnypilot/qt/onroad/hud.cc          | 15 +++++++++++++++
 selfdrive/ui/sunnypilot/qt/onroad/hud.h           |  1 +
 selfdrive/ui/sunnypilot/ui.cc                     |  1 +
 selfdrive/ui/sunnypilot/ui_scene.h                |  1 +
 7 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/common/params_keys.h b/common/params_keys.h
index b950f4334..1c7b9985c 100644
--- a/common/params_keys.h
+++ b/common/params_keys.h
@@ -168,6 +168,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc
index d95fe5fd8..540643bed 100644
--- a/selfdrive/ui/qt/onroad/hud.cc
+++ b/selfdrive/ui/qt/onroad/hud.cc
@@ -51,8 +51,8 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) {
   if (is_cruise_available) {
     drawSetSpeed(p, surface_rect);
   }
-#endif
   drawCurrentSpeed(p, surface_rect);
+#endif
 
   p.restore();
 }
diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
index 7c1107d65..98f563440 100644
--- a/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
+++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc
@@ -69,6 +69,13 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "",
       false,
     },
+    {
+      "TrueVEgoUI",
+      tr("Speedometer: Always Display True Speed"),
+      tr("Always display the true vehicle current speed from wheel speed sensors."),
+      "",
+      false,
+    },
   };
 
   // Add regular toggles first
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
index 21d66ffa9..e539f1c11 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.cc
@@ -115,6 +115,10 @@ void HudRendererSP::updateState(const UIState &s) {
 
   greenLightAlert = lp_sp.getE2eAlerts().getGreenLightAlert();
   leadDepartAlert = lp_sp.getE2eAlerts().getLeadDepartAlert();
+
+  // override stock current speed values
+  float v_ego = (v_ego_cluster_seen && !s.scene.trueVEgoUI) ? car_state.getVEgoCluster() : car_state.getVEgo();
+  speed = std::max<float>(0.0f, v_ego * (is_metric ? MS_TO_KPH : MS_TO_MPH));
 }
 
 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
@@ -127,6 +131,7 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
   if (is_cruise_available) {
     drawSetSpeedSP(p, surface_rect);
   }
+  drawCurrentSpeedSP(p, surface_rect);
 
   if (!reversing) {
     // Smart Cruise Control
@@ -731,3 +736,13 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect) {
   QPointF drawPoint = center - pixmapCenterOffset;
   p.drawPixmap(drawPoint, alert_img);
 }
+
+void HudRendererSP::drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect) {
+  QString speedStr = QString::number(std::nearbyint(speed));
+
+  p.setFont(InterFont(176, QFont::Bold));
+  HudRenderer::drawText(p, surface_rect.center().x(), 210, speedStr);
+
+  p.setFont(InterFont(66));
+  HudRenderer::drawText(p, surface_rect.center().x(), 290, is_metric ? tr("km/h") : tr("mph"), 200);
+}
diff --git a/selfdrive/ui/sunnypilot/qt/onroad/hud.h b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
index cf840ee3d..066a01c80 100644
--- a/selfdrive/ui/sunnypilot/qt/onroad/hud.h
+++ b/selfdrive/ui/sunnypilot/qt/onroad/hud.h
@@ -37,6 +37,7 @@ private:
   void drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect);
   void drawSetSpeedSP(QPainter &p, const QRect &surface_rect);
   void drawE2eAlert(QPainter &p, const QRect &surface_rect);
+  void drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect);
 
   bool lead_status;
   float lead_d_rel;
diff --git a/selfdrive/ui/sunnypilot/ui.cc b/selfdrive/ui/sunnypilot/ui.cc
index 7f4fc8823..30472ebdb 100644
--- a/selfdrive/ui/sunnypilot/ui.cc
+++ b/selfdrive/ui/sunnypilot/ui.cc
@@ -64,6 +64,7 @@ void ui_update_params_sp(UIStateSP *s) {
   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("RoadNameToggle");
+  s->scene.trueVEgoUI = params.getBool("TrueVEgoUI");
 
   // Onroad Screen Brightness
   s->scene.onroadScreenOffBrightness = std::atoi(params.get("OnroadScreenOffBrightness").c_str());
diff --git a/selfdrive/ui/sunnypilot/ui_scene.h b/selfdrive/ui/sunnypilot/ui_scene.h
index 7cfabb15c..6675916f3 100644
--- a/selfdrive/ui/sunnypilot/ui_scene.h
+++ b/selfdrive/ui/sunnypilot/ui_scene.h
@@ -15,4 +15,5 @@ typedef struct UISceneSP : UIScene {
   int onroadScreenOffBrightness, onroadScreenOffTimer = 0;
   bool onroadScreenOffControl;
   int onroadScreenOffTimerParam;
+  bool trueVEgoUI;
 } UISceneSP;