From 119e63952b3d1c52bf2fca5e6e2ad6e36df5bb50 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sat, 11 May 2024 14:05:58 -0700
Subject: [PATCH] Visuals - Developer UI - Lateral Metrics

Display various metrics related to the lateral performance of openpilot.
---
 selfdrive/ui/qt/onroad.cc | 27 +++++++++++++++++++++++++++
 selfdrive/ui/ui.cc        |  4 ++++
 selfdrive/ui/ui.h         |  5 +++++
 3 files changed, 36 insertions(+)

diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index a1814ce..7928362 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -351,6 +351,33 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
 
     update();
   }
+
+  QString logicsDisplayString = QString();
+  if (scene.show_tuning) {
+    if (!scene.live_valid) {
+      logicsDisplayString += "Friction: Calculating... | Lateral Acceleration: Calculating...";
+    } else {
+      logicsDisplayString += QString("Friction: %1 | Lateral Acceleration: %2")
+        .arg(scene.friction, 0, 'f', 3)
+        .arg(scene.lat_accel, 0, 'f', 3);
+    }
+  }
+  if (logicsDisplayString.endsWith(" | ")) {
+    logicsDisplayString.chop(3);
+  }
+
+  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;
+
+    p.drawText(logicsX, logicsY, logicsDisplayString);
+    update();
+  }
 }
 
 // ***** onroad widgets *****
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index e24ae1f..d1d3bc7 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -270,6 +270,9 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("liveTorqueParameters")) {
     auto torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
+    scene.friction = torque_params.getFrictionCoefficientFiltered();
+    scene.lat_accel = torque_params.getLatAccelFactorFiltered();
+    scene.live_valid = torque_params.getLiveValid();
   }
   if (sm.updated("wideRoadCameraState")) {
     auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
@@ -330,6 +333,7 @@ void ui_update_frogpilot_params(UIState *s) {
   scene.show_signal = border_metrics && params.getBool("SignalMetrics");
   scene.show_steering = border_metrics && params.getBool("ShowSteering");
   scene.fps_counter = developer_ui && params.getBool("FPSCounter");
+  scene.show_tuning = developer_ui && scene.has_auto_tune && params.getBool("LateralMetrics");
 
   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 c67c071..c520966 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -203,7 +203,9 @@ typedef struct UIScene {
   bool experimental_mode;
   bool experimental_mode_via_screen;
   bool fps_counter;
+  bool has_auto_tune;
   bool holiday_themes;
+  bool live_valid;
   bool map_open;
   bool online;
   bool onroad_distance_button;
@@ -222,6 +224,7 @@ typedef struct UIScene {
   bool show_slc_offset;
   bool show_slc_offset_ui;
   bool show_steering;
+  bool show_tuning;
   bool speed_limit_changed;
   bool speed_limit_controller;
   bool speed_limit_overridden;
@@ -237,9 +240,11 @@ typedef struct UIScene {
   bool vtsc_controlling_curve;
 
   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 speed_limit;
   float speed_limit_offset;