Display openpilot logics in the onroad UI

Added toggle to show openpilot's logic in regards to following lead vehicles and the acceleration in the onroad UI.

cereal/custom.capnp

@@ -26,12 +26,20 @@ struct FrogPilotNavigation @0xda96579883444c35 {
 }
 
 struct FrogPilotPlan @0x80ae746ee2596b11 {
+  accelerationJerk @0 :Float32;
+  accelerationJerkStock @1 :Float32;
   conditionalExperimental @3 :Bool;
+  desiredFollowDistance @4 :Int16;
+  egoJerk @5 :Float32;
+  egoJerkStock @6 :Float32;
   jerk @7 :Float32;
   laneWidthLeft @8 :Float32;
   laneWidthRight @9 :Float32;
   minAcceleration @10 :Float32;
   maxAcceleration @11 :Float32;
+  safeObstacleDistance @13 :Int16;
+  safeObstacleDistanceStock @14 :Int16;
+  stoppedEquivalenceFactor @19 :Int16;
   tFollow @20 :Float32;
   vCruise @22 :Float32;
 }

common/params.cc

@@ -247,6 +247,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CustomTheme", PERSISTENT},
     {"CydiaTune", PERSISTENT},
     {"DecelerationProfile", PERSISTENT},
+    {"DeveloperUI", PERSISTENT},
     {"DeviceManagement", PERSISTENT},
     {"DeviceShutdown", PERSISTENT},
     {"DisableOnroadUploads", PERSISTENT},
@@ -264,6 +265,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"IncreaseThermalLimits", PERSISTENT},
     {"LaneLinesWidth", PERSISTENT},
     {"LateralTune", PERSISTENT},
+    {"LeadInfo", PERSISTENT},
     {"LongitudinalTune", PERSISTENT},
     {"LowVoltageShutdown", PERSISTENT},
     {"ManualUpdateInitiated", PERSISTENT},
@@ -284,13 +286,16 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RoadEdgesWidth", PERSISTENT},
     {"ShowCPU", PERSISTENT},
     {"ShowGPU", PERSISTENT},
+    {"ShowJerk", PERSISTENT},
     {"ShowMemoryUsage", PERSISTENT},
     {"ShowStorageLeft", PERSISTENT},
     {"ShowStorageUsed", PERSISTENT},
+    {"ShowTuning", PERSISTENT},
     {"StandardFollow", PERSISTENT},
     {"StandardJerk", PERSISTENT},
     {"StockTune", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
+    {"UseSI", PERSISTENT},
     {"WarningImmediateVolume", PERSISTENT},
     {"WarningSoftVolume", PERSISTENT},
 };

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -9,7 +9,8 @@ from openpilot.common.params import Params
 from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
-from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import A_CHANGE_COST, J_EGO_COST, COMFORT_BRAKE, STOP_DISTANCE, get_jerk_factor, \
+                                                                           get_safe_obstacle_distance, get_stopped_equivalence_factor, get_T_FOLLOW
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
 
 from openpilot.system.version import get_short_branch
@@ -92,8 +93,14 @@ class FrogPilotPlanner:
     if radarState.leadOne.status and self.CP.openpilotLongitudinalControl:
       base_jerk = get_jerk_factor(self.custom_personalities, self.aggressive_jerk, self.standard_jerk, self.relaxed_jerk, controlsState.personality)
       base_t_follow = get_T_FOLLOW(self.custom_personalities, self.aggressive_follow, self.standard_follow, self.relaxed_follow, controlsState.personality)
+      self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
+      self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, base_t_follow)))
+      self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(v_lead)))
       self.jerk, self.t_follow = self.update_follow_values(base_jerk, radarState, base_t_follow, v_ego, v_lead)
     else:
+      self.safe_obstacle_distance = 0
+      self.safe_obstacle_distance_stock = 0
+      self.stopped_equivalence_factor = 0
       self.t_follow = 1.45
 
     self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego)
@@ -138,8 +145,16 @@ class FrogPilotPlanner:
     frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState'])
     frogpilotPlan = frogpilot_plan_send.frogpilotPlan
 
+    frogpilotPlan.accelerationJerk = A_CHANGE_COST * (float(self.jerk) if self.lead_one.status else 1)
+    frogpilotPlan.accelerationJerkStock = A_CHANGE_COST
     frogpilotPlan.conditionalExperimental = self.cem.experimental_mode
+    frogpilotPlan.desiredFollowDistance = self.safe_obstacle_distance - self.stopped_equivalence_factor
+    frogpilotPlan.egoJerk = J_EGO_COST * (float(self.jerk) if self.lead_one.status else 1)
+    frogpilotPlan.egoJerkStock = J_EGO_COST
     frogpilotPlan.jerk = float(self.jerk)
+    frogpilotPlan.safeObstacleDistance = self.safe_obstacle_distance
+    frogpilotPlan.safeObstacleDistanceStock = self.safe_obstacle_distance_stock
+    frogpilotPlan.stoppedEquivalenceFactor = self.stopped_equivalence_factor
     frogpilotPlan.laneWidthLeft = self.lane_width_left
     frogpilotPlan.laneWidthRight = self.lane_width_right
     frogpilotPlan.minAcceleration = self.min_accel

selfdrive/frogpilot/ui/qt/offroad/control_settings.cc

@@ -318,6 +318,8 @@ void FrogPilotControlsPanel::updateCarToggles() {
     auto carFingerprint = CP.getCarFingerprint();
     auto carName = CP.getCarName();
 
+    hasAutoTune = (carName == "hyundai" || carName == "toyota") && CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE;
+    uiState()->scene.has_auto_tune = hasAutoTune;
     hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal");
     hasPCMCruise = CP.getPcmCruise();
     isToyota = carName == "toyota";

selfdrive/frogpilot/ui/qt/offroad/visual_settings.cc

@@ -1,6 +1,9 @@
 #include "selfdrive/frogpilot/ui/qt/offroad/visual_settings.h"
 
 FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilotListWidget(parent) {
+  std::string branch = params.get("GitBranch");
+  isRelease = branch == "FrogPilot";
+
   const std::vector<std::tuple<QString, QString, QString, QString>> visualToggles {
     {"AlertVolumeControl", tr("Alert Volume Controller"), tr("Control the volume level for each individual sound in openpilot."), "../frogpilot/assets/toggle_icons/icon_mute.png"},
     {"DisengageVolume", tr("Disengage Volume"), tr("Related alerts:\n\nAdaptive Cruise Disabled\nParking Brake Engaged\nBrake Pedal Pressed\nSpeed too Low"), ""},
@@ -14,6 +17,8 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
     {"CustomAlerts", tr("Custom Alerts"), tr("Enable custom alerts for openpilot events."), "../frogpilot/assets/toggle_icons/icon_green_light.png"},
 
     {"CustomUI", tr("Custom Onroad UI"), tr("Customize the Onroad UI."), "../assets/offroad/icon_road.png"},
+    {"DeveloperUI", tr("Developer UI"), tr("Get various detailed information of what openpilot is doing behind the scenes."), ""},
+    {"LeadInfo", tr("Lead Info and Logics"), tr("Get detailed information about the vehicle ahead, including speed and distance, and the logic behind your following distance."), ""},
     {"CustomPaths", tr("Paths"), tr("Show your projected acceleration on the driving path, detected adjacent lanes, or when a vehicle is detected in your blindspot."), ""},
 
     {"CustomTheme", tr("Custom Themes"), tr("Enable the ability to use custom themes."), "../frogpilot/assets/wheel_images/frog.png"},
@@ -99,10 +104,26 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot
         for (auto &[key, toggle] : toggles) {
           std::set<QString> modifiedCustomOnroadUIKeys = customOnroadUIKeys;
 
+          if (!hasOpenpilotLongitudinal && !hasAutoTune || isRelease) {
+            modifiedCustomOnroadUIKeys.erase("DeveloperUI");
+          }
+
+          if (!hasOpenpilotLongitudinal || !isRelease) {
+            modifiedCustomOnroadUIKeys.erase("LeadInfo");
+          }
+
           toggle->setVisible(modifiedCustomOnroadUIKeys.find(key.c_str()) != modifiedCustomOnroadUIKeys.end());
         }
       });
       toggle = customUIToggle;
+    } else if (param == "DeveloperUI") {
+      std::vector<QString> developerUIToggles{"LeadInfo", "ShowTuning", "ShowJerk", "UseSI"};
+      std::vector<QString> developerUIToggleNames{tr("Lead Info"), tr("Live Tuning"), tr("Jerk"), tr("Use SI")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, developerUIToggles, developerUIToggleNames);
+    } else if (param == "LeadInfo") {
+      std::vector<QString> leadInfoToggles{"UseSI"};
+      std::vector<QString> leadInfoToggleNames{tr("Use SI Values")};
+      toggle = new FrogPilotParamToggleControl(param, title, desc, icon, leadInfoToggles, leadInfoToggleNames);
     } else if (param == "CustomPaths") {
       std::vector<QString> pathToggles{"AccelerationPath", "BlindSpotPath"};
       std::vector<QString> pathToggleNames{tr("Acceleration"), tr("Blind Spot")};
@@ -204,7 +225,9 @@ void FrogPilotVisualsPanel::updateCarToggles() {
     AlignedBuffer aligned_buf;
     capnp::FlatArrayMessageReader cmsg(aligned_buf.align(carParams.data(), carParams.size()));
     cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+    auto carName = CP.getCarName();
 
+    hasAutoTune = (carName == "hyundai" || carName == "toyota") && CP.getLateralTuning().which() == cereal::CarParams::LateralTuning::TORQUE;
     hasBSM = CP.getEnableBsm();
     hasOpenpilotLongitudinal = CP.getOpenpilotLongitudinalControl() && !params.getBool("DisableOpenpilotLongitudinal");
   } else {

selfdrive/frogpilot/ui/qt/offroad/visual_settings.h

@@ -25,7 +25,7 @@ private:
 
   std::set<QString> alertVolumeControlKeys = {"DisengageVolume", "EngageVolume", "PromptDistractedVolume", "PromptVolume", "RefuseVolume", "WarningImmediateVolume", "WarningSoftVolume"};
   std::set<QString> customAlertsKeys = {};
-  std::set<QString> customOnroadUIKeys = {"CustomPaths"};
+  std::set<QString> customOnroadUIKeys = {"CustomPaths", "DeveloperUI", "LeadInfo"};
   std::set<QString> customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"};
   std::set<QString> modelUIKeys = {"DynamicPathWidth", "HideLeadMarker", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"};
   std::set<QString> qolKeys = {"CameraView"};
@@ -36,8 +36,10 @@ private:
   Params params;
   Params paramsMemory{"/dev/shm/params"};
 
+  bool hasAutoTune;
   bool hasBSM;
   bool hasOpenpilotLongitudinal;
   bool isMetric = params.getBool("IsMetric");
+  bool isRelease;
   bool started;
 };

selfdrive/ui/qt/onroad.cc

@@ -148,6 +148,43 @@ void OnroadWindow::primeChanged(bool prime) {
 void OnroadWindow::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   p.fillRect(rect(), QColor(bg.red(), bg.green(), bg.blue(), 255));
+
+  QString logicsDisplayString = QString();
+  if (scene.show_jerk) {
+    logicsDisplayString += QString("Acceleration Jerk: %1 (%2%3) | Speed Jerk: %4 (%5%6) | ")
+      .arg(scene.acceleration_jerk, 0, 'f', 3)
+      .arg(scene.acceleration_jerk_difference > 0 ? "-" : "", 0)
+      .arg(abs(scene.acceleration_jerk_difference), 0, 'f', 3)
+      .arg(scene.ego_jerk, 0, 'f', 3)
+      .arg(scene.ego_jerk_difference > 0 ? "-" : "", 0)
+      .arg(abs(scene.ego_jerk_difference), 0, 'f', 3);
+  }
+  if (scene.show_tuning) {
+    if (!scene.live_valid) {
+      logicsDisplayString += "Friction: Calculating... | Lateral Acceleration: Calculating...";
+    } else {
+      logicsDisplayString += QString("Friction: %1 | Lateral Acceleration: %2")
+        .arg(scene.friction, 0, 'f', 3)
+        .arg(scene.lat_accel, 0, 'f', 3);
+    }
+  }
+  if (logicsDisplayString.endsWith(" | ")) {
+    logicsDisplayString.chop(3);
+  }
+
+  if (!logicsDisplayString.isEmpty()) {
+    p.setFont(InterFont(28, QFont::DemiBold));
+    p.setRenderHint(QPainter::TextAntialiasing);
+    p.setPen(Qt::white);
+
+    QRect currentRect = rect();
+    int logicsWidth = p.fontMetrics().horizontalAdvance(logicsDisplayString);
+    int logicsX = (currentRect.width() - logicsWidth) / 2;
+    int logicsY = currentRect.top() + 27;
+
+    p.drawText(logicsX, logicsY, logicsDisplayString);
+    update();
+  }
 }
 
 // ***** onroad widgets *****
@@ -221,7 +258,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
 
 // ExperimentalButton
 ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) {
-  setFixedSize(btn_size, btn_size);
+  setFixedSize(btn_size, btn_size + 10);
 
   engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size});
   experimental_img = loadPixmap("../assets/img_experimental.svg", {img_size, img_size});
@@ -236,7 +273,7 @@ void ExperimentalButton::changeMode() {
   }
 }
 
-void ExperimentalButton::updateState(const UIState &s) {
+void ExperimentalButton::updateState(const UIState &s, bool leadInfo) {
   const auto cs = (*s.sm)["controlsState"].getControlsState();
   bool eng = cs.getEngageable() || cs.getEnabled() || scene.always_on_lateral_active;
   if ((cs.getExperimentalMode() != experimental_mode) || (eng != engageable)) {
@@ -246,12 +283,13 @@ void ExperimentalButton::updateState(const UIState &s) {
   }
 
   // FrogPilot variables
+  y_offset = leadInfo ? 10 : 0;
 }
 
 void ExperimentalButton::paintEvent(QPaintEvent *event) {
   QPainter p(this);
   QPixmap img = experimental_mode ? experimental_img : engage_img;
-  drawIcon(p, QPoint(btn_size / 2, btn_size / 2), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
+  drawIcon(p, QPoint(btn_size / 2, btn_size / 2 + y_offset), img, QColor(0, 0, 0, 166), (isDown() || !engageable) ? 0.6 : 1.0);
 }
 
 
@@ -327,7 +365,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   status = s.status;
 
   // update engageability/experimental mode button
-  experimental_btn->updateState(s);
+  experimental_btn->updateState(s, leadInfo);
 
   // update DM icon
   auto dm_state = sm["driverMonitoringState"].getDriverMonitoringState();
@@ -700,6 +738,24 @@ void AnnotatedCameraWidget::drawLead(QPainter &painter, const cereal::RadarState
   }
   painter.drawPolygon(chevron, std::size(chevron));
 
+  if (leadInfo) {
+    float lead_speed = std::max(lead_data.getVLead(), 0.0f);
+
+    painter.setPen(Qt::white);
+    painter.setFont(InterFont(35, QFont::Bold));
+
+    QString text = QString("%1 %2 | %3 %4")
+                      .arg(qRound(d_rel * distanceConversion))
+                      .arg(leadDistanceUnit)
+                      .arg(qRound(lead_speed * speedConversion))
+                      .arg(leadSpeedUnit);
+
+    QFontMetrics metrics(painter.font());
+    int middle_x = (chevron[2].x() + chevron[0].x()) / 2;
+    int textWidth = metrics.horizontalAdvance(text);
+    painter.drawText(middle_x - textWidth / 2, chevron[0].y() + metrics.height() + 5, text);
+  }
+
   painter.restore();
 }
 
@@ -854,6 +910,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
 
   experimentalMode = scene.experimental_mode;
 
+  leadInfo = scene.lead_info;
+  obstacleDistance = scene.obstacle_distance;
+  obstacleDistanceStock = scene.obstacle_distance_stock;
+
   mapOpen = scene.map_open;
 
   turnSignalLeft = scene.turn_signal_left;
@@ -904,6 +964,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
     animationTimer->stop();
   }
 
+  if (leadInfo) {
+    drawLeadInfo(p);
+  }
+
   map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled());
   if (map_settings_btn_bottom->isEnabled()) {
     map_settings_btn_bottom->setVisible(!hideBottomIcons);
@@ -911,6 +975,107 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) {
   }
 }
 
+void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
+  static QElapsedTimer timer;
+  static bool isFiveSecondsPassed = false;
+  static double maxAcceleration = 0.0;
+  constexpr int maxAccelDuration = 5000;
+
+  QString accelerationUnit = tr(" m/s²");
+  leadDistanceUnit = tr(mapOpen ? "m" : "meters");
+  leadSpeedUnit = tr("m/s");
+
+  float accelerationConversion = 1.0f;
+  distanceConversion = 1.0f;
+  speedConversion = 1.0f;
+
+  if (!scene.use_si) {
+    if (is_metric) {
+      leadSpeedUnit = tr("kph");
+      speedConversion = MS_TO_KPH;
+    } else {
+      accelerationUnit = tr(" ft/s²");
+      leadDistanceUnit = tr(mapOpen ? "ft" : "feet");
+      leadSpeedUnit = tr("mph");
+
+      accelerationConversion = METER_TO_FOOT;
+      distanceConversion = METER_TO_FOOT;
+      speedConversion = MS_TO_MPH;
+    }
+  }
+
+  double acceleration = std::round(scene.acceleration * 100) / 100;
+
+  if (acceleration > maxAcceleration && status == STATUS_ENGAGED) {
+    maxAcceleration = acceleration;
+    isFiveSecondsPassed = false;
+    timer.start();
+  } else {
+    isFiveSecondsPassed = timer.hasExpired(maxAccelDuration);
+  }
+
+  auto createText = [&](const QString &title, const double data) {
+    return title + QString::number(std::round(data * distanceConversion)) + " " + leadDistanceUnit;
+  };
+
+  QString accelText = QString(tr("Accel: %1%2"))
+                      .arg(acceleration * accelerationConversion, 0, 'f', 2)
+                      .arg(accelerationUnit);
+
+  QString maxAccSuffix;
+  if (!mapOpen) {
+    maxAccSuffix = tr(" - Max: %1%2")
+                      .arg(maxAcceleration * accelerationConversion, 0, 'f', 2)
+                      .arg(accelerationUnit);
+  }
+
+  QString obstacleText = createText(mapOpen ? tr(" | Obstacle: ") : tr("  |  Obstacle Factor: "), obstacleDistance);
+  QString stopText = createText(mapOpen ? tr(" - Stop: ") : tr("  -  Stop Factor: "), scene.stopped_equivalence);
+  QString followText = " = " + createText(mapOpen ? tr("Follow: ") : tr("Follow Distance: "), scene.desired_follow);
+
+  auto createDiffText = [&](const double data, const double stockData) {
+    double difference = std::round((data - stockData) * distanceConversion);
+    return difference != 0 ? QString(" (%1%2)").arg(difference > 0 ? "+" : "").arg(difference) : QString();
+  };
+
+  p.save();
+
+  QRect insightsRect(rect().left() - 1, rect().top() - 60, rect().width() + 2, 100);
+  p.setBrush(QColor(0, 0, 0, 150));
+  p.drawRoundedRect(insightsRect, 30, 30);
+  p.setFont(InterFont(28, QFont::Bold));
+  p.setRenderHint(QPainter::TextAntialiasing);
+
+  QRect adjustedRect = insightsRect.adjusted(0, 27, 0, 27);
+  int textBaseLine = adjustedRect.y() + (adjustedRect.height() + p.fontMetrics().height()) / 2 - p.fontMetrics().descent();
+
+  int totalTextWidth = p.fontMetrics().horizontalAdvance(accelText)
+                     + p.fontMetrics().horizontalAdvance(maxAccSuffix)
+                     + p.fontMetrics().horizontalAdvance(obstacleText)
+                     + p.fontMetrics().horizontalAdvance(createDiffText(obstacleDistance, obstacleDistanceStock))
+                     + p.fontMetrics().horizontalAdvance(stopText)
+                     + p.fontMetrics().horizontalAdvance(followText);
+
+  int textStartPos = adjustedRect.x() + (adjustedRect.width() - totalTextWidth) / 2;
+
+  auto drawText = [&](const QString &text, const QColor &color) {
+    p.setPen(color);
+    p.drawText(textStartPos, textBaseLine, text);
+    textStartPos += p.fontMetrics().horizontalAdvance(text);
+  };
+
+  drawText(accelText, Qt::white);
+  if (!maxAccSuffix.isEmpty()) {
+    drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red);
+  }
+  drawText(obstacleText, Qt::white);
+  drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red);
+  drawText(stopText, Qt::white);
+  drawText(followText, Qt::white);
+
+  p.restore();
+}
+
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {
   p.save();
 

selfdrive/ui/qt/onroad.h

@@ -39,7 +39,7 @@ class ExperimentalButton : public QPushButton {
 
 public:
   explicit ExperimentalButton(QWidget *parent = 0);
-  void updateState(const UIState &s);
+  void updateState(const UIState &s, bool leadInfo);
 
 private:
   void paintEvent(QPaintEvent *event) override;
@@ -54,6 +54,8 @@ private:
   // FrogPilot variables
   Params paramsMemory{"/dev/shm/params"};
   UIScene &scene;
+
+  int y_offset;
 };
 
 
@@ -110,6 +112,7 @@ private:
   void paintFrogPilotWidgets(QPainter &p);
   void updateFrogPilotWidgets();
 
+  void drawLeadInfo(QPainter &p);
   void drawStatusBar(QPainter &p);
   void drawTurnSignals(QPainter &p);
 
@@ -123,19 +126,28 @@ private:
   bool blindSpotLeft;
   bool blindSpotRight;
   bool experimentalMode;
+  bool leadInfo;
   bool mapOpen;
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
   bool turnSignalLeft;
   bool turnSignalRight;
 
+  float distanceConversion;
+  float speedConversion;
+
   int alertSize;
   int cameraView;
   int conditionalStatus;
   int customColors;
   int customSignals;
+  int obstacleDistance;
+  int obstacleDistanceStock;
   int totalFrames = 8;
 
+  QString leadDistanceUnit;
+  QString leadSpeedUnit;
+
   size_t animationFrameIndex;
 
   std::unordered_map<int, std::tuple<QString, QColor, std::map<double, QBrush>>> themeConfiguration;

selfdrive/ui/ui.cc

@@ -244,12 +244,26 @@ 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.desired_follow = frogpilotPlan.getDesiredFollowDistance();
+    scene.ego_jerk = frogpilotPlan.getEgoJerk();
+    scene.ego_jerk_difference = frogpilotPlan.getEgoJerkStock() - scene.ego_jerk;
     scene.lane_width_left = frogpilotPlan.getLaneWidthLeft();
     scene.lane_width_right = frogpilotPlan.getLaneWidthRight();
+    scene.obstacle_distance = frogpilotPlan.getSafeObstacleDistance();
+    scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock();
+    scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor();
   }
   if (sm.updated("liveLocationKalman")) {
     auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman();
   }
+  if (sm.updated("liveTorqueParameters")) {
+    auto torque_params = sm["liveTorqueParameters"].getLiveTorqueParameters();
+    scene.friction = torque_params.getFrictionCoefficientFiltered();
+    scene.lat_accel = torque_params.getLatAccelFactorFiltered();
+    scene.live_valid = torque_params.getLiveValid();
+  }
   if (sm.updated("wideRoadCameraState")) {
     auto cam_state = sm["wideRoadCameraState"].getWideRoadCameraState();
     float scale = (cam_state.getSensor() == cereal::FrameData::ImageSensor::AR0231) ? 6.0f : 1.0f;
@@ -274,6 +288,9 @@ void ui_update_frogpilot_params(UIState *s) {
   Params params = Params();
   UIScene &scene = s->scene;
 
+  std::string branch = params.get("GitBranch");
+  bool isRelease = branch == "FrogPilot";
+
   bool always_on_lateral = params.getBool("AlwaysOnLateral");
   scene.show_aol_status_bar = always_on_lateral && !params.getBool("HideAOLStatusBar");
 
@@ -284,8 +301,13 @@ void ui_update_frogpilot_params(UIState *s) {
 
   bool custom_onroad_ui = params.getBool("CustomUI");
   bool custom_paths = custom_onroad_ui && params.getBool("CustomPaths");
+  bool developer_ui = !isRelease && custom_onroad_ui && params.getBool("DeveloperUI");
   scene.acceleration_path = custom_paths && params.getBool("AccelerationPath");
   scene.blind_spot_path = custom_paths && params.getBool("BlindSpotPath");
+  scene.lead_info = scene.longitudinal_control && custom_onroad_ui && params.getBool("LeadInfo");
+  scene.show_jerk = scene.longitudinal_control && developer_ui && params.getBool("ShowJerk");
+  scene.show_tuning = scene.has_auto_tune && developer_ui && params.getBool("ShowTuning");
+  scene.use_si = (scene.lead_info || developer_ui) && params.getBool("UseSI");
 
   bool custom_theme = params.getBool("CustomTheme");
   scene.custom_colors = custom_theme ? params.getInt("CustomColors") : 0;
@@ -336,7 +358,7 @@ UIState::UIState(QObject *parent) : QObject(parent) {
   sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2",
-    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan",
+    "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "liveTorqueParameters",
     "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan",
   });
 

selfdrive/ui/ui.h

@@ -180,20 +180,32 @@ typedef struct UIScene {
   bool dynamic_path_width;
   bool enabled;
   bool experimental_mode;
+  bool has_auto_tune;
   bool hide_lead_marker;
+  bool lead_info;
+  bool live_valid;
   bool map_open;
   bool model_ui;
   bool right_hand_drive;
   bool show_aol_status_bar;
   bool show_cem_status_bar;
+  bool show_jerk;
+  bool show_tuning;
   bool turn_signal_left;
   bool turn_signal_right;
   bool unlimited_road_ui_length;
+  bool use_si;
 
   float acceleration;
+  float acceleration_jerk;
+  float acceleration_jerk_difference;
+  float ego_jerk;
+  float ego_jerk_difference;
+  float friction;
   float lane_line_width;
   float lane_width_left;
   float lane_width_right;
+  float lat_accel;
   float path_edge_width;
   float path_width;
   float road_edge_width;
@@ -206,6 +218,10 @@ typedef struct UIScene {
   int custom_colors;
   int custom_icons;
   int custom_signals;
+  int desired_follow;
+  int obstacle_distance;
+  int obstacle_distance_stock;
+  int stopped_equivalence;
 
   QPolygonF track_adjacent_vertices[6];
   QPolygonF track_edge_vertices;