Developer Sidebar

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)

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;

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<QString, QString> &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<QString, QString>(tr("ACCEL"), QString::number(acceleration, 'f', 2) + accelerationUnit), metricColor);
+  accelerationJerkStatus = ItemStatus(QPair<QString, QString>(tr("ACCEL JERK"), QString::number(frogpilotPlan.getAccelerationJerk())), metricColor);
+  actuatorAccelerationStatus = ItemStatus(QPair<QString, QString>(tr("ACT ACCEL"), QString::number(carControl.getActuators().getAccel() * accelerationConversion, 'f', 2) + accelerationUnit), metricColor);
+  dangerFactorStatus = ItemStatus(QPair<QString, QString>(tr("DANGER %"), QString::number(frogpilotPlan.getDangerFactor() * 100.0f, 'f', 2) + "%"), metricColor);
+  dangerJerkStatus = ItemStatus(QPair<QString, QString>(tr("DANGER JERK"), QString::number(frogpilotPlan.getDangerJerk())), metricColor);
+  delayStatus = ItemStatus(QPair<QString, QString>(tr("STEER DELAY"), QString::number(liveDelay.getLateralDelay(), 'f', 5)), metricColor);
+  frictionStatus = ItemStatus(QPair<QString, QString>(tr("FRICTION"), QString::number(liveTorqueParameters.getFrictionCoefficientFiltered(), 'f', 5)), metricColor);
+  latAccelStatus = ItemStatus(QPair<QString, QString>(tr("LAT ACCEL"), QString::number(liveTorqueParameters.getLatAccelFactorFiltered(), 'f', 5)), metricColor);
+  lateralEngagementStatus = ItemStatus(QPair<QString, QString>(tr("LATERAL %"), QString::number((lateralEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor);
+  longitudinalEngagementStatus = ItemStatus(QPair<QString, QString>(tr("LONG %"), QString::number((longitudinalEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor);
+  maxAccelerationStatus = ItemStatus(QPair<QString, QString>(tr("MAX ACCEL"), QString::number(maxAcceleration, 'f', 2) + accelerationUnit), metricColor);
+  speedJerkStatus = ItemStatus(QPair<QString, QString>(tr("SPEED JERK"), QString::number(frogpilotPlan.getSpeedJerk())), metricColor);
+  steerAngleStatus = ItemStatus(QPair<QString, QString>(tr("STEER ANGLE"), steerLabel), metricColor);
+  steerRatioStatus = ItemStatus(QPair<QString, QString>(tr("STEER RATIO"), QString::number(liveParameters.getSteerRatio(), 'f', 5)), metricColor);
+  stiffnessFactorStatus = ItemStatus(QPair<QString, QString>(tr("STEER STIFF"), QString::number(liveParameters.getStiffnessFactor(), 'f', 5)), metricColor);
+  torqueStatus = ItemStatus(QPair<QString, QString>(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<int, ItemStatus*> 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;
+  }
+}

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<QString, QString> &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<int> 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;
+};

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
 

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) {

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:

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;

selfdrive/ui/qt/onroad/alerts.h

@@ -43,6 +43,8 @@ protected:
   Alert alert = {};
 
   // FrogPilot variables
+  bool sidebarsOpen;
+
   const QMap<cereal::FrogPilotSelfdriveState::AlertStatus, QColor> frogpilot_alert_colors = {
     {cereal::FrogPilotSelfdriveState::AlertStatus::NORMAL, QColor(0x15, 0x15, 0x15, 0xf1)},
     {cereal::FrogPilotSelfdriveState::AlertStatus::USER_PROMPT, QColor(0xDA, 0x6F, 0x25, 0xf1)},