ui: Speedometer: Always display true speed (#1335)

* ui: Speedometer: Display True Speed

* update

* use stock one

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"}},

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();
 }

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

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);
+}

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;

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());

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;