From 31c9f336234876f30bda6c1205df3c7e6ff5b196 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sun, 12 May 2024 03:23:44 -0700
Subject: [PATCH] Visuals - Developer UI - Border Metrics - Steering Torque

Display steering torque metrics in onroad UI border.
---
 selfdrive/ui/qt/onroad.cc | 27 +++++++++++++++++++++++++++
 selfdrive/ui/qt/onroad.h  |  2 ++
 selfdrive/ui/ui.cc        |  2 ++
 selfdrive/ui/ui.h         |  2 ++
 4 files changed, 33 insertions(+)

diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 2610e34..0ba2929 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -223,6 +223,33 @@ void OnroadWindow::paintEvent(QPaintEvent *event) {
   QRect rect = this->rect();
   p.fillRect(rect, QColor(bg.red(), bg.green(), bg.blue(), 255));
 
+  if (scene.show_steering) {
+    QLinearGradient gradient(rect.topLeft(), rect.bottomLeft());
+    gradient.setColorAt(0.0, bg_colors[STATUS_TRAFFIC_MODE_ACTIVE]);
+    gradient.setColorAt(0.15, bg_colors[STATUS_EXPERIMENTAL_MODE_ACTIVE]);
+    gradient.setColorAt(0.5, bg_colors[STATUS_CONDITIONAL_OVERRIDDEN]);
+    gradient.setColorAt(0.85, bg_colors[STATUS_ENGAGED]);
+    gradient.setColorAt(1.0, bg_colors[STATUS_ENGAGED]);
+
+    QBrush brush(gradient);
+    int fillWidth = UI_BORDER_SIZE;
+
+    steer = 0.10 * abs(scene.steer) + 0.90 * steer;
+    int visibleHeight = rect.height() * steer;
+
+    if (scene.steering_angle_deg < 0) {
+      QRect leftRect(rect.x(), rect.y() + rect.height() - visibleHeight, fillWidth, visibleHeight);
+      p.fillRect(leftRect, brush);
+      QRect leftRectHidden(rect.x(), rect.y(), fillWidth, rect.height() - visibleHeight);
+      p.fillRect(leftRectHidden, QColor(bg.red(), bg.green(), bg.blue(), 255));
+    } else if (scene.steering_angle_deg > 0 ) {
+      QRect rightRect(rect.x() + rect.width() - fillWidth, rect.y() + rect.height() - visibleHeight, fillWidth, visibleHeight);
+      p.fillRect(rightRect, brush);
+      QRect rightRectHidden(rect.x() + rect.width() - fillWidth, rect.y(), fillWidth, rect.height() - visibleHeight);
+      p.fillRect(rightRectHidden, QColor(bg.red(), bg.green(), bg.blue(), 255));
+    }
+  }
+
   if (scene.show_blind_spot) {
     auto getBlindspotColor = [&](bool turn_signal, int &frames) {
       if (turn_signal) {
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index 0935b01..e36097d 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -306,6 +306,8 @@ private:
   Params params;
   Params paramsMemory{"/dev/shm/params"};
 
+  float steer;
+
   QPoint timeoutPoint = QPoint(420, 69);
   QTimer clickTimer;
 
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index 3945230..c52fa44 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -213,6 +213,7 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("carControl")) {
     auto carControl = sm["carControl"].getCarControl();
+    scene.steer = carControl.getActuators().getSteer();
   }
   if (sm.updated("carParams")) {
     scene.longitudinal_control = sm["carParams"].getCarParams().getOpenpilotLongitudinalControl();
@@ -326,6 +327,7 @@ void ui_update_frogpilot_params(UIState *s) {
   bool developer_ui = params.getBool("DeveloperUI");
   bool border_metrics = developer_ui && params.getBool("BorderMetrics");
   scene.show_blind_spot = border_metrics && params.getBool("BlindSpotMetrics");
+  scene.show_steering = border_metrics && params.getBool("ShowSteering");
 
   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 a651f63..437f006 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -219,6 +219,7 @@ typedef struct UIScene {
   bool show_cem_status_bar;
   bool show_slc_offset;
   bool show_slc_offset_ui;
+  bool show_steering;
   bool speed_limit_changed;
   bool speed_limit_controller;
   bool speed_limit_overridden;
@@ -241,6 +242,7 @@ typedef struct UIScene {
   float speed_limit;
   float speed_limit_offset;
   float speed_limit_overridden_speed;
+  float steer;
   float unconfirmed_speed_limit;
 
   int alert_size;