diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 6f48cf399..813914d43 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; } @@ -326,6 +330,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> 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 e4173113f..5bd7cb6f5 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -272,6 +272,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(); @@ -345,6 +348,8 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { 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 03e23fa45..cb2218e3b 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -138,8 +138,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 model_randomizer; bool online; @@ -164,6 +166,7 @@ typedef struct UIScene { bool show_steering; bool show_stopping_point; bool show_stopping_point_metrics; + bool show_tuning; bool speed_limit_changed; bool speed_limit_controller; bool speed_limit_overridden; @@ -181,9 +184,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 speed_limit; float speed_limit_offset;