From 21a6bb61d94cd5b14821f6c45ee56b76a99f1bd8 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 7 Jul 2024 05:46:38 -0700 Subject: [PATCH] Visuals - Developer UI - Longitudinal Metrics - Longitudinal Jerk --- selfdrive/ui/qt/onroad/onroad_home.cc | 33 +++++++++++++++++++++++++-- selfdrive/ui/qt/onroad/onroad_home.h | 8 +++++++ selfdrive/ui/ui.cc | 5 ++++ selfdrive/ui/ui.h | 5 ++++ 4 files changed, 49 insertions(+), 2 deletions(-) diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 813914d43..953bde332 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -84,23 +84,29 @@ void OnroadWindow::updateState(const UIState &s) { // FrogPilot variables const UIScene &scene = s.scene; + accelerationJerk = scene.acceleration_jerk; + accelerationJerkDifference = scene.acceleration_jerk_difference; blindSpotLeft = scene.blind_spot_left; blindSpotRight = scene.blind_spot_right; fps = scene.fps; friction = scene.friction; + hasLead = scene.has_lead; latAccel = scene.lat_accel; liveValid = scene.live_valid; showBlindspot = scene.show_blind_spot && (blindSpotLeft || blindSpotRight); showFPS = scene.show_fps; + showJerk = scene.show_jerk; showSignal = scene.show_signal && (turnSignalLeft || turnSignalRight); showSteering = scene.show_steering; showTuning = scene.show_tuning; + speedJerk = scene.speed_jerk; + speedJerkDifference = scene.speed_jerk_difference; steer = scene.steer; steeringAngleDeg = scene.steering_angle_deg; turnSignalLeft = scene.turn_signal_left; turnSignalRight = scene.turn_signal_right; - if (showBlindspot || showFPS || showSignal || showSteering || showTuning) { + if (showBlindspot || showFPS || (showJerk && hasLead) || showSignal || showSteering || showTuning) { shouldUpdate = true; } @@ -331,12 +337,29 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { } QString logicsDisplayString; + auto appendJerkInfo = [&](const QString &label, float value, float difference) { + logicsDisplayString += QString("%1: %2").arg(label).arg(value, 0, 'f', 3); + if (difference != 0) { + logicsDisplayString += QString(" (%1%2)").arg(difference > 0 ? "-" : "").arg(difference, 0, 'f', 3); + } + logicsDisplayString += " | "; + }; + + if (showJerk) { + appendJerkInfo("Acceleration Jerk", accelerationJerk, accelerationJerkDifference); + appendJerkInfo("Speed Jerk", speedJerk, speedJerkDifference); + } + 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.endsWith(" | ")) { + logicsDisplayString.chop(3); + } + if (!logicsDisplayString.isEmpty()) { p.setFont(InterFont(28, QFont::DemiBold)); p.setRenderHint(QPainter::TextAntialiasing); @@ -354,7 +377,13 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { for (int i = 0; i < subParts.size(); ++i) { QString text = subParts[i]; - if (text.startsWith("(") && i > 0) { + if (text.endsWith(")") && i > 0 && (subParts[i - 1].contains("Acceleration") || subParts[i - 1].contains("Speed"))) { + QString prefix = subParts[i - 1] + " ("; + p.drawText(currentX, logicsY, prefix); + currentX += p.fontMetrics().horizontalAdvance(prefix); + text.chop(1); + p.setPen(text.contains("-") ? redColor() : Qt::white); + } else if (text.startsWith("(") && i > 0) { p.drawText(currentX, logicsY, " ("); currentX += p.fontMetrics().horizontalAdvance(" ("); text = text.mid(1); diff --git a/selfdrive/ui/qt/onroad/onroad_home.h b/selfdrive/ui/qt/onroad/onroad_home.h index 593759a3a..9d4a2a256 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.h +++ b/selfdrive/ui/qt/onroad/onroad_home.h @@ -27,18 +27,24 @@ private: // FrogPilot variables bool blindSpotLeft; bool blindSpotRight; + bool hasLead; bool liveValid; bool showBlindspot; bool showFPS; + bool showJerk; bool showSignal; bool showSteering; bool showTuning; bool turnSignalLeft; bool turnSignalRight; + float accelerationJerk; + float accelerationJerkDifference; float fps; float friction; float latAccel; + float speedJerk; + float speedJerkDifference; float steer; int steeringAngleDeg; @@ -47,6 +53,8 @@ private: QTimer clickTimer; + inline QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } + Params params; Params paramsMemory{"/dev/shm/params"}; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b12022204..9a95be28e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -253,6 +253,8 @@ static void update_state(UIState *s) { } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); + scene.acceleration_jerk = frogpilotPlan.getAccelerationJerk(); + scene.acceleration_jerk_difference = frogpilotPlan.getAccelerationJerkStock() - scene.acceleration_jerk; scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); scene.desired_follow = frogpilotPlan.getDesiredFollowDistance(); scene.lane_width_left = frogpilotPlan.getLaneWidthLeft(); @@ -260,6 +262,8 @@ static void update_state(UIState *s) { scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance(); scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); scene.red_light = frogpilotPlan.getRedLight(); + scene.speed_jerk = frogpilotPlan.getSpeedJerk(); + scene.speed_jerk_difference = frogpilotPlan.getSpeedJerkStock() - scene.speed_jerk; scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); @@ -357,6 +361,7 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.show_tuning = show_lateral && scene.has_auto_tune && params.getBool("TuningInfo"); bool show_longitudinal = scene.longitudinal_control && developer_ui && params.getBool("LongitudinalMetrics"); scene.lead_info = show_longitudinal && params.getBool("LeadInfo"); + scene.show_jerk = show_longitudinal && params.getBool("JerkInfo"); 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 44d526173..f5fc3d598 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -161,6 +161,7 @@ typedef struct UIScene { bool show_blind_spot; bool show_cem_status_bar; bool show_fps; + bool show_jerk; bool show_signal; bool show_slc_offset; bool show_slc_offset_ui; @@ -185,6 +186,8 @@ typedef struct UIScene { double fps; float acceleration; + float acceleration_jerk; + float acceleration_jerk_difference; float adjusted_cruise; float friction; float lane_detection_width; @@ -192,6 +195,8 @@ typedef struct UIScene { float lane_width_right; float lat_accel; float lead_detection_threshold; + float speed_jerk; + float speed_jerk_difference; float speed_limit; float speed_limit_offset; float speed_limit_overridden_speed;