From 05029c814c2044c3c8cb93a8b55532a6464022cc Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Developer Sidebar --- frogpilot/common/frogpilot_variables.py | 27 +++ frogpilot/ui/frogpilot_ui.h | 1 + frogpilot/ui/qt/widgets/developer_sidebar.cc | 193 +++++++++++++++++++ frogpilot/ui/qt/widgets/developer_sidebar.h | 44 +++++ selfdrive/ui/SConscript | 2 +- selfdrive/ui/qt/home.cc | 11 ++ selfdrive/ui/qt/home.h | 4 + selfdrive/ui/qt/onroad/alerts.cc | 10 +- selfdrive/ui/qt/onroad/alerts.h | 2 + 9 files changed, 290 insertions(+), 4 deletions(-) create mode 100644 frogpilot/ui/qt/widgets/developer_sidebar.cc create mode 100644 frogpilot/ui/qt/widgets/developer_sidebar.h diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index c7ca7df1..5eba67b1 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -77,6 +77,25 @@ BUTTON_FUNCTIONS = { "TRAFFIC_MODE": 6 } +DEVELOPER_SIDEBAR_METRICS = { + "NONE": 0, + "ACCELERATION_CURRENT": 1, + "ACCELERATION_MAX": 2, + "AUTOTUNE_ACTUATOR_DELAY": 3, + "AUTOTUNE_FRICTION": 4, + "AUTOTUNE_LATERAL_ACCELERATION": 5, + "AUTOTUNE_STEER_RATIO": 6, + "AUTOTUNE_STIFFNESS_FACTOR": 7, + "ENGAGEMENT_LATERAL": 8, + "ENGAGEMENT_LONGITUDINAL": 9, + "LATERAL_STEERING_ANGLE": 10, + "LATERAL_TORQUE_USED": 11, + "LONGITUDINAL_ACTUATOR_ACCELERATION": 12, + "LONGITUDINAL_MPC_JERK_ACCELERATION": 13, + "LONGITUDINAL_MPC_JERK_DANGER_ZONE": 14, + "LONGITUDINAL_MPC_JERK_SPEED_CONTROL": 15, +} + EXCLUDED_KEYS = { "AvailableModelNames", "AvailableModels", @@ -395,6 +414,14 @@ class FrogPilotVariables: toggle.storage_left_metrics = self.get_value("ShowStorageLeft", condition=developer_metrics and not toggle.debug_mode) toggle.storage_used_metrics = self.get_value("ShowStorageUsed", condition=developer_metrics and not toggle.debug_mode) toggle.use_si_metrics = self.get_value("UseSI", condition=developer_metrics) or toggle.debug_mode + toggle.developer_sidebar = self.get_value("DeveloperSidebar", condition=toggle.developer_ui) or toggle.debug_mode + toggle.developer_sidebar_metric1 = self.get_value("DeveloperSidebarMetric1", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LONGITUDINAL_ACTUATOR_ACCELERATION"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric2 = self.get_value("DeveloperSidebarMetric2", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["ACCELERATION_CURRENT"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric3 = self.get_value("DeveloperSidebarMetric3", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LATERAL_STEERING_ANGLE"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric4 = self.get_value("DeveloperSidebarMetric4", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LATERAL_TORQUE_USED"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric5 = self.get_value("DeveloperSidebarMetric5", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LONGITUDINAL_MPC_JERK_ACCELERATION"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric6 = self.get_value("DeveloperSidebarMetric6", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LONGITUDINAL_MPC_JERK_DANGER_ZONE"] if toggle.debug_mode else None) + toggle.developer_sidebar_metric7 = self.get_value("DeveloperSidebarMetric7", cast=None, condition=toggle.developer_sidebar, default=DEVELOPER_SIDEBAR_METRICS["LONGITUDINAL_MPC_JERK_SPEED_CONTROL"] if toggle.debug_mode else None) developer_widgets = self.get_value("DeveloperWidgets", condition=toggle.developer_ui) toggle.adjacent_lead_tracking = has_radar and (self.get_value("AdjacentLeadsUI", condition=developer_widgets) or toggle.debug_mode) toggle.radar_tracks = has_radar and (self.get_value("RadarTracksUI", condition=developer_widgets) or toggle.debug_mode) diff --git a/frogpilot/ui/frogpilot_ui.h b/frogpilot/ui/frogpilot_ui.h index 09ef2052..c61c0bc8 100644 --- a/frogpilot/ui/frogpilot_ui.h +++ b/frogpilot/ui/frogpilot_ui.h @@ -13,6 +13,7 @@ struct FrogPilotUIScene { bool online; bool parked; bool reverse; + bool sidebars_open; bool standstill; int conditional_status; diff --git a/frogpilot/ui/qt/widgets/developer_sidebar.cc b/frogpilot/ui/qt/widgets/developer_sidebar.cc new file mode 100644 index 00000000..5056f4e3 --- /dev/null +++ b/frogpilot/ui/qt/widgets/developer_sidebar.cc @@ -0,0 +1,193 @@ +#include "frogpilot/ui/qt/widgets/developer_sidebar.h" + +void DeveloperSidebar::drawMetric(QPainter &p, const QPair &label, QColor c, int y) { + const QRect rect = {12, y, 275, 126}; + + p.setPen(Qt::NoPen); + p.setBrush(QBrush(c)); + p.setClipRect(rect.x() + rect.width() - 4 - 18, rect.y(), 18, rect.height(), Qt::ClipOperation::ReplaceClip); + p.drawRoundedRect(QRect(rect.x() + rect.width() - 4 - 100, rect.y() + 4, 100, 118), 18, 18); + p.setClipping(false); + + QPen pen = QPen(QColor(0xff, 0xff, 0xff, 0x55)); + pen.setWidth(2); + p.setPen(pen); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(rect, 20, 20); + + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setFont(InterFont(35, QFont::DemiBold)); + p.drawText(rect.adjusted(0, 0, -22, 0), Qt::AlignCenter, label.first + "\n" + label.second); +} + +DeveloperSidebar::DeveloperSidebar(QWidget *parent) : QFrame(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Expanding); + setFixedWidth(300); + + QObject::connect(frogpilotUIState(), &FrogPilotUIState::themeUpdated, this, &DeveloperSidebar::updateToggles); + QObject::connect(uiState(), &UIState::offroadTransition, this, &DeveloperSidebar::resetVariables); + QObject::connect(uiState(), &UIState::uiUpdate, this, &DeveloperSidebar::updateState); +} + +void DeveloperSidebar::showEvent(QShowEvent *event) { + updateToggles(); +} + +void DeveloperSidebar::updateToggles() { + FrogPilotUIState &fs = *frogpilotUIState(); + FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + QJsonObject &frogpilot_toggles = frogpilot_scene.frogpilot_toggles; + + metricAssignments.clear(); + for (int i = 1; i <= 7; ++i) { + QString key = QString("developer_sidebar_metric%1").arg(i); + int metricId = frogpilot_toggles.value(key).toInt(); + metricAssignments.push_back(metricId); + } + + metricColor = QColor(frogpilot_toggles.value("sidebar_color1").toString()); +} + +void DeveloperSidebar::resetVariables() { + lateralEngagementTime = 0; + longitudinalEngagementTime = 0; + maxAcceleration = 0; + totalEngagementTime = 0; +} + +void DeveloperSidebar::updateState(const UIState &s, const FrogPilotUIState &fs) { + if (!isVisible()) { + return; + } + + const SubMaster &sm = *(s.sm); + + const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; + const SubMaster &fpsm = *(fs.sm); + + const cereal::CarControl::Reader &carControl = fpsm["carControl"].getCarControl(); + const cereal::CarState::Reader &carState = sm["carState"].getCarState(); + const cereal::FrogPilotPlan::Reader &frogpilotPlan = fpsm["frogpilotPlan"].getFrogpilotPlan(); + const cereal::LiveDelayData::Reader &liveDelay = fpsm["liveDelay"].getLiveDelay(); + const cereal::LiveParametersData::Reader &liveParameters = fpsm["liveParameters"].getLiveParameters(); + const cereal::LiveTorqueParametersData::Reader &liveTorqueParameters = fpsm["liveTorqueParameters"].getLiveTorqueParameters(); + + const bool is_metric = s.scene.is_metric; + const bool use_si = frogpilot_scene.frogpilot_toggles.value("use_si_metrics").toBool(); + + const QString accelerationUnit = (is_metric || use_si) ? tr(" m/s²") : tr(" ft/s²"); + const float accelerationConversion = (is_metric || use_si) ? 1.0f : METER_TO_FOOT; + + double acceleration = carState.getAEgo() * accelerationConversion; + if (!carState.getGasPressed()) { + maxAcceleration = std::max(maxAcceleration, acceleration); + } + + lateralEngagementTime += carControl.getLatActive() && !frogpilot_scene.reverse && !frogpilot_scene.standstill ? 1 : 0; + longitudinalEngagementTime += carControl.getLongActive() && !frogpilot_scene.reverse && !frogpilot_scene.standstill ? 1 : 0; + totalEngagementTime += !(frogpilot_scene.reverse || frogpilot_scene.standstill) || totalEngagementTime == 0 ? 1 : 0; + + static int maxSteerAngle = 0; + int currentSteerAngle = fabs(carState.getSteeringAngleDeg()); + + static int maxTorque = 0; + int currentTorque = fabs(carControl.getActuators().getTorque() * 100); + + static QElapsedTimer torqueTimer; + + if (currentTorque >= 50) { + maxSteerAngle = std::max(maxSteerAngle, currentSteerAngle); + maxTorque = std::max(maxTorque, currentTorque); + + torqueTimer.start(); + } else if (torqueTimer.elapsed() >= 10000) { + maxTorque = 0; + maxSteerAngle = 0; + + torqueTimer.invalidate(); + } + + QString steerLabel = QString::number(currentSteerAngle) + "°"; + QString torqueLabel = QString::number(currentTorque) + "%"; + + if (currentTorque >= 50 || torqueTimer.isValid()) { + steerLabel += QString(" - (%1°)").arg(maxSteerAngle); + torqueLabel += QString(" - (%1%)").arg(maxTorque); + } + + accelerationStatus = ItemStatus(QPair(tr("ACCEL"), QString::number(acceleration, 'f', 2) + accelerationUnit), metricColor); + accelerationJerkStatus = ItemStatus(QPair(tr("ACCEL JERK"), QString::number(frogpilotPlan.getAccelerationJerk())), metricColor); + actuatorAccelerationStatus = ItemStatus(QPair(tr("ACT ACCEL"), QString::number(carControl.getActuators().getAccel() * accelerationConversion, 'f', 2) + accelerationUnit), metricColor); + dangerFactorStatus = ItemStatus(QPair(tr("DANGER %"), QString::number(frogpilotPlan.getDangerFactor() * 100.0f, 'f', 2) + "%"), metricColor); + dangerJerkStatus = ItemStatus(QPair(tr("DANGER JERK"), QString::number(frogpilotPlan.getDangerJerk())), metricColor); + delayStatus = ItemStatus(QPair(tr("STEER DELAY"), QString::number(liveDelay.getLateralDelay(), 'f', 5)), metricColor); + frictionStatus = ItemStatus(QPair(tr("FRICTION"), QString::number(liveTorqueParameters.getFrictionCoefficientFiltered(), 'f', 5)), metricColor); + latAccelStatus = ItemStatus(QPair(tr("LAT ACCEL"), QString::number(liveTorqueParameters.getLatAccelFactorFiltered(), 'f', 5)), metricColor); + lateralEngagementStatus = ItemStatus(QPair(tr("LATERAL %"), QString::number((lateralEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor); + longitudinalEngagementStatus = ItemStatus(QPair(tr("LONG %"), QString::number((longitudinalEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor); + maxAccelerationStatus = ItemStatus(QPair(tr("MAX ACCEL"), QString::number(maxAcceleration, 'f', 2) + accelerationUnit), metricColor); + speedJerkStatus = ItemStatus(QPair(tr("SPEED JERK"), QString::number(frogpilotPlan.getSpeedJerk())), metricColor); + steerAngleStatus = ItemStatus(QPair(tr("STEER ANGLE"), steerLabel), metricColor); + steerRatioStatus = ItemStatus(QPair(tr("STEER RATIO"), QString::number(liveParameters.getSteerRatio(), 'f', 5)), metricColor); + stiffnessFactorStatus = ItemStatus(QPair(tr("STEER STIFF"), QString::number(liveParameters.getStiffnessFactor(), 'f', 5)), metricColor); + torqueStatus = ItemStatus(QPair(tr("TORQUE %"), torqueLabel), metricColor); + + update(); +} + +void DeveloperSidebar::paintEvent(QPaintEvent *event) { + QPainter p(this); + p.setPen(Qt::NoPen); + p.setRenderHint(QPainter::Antialiasing); + + p.fillRect(rect(), QColor(57, 57, 57)); + + QMap metricMap; + metricMap.insert(1, &accelerationStatus); + metricMap.insert(2, &maxAccelerationStatus); + metricMap.insert(3, &delayStatus); + metricMap.insert(4, &frictionStatus); + metricMap.insert(5, &latAccelStatus); + metricMap.insert(6, &steerRatioStatus); + metricMap.insert(7, &stiffnessFactorStatus); + metricMap.insert(8, &lateralEngagementStatus); + metricMap.insert(9, &longitudinalEngagementStatus); + metricMap.insert(10, &steerAngleStatus); + metricMap.insert(11, &torqueStatus); + metricMap.insert(12, &actuatorAccelerationStatus); + metricMap.insert(13, &dangerFactorStatus); + metricMap.insert(14, &accelerationJerkStatus); + metricMap.insert(15, &dangerJerkStatus); + metricMap.insert(16, &speedJerkStatus); + + int count = 0; + for (size_t i = 0; i < metricAssignments.size(); ++i) { + if (metricAssignments[i] > 0 && metricMap.contains(metricAssignments[i])) { + count++; + } + } + if (count == 0) { + return; + } + + int metricHeight = 126; + int spacing = (height() - (count * metricHeight)) / (count + 1); + int y = spacing; + + for (size_t i = 0; i < metricAssignments.size(); ++i) { + int metricId = metricAssignments[i]; + + if (metricId == 0) { + continue; + } + + if (!metricMap.contains(metricId)) { + continue; + } + + ItemStatus *status = metricMap[metricId]; + drawMetric(p, status->first, status->second, y); + y += metricHeight + spacing; + } +} diff --git a/frogpilot/ui/qt/widgets/developer_sidebar.h b/frogpilot/ui/qt/widgets/developer_sidebar.h new file mode 100644 index 00000000..74d828ea --- /dev/null +++ b/frogpilot/ui/qt/widgets/developer_sidebar.h @@ -0,0 +1,44 @@ +#pragma once + +#include "selfdrive/ui/qt/sidebar.h" + +class DeveloperSidebar : public QFrame { + Q_OBJECT + +public: + explicit DeveloperSidebar(QWidget* parent = 0); + +private: + void drawMetric(QPainter &p, const QPair &label, QColor c, int y); + void paintEvent(QPaintEvent *event) override; + void resetVariables(); + void showEvent(QShowEvent *event); + void updateState(const UIState &s, const FrogPilotUIState &fs); + void updateToggles(); + + double lateralEngagementTime; + double longitudinalEngagementTime; + double maxAcceleration; + double totalEngagementTime; + + std::vector metricAssignments; + + QColor metricColor; + + ItemStatus accelerationJerkStatus; + ItemStatus accelerationStatus; + ItemStatus actuatorAccelerationStatus; + ItemStatus dangerFactorStatus; + ItemStatus dangerJerkStatus; + ItemStatus delayStatus; + ItemStatus frictionStatus; + ItemStatus latAccelStatus; + ItemStatus lateralEngagementStatus; + ItemStatus longitudinalEngagementStatus; + ItemStatus maxAccelerationStatus; + ItemStatus speedJerkStatus; + ItemStatus steerAngleStatus; + ItemStatus steerRatioStatus; + ItemStatus stiffnessFactorStatus; + ItemStatus torqueStatus; +}; diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index dcc185a6..487e0698 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -48,7 +48,7 @@ frogpilot_src = ["../../frogpilot/ui/frogpilot_ui.cc", "../../frogpilot/ui/qt/of "../../frogpilot/ui/qt/offroad/vehicle_settings.cc", "../../frogpilot/ui/qt/offroad/visual_settings.cc", "../../frogpilot/ui/qt/offroad/wheel_settings.cc", "../../frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc", "../../frogpilot/ui/qt/onroad/frogpilot_buttons.cc", "../../frogpilot/ui/qt/onroad/frogpilot_onroad.cc", - "../../frogpilot/ui/qt/widgets/drive_stats.cc", + "../../frogpilot/ui/qt/widgets/developer_sidebar.cc", "../../frogpilot/ui/qt/widgets/drive_stats.cc", "../../frogpilot/ui/qt/widgets/navigation_functions.cc"] qt_src += frogpilot_src diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index a7dfc5d8..80532608 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -46,6 +46,9 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { QObject::connect(uiState(), &UIState::offroadTransition, sidebar, &Sidebar::offroadTransition); // FrogPilot variables + developer_sidebar = new DeveloperSidebar(this); + main_layout->addWidget(developer_sidebar); + developer_sidebar->setVisible(false); } void HomeWindow::showSidebar(bool show) { @@ -64,6 +67,12 @@ void HomeWindow::updateState(const UIState &s, const FrogPilotUIState &fs) { // FrogPilot variables const FrogPilotUIScene &frogpilot_scene = fs.frogpilot_scene; const QJsonObject &frogpilot_toggles = frogpilot_scene.frogpilot_toggles; + + if (s.scene.started) { + developer_sidebar->setVisible(frogpilot_toggles.value("developer_sidebar").toBool()); + + frogpilotUIState()->frogpilot_scene.sidebars_open = developer_sidebar->isVisible() && sidebar->isVisible(); + } } void HomeWindow::offroadTransition(bool offroad) { @@ -78,6 +87,7 @@ void HomeWindow::offroadTransition(bool offroad) { slayout->setCurrentWidget(home); // FrogPilot variables + developer_sidebar->setVisible(false); } else { slayout->setCurrentWidget(onroad); } @@ -93,6 +103,7 @@ void HomeWindow::showDriverView(bool show) { sidebar->setVisible(show == false); // FrogPilot variables + developer_sidebar->setVisible(false); } void HomeWindow::mousePressEvent(QMouseEvent* e) { diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index d700ba07..18e98167 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -16,6 +16,8 @@ #include "selfdrive/ui/qt/widgets/offroad_alerts.h" #include "selfdrive/ui/ui.h" +#include "frogpilot/ui/qt/widgets/developer_sidebar.h" + class OffroadHome : public QFrame { Q_OBJECT @@ -72,6 +74,8 @@ private: QStackedLayout *slayout; // FrogPilot variables + DeveloperSidebar *developer_sidebar; + Params params; private slots: diff --git a/selfdrive/ui/qt/onroad/alerts.cc b/selfdrive/ui/qt/onroad/alerts.cc index 7704d04e..b316fe00 100644 --- a/selfdrive/ui/qt/onroad/alerts.cc +++ b/selfdrive/ui/qt/onroad/alerts.cc @@ -13,6 +13,7 @@ void OnroadAlerts::updateState(const UIState &s, const FrogPilotUIState &fs) { } // FrogPilot variables + sidebarsOpen = fs.frogpilot_scene.sidebars_open; } void OnroadAlerts::clear() { @@ -115,12 +116,15 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { p.setPen(QColor(0xff, 0xff, 0xff)); p.setRenderHint(QPainter::TextAntialiasing); if (alert.size == cereal::SelfdriveState::AlertSize::SMALL) { - p.setFont(InterFont(74, QFont::DemiBold)); + bool long_alert1 = alert.text1.length() > 40; + p.setFont(InterFont(long_alert1 && sidebarsOpen ? 64 : 74, QFont::DemiBold)); p.drawText(r, Qt::AlignCenter, alert.text1); } else if (alert.size == cereal::SelfdriveState::AlertSize::MID) { - p.setFont(InterFont(88, QFont::Bold)); + bool long_alert1 = alert.text1.length() > 30; + p.setFont(InterFont(long_alert1 && sidebarsOpen ? 78 : 88, QFont::Bold)); p.drawText(QRect(0, c.y() - 125, width(), 150), Qt::AlignHCenter | Qt::AlignTop, alert.text1); - p.setFont(InterFont(66)); + bool long_alert2 = alert.text2.length() > 40; + p.setFont(InterFont(long_alert2 && sidebarsOpen ? 56 : 66)); p.drawText(QRect(0, c.y() + 21, width(), 90), Qt::AlignHCenter, alert.text2); } else if (alert.size == cereal::SelfdriveState::AlertSize::FULL) { bool l = alert.text1.length() > 15; diff --git a/selfdrive/ui/qt/onroad/alerts.h b/selfdrive/ui/qt/onroad/alerts.h index 740e5548..d65aaf80 100644 --- a/selfdrive/ui/qt/onroad/alerts.h +++ b/selfdrive/ui/qt/onroad/alerts.h @@ -43,6 +43,8 @@ protected: Alert alert = {}; // FrogPilot variables + bool sidebarsOpen; + const QMap frogpilot_alert_colors = { {cereal::FrogPilotSelfdriveState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)}, {cereal::FrogPilotSelfdriveState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)},