From 8e669aa4c791015abd7e5a87d1549b81def06df3 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 24 Jun 2024 20:51:34 -0700
Subject: [PATCH] Visuals - Developer UI - Lateral Metrics

Display various metrics related to the lateral performance of openpilot.
---
 selfdrive/ui/qt/onroad/onroad_home.cc | 45 ++++++++++++++++++++++++++-
 selfdrive/ui/qt/onroad/onroad_home.h  |  4 +++
 selfdrive/ui/ui.cc                    |  5 +++
 selfdrive/ui/ui.h                     |  5 +++
 4 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc
index 924d2333c..0f7b74206 100644
--- a/selfdrive/ui/qt/onroad/onroad_home.cc
+++ b/selfdrive/ui/qt/onroad/onroad_home.cc
@@ -87,16 +87,20 @@ void OnroadWindow::updateState(const UIState &s) {
   blindSpotLeft = scene.blind_spot_left;
   blindSpotRight = scene.blind_spot_right;
   fps = scene.fps;
+  friction = scene.friction;
+  latAccel = scene.lat_accel;
+  liveValid = scene.live_valid;
   showBlindspot = scene.show_blind_spot && (blindSpotLeft || blindSpotRight);
   showFPS = scene.show_fps;
   showSignal = scene.show_signal && (turnSignalLeft || turnSignalRight);
   showSteering = scene.show_steering;
+  showTuning = scene.show_tuning;
   steer = scene.steer;
   steeringAngleDeg = scene.steering_angle_deg;
   turnSignalLeft = scene.turn_signal_left;
   turnSignalRight = scene.turn_signal_right;
 
-  if (showBlindspot || showFPS || showSignal || showSteering) {
+  if (showBlindspot || showFPS || showSignal || showSteering || showTuning) {
     shouldUpdate = true;
   }
 
@@ -319,6 +323,45 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
     }
   }
 
+  QString logicsDisplayString;
+  if (showTuning) {
+    logicsDisplayString += liveValid
+        ? QString("Friction: %1 | Lateral Acceleration: %2").arg(friction, 0, 'f', 3).arg(latAccel, 0, 'f', 3)
+        : "Friction: Calculating... | Lateral Acceleration: Calculating...";
+  }
+
+  if (!logicsDisplayString.isEmpty()) {
+    p.setFont(InterFont(28, QFont::DemiBold));
+    p.setRenderHint(QPainter::TextAntialiasing);
+    p.setPen(Qt::white);
+
+    int logicsWidth = p.fontMetrics().horizontalAdvance(logicsDisplayString);
+    int logicsX = (rect.width() - logicsWidth) / 2;
+    int logicsY = rect.top() + 27;
+
+    QStringList parts = logicsDisplayString.split(" | ");
+    int currentX = logicsX;
+
+    for (const QString &part : parts) {
+      QStringList subParts = part.split(" ");
+      for (int i = 0; i < subParts.size(); ++i) {
+        QString text = subParts[i];
+
+        if (text.startsWith("(") && i > 0) {
+          p.drawText(currentX, logicsY, " (");
+          currentX += p.fontMetrics().horizontalAdvance(" (");
+          text = text.mid(1);
+          p.setPen(text.contains("-") ? redColor() : Qt::white);
+        } else {
+          p.setPen(Qt::white);
+        }
+
+        p.drawText(currentX, logicsY, text);
+        currentX += p.fontMetrics().horizontalAdvance(text + " ");
+      }
+    }
+  }
+
   if (showFPS) {
     qint64 currentMillis = QDateTime::currentMSecsSinceEpoch();
     static std::queue<std::pair<qint64, float>> fpsQueue;
diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h
index 3312b9d3d..593759a3a 100644
--- a/selfdrive/ui/qt/onroad/onroad_home.h
+++ b/selfdrive/ui/qt/onroad/onroad_home.h
@@ -27,14 +27,18 @@ private:
   // FrogPilot variables
   bool blindSpotLeft;
   bool blindSpotRight;
+  bool liveValid;
   bool showBlindspot;
   bool showFPS;
   bool showSignal;
   bool showSteering;
+  bool showTuning;
   bool turnSignalLeft;
   bool turnSignalRight;
 
   float fps;
+  float friction;
+  float latAccel;
   float steer;
 
   int steeringAngleDeg;
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index bc61f91ce..28d98d7d5 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -273,6 +273,9 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("liveTorqueParameters")) {
     auto liveTorqueParameters = sm["liveTorqueParameters"].getLiveTorqueParameters();
+    scene.friction = liveTorqueParameters.getFrictionCoefficientFiltered();
+    scene.lat_accel = liveTorqueParameters.getLatAccelFactorFiltered();
+    scene.live_valid = liveTorqueParameters.getLiveValid();
   }
   if (sm.updated("wideRoadCameraState")) {
     auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
@@ -344,6 +347,8 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.show_fps = developer_ui && params.getBool("FPSCounter");
   scene.show_signal = border_metrics && params.getBool("SignalMetrics");
   scene.show_steering = border_metrics && params.getBool("ShowSteering");
+  bool show_lateral = developer_ui && params.getBool("LateralMetrics");
+  scene.show_tuning = show_lateral && scene.has_auto_tune && params.getBool("TuningInfo");
 
   scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing");
   scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing");
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 928c2abb4..36995517d 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -137,8 +137,10 @@ typedef struct UIScene {
   bool enabled;
   bool experimental_mode;
   bool experimental_mode_via_screen;
+  bool has_auto_tune;
   bool has_lead;
   bool holiday_themes;
+  bool live_valid;
   bool map_open;
   bool online;
   bool onroad_distance_button;
@@ -160,6 +162,7 @@ typedef struct UIScene {
   bool show_slc_offset_ui;
   bool show_steering;
   bool show_stopping_point;
+  bool show_tuning;
   bool speed_limit_changed;
   bool speed_limit_controller;
   bool speed_limit_overridden;
@@ -177,9 +180,11 @@ typedef struct UIScene {
   double fps;
 
   float adjusted_cruise;
+  float friction;
   float lane_detection_width;
   float lane_width_left;
   float lane_width_right;
+  float lat_accel;
   float lead_detection_threshold;
   float road_curvature;
   float speed_limit;