Smart Cruise Control: Vision (SCC-V) (#997)

* Controls: Vision Turn Speed Control * fix * Data type temp fix * format * more * even more * self contain targets * state cleanup * fix * param updates * no need * use similar state machine * raise exception if not found * new state * entirely internal * use long active * more * rename and expose aTarget * rename to SCC-V * init tests * slight tests * expose toggle * lint * todo * remove lat planner sub and mock sm data * introduce aTarget * rename * rename * update fill_model_msg.py to calculate PLAN_T_IDXS for lanelines and road edges * sync upstream * no SCC-V yet * Revert "no SCC-V yet" This reverts commit b67281bcac0dce0c6c3237afc2584cf29144c83c. * wrap it with SCC main * leave enabled out of here * wat * enabled and active on cereal * OP long for now, enable for ICBM once merged * need this * unused * let's go hybrid * fix * add override state * update tests * huh * don't math here if not enabled * ui: Smart Cruise Control - Vision (SCC-V) (#1253) * vtsc-ui * slight cleanup * more cleanup * unused * a bit more * pulse like it's hot * draw only enabled and active * let's try this for now * settle * finalize UI * brighter color so we blind devtekve * add long override --------- Co-authored-by: Jason Wen <haibin.wen3@gmail.com> * slight cleanup * more * type hints --------- Co-authored-by: discountchubbs <alexgrant990@gmail.com> Co-authored-by: Kumar <36933347+rav4kumar@users.noreply.github.com>
2026-09-15 03:53:58 +08:00 · 2025-09-17 22:55:36 -04:00
parent cb94d3b055
commit 784e1d6658
10 changed files with 431 additions and 5 deletions
@@ -18,6 +18,13 @@ LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) {
  cruisePanelScroller = new ScrollViewSP(list, this);
  vlayout->addWidget(cruisePanelScroller);

+  SmartCruiseControlVision = new ParamControl(
+    "SmartCruiseControlVision",
+    tr("Smart Cruise Control - Vision"),
+    tr("Use vision path predictions to estimate the appropriate speed to drive through turns ahead."),
+    "");
+  list->addItem(SmartCruiseControlVision);
+
  customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this);
  list->addItem(customAccIncrement);

@@ -75,5 +82,7 @@ void LongitudinalPanel::refresh(bool _offroad) {
  customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad);
  customAccIncrement->refresh();

+  SmartCruiseControlVision->setEnabled(has_longitudinal_control);
+
  offroad = _offroad;
 }
@@ -29,4 +29,5 @@ private:
  ScrollViewSP *cruisePanelScroller = nullptr;
  QWidget *cruisePanelScreen = nullptr;
  CustomAccIncrement *customAccIncrement = nullptr;
+  ParamControl *SmartCruiseControlVision;
 };
@@ -4,6 +4,7 @@
 * This file is part of sunnypilot and is licensed under the MIT License.
 * See the LICENSE.md file in the root directory for more details.
 */
+#include <QPainterPath>

 #include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"

@@ -25,6 +26,7 @@ void HudRendererSP::updateState(const UIState &s) {
  const auto gpsLocation = is_gps_location_external ? sm["gpsLocationExternal"].getGpsLocationExternal() : sm["gpsLocation"].getGpsLocation();
  const auto ltp = sm["liveTorqueParameters"].getLiveTorqueParameters();
  const auto car_params = sm["carParams"].getCarParams();
+  const auto lp_sp = sm["longitudinalPlanSP"].getLongitudinalPlanSP();

  static int reverse_delay = 0;
  bool reverse_allowed = false;
@@ -78,11 +80,30 @@ void HudRendererSP::updateState(const UIState &s) {

  standstillTimer = s.scene.standstill_timer;
  isStandstill = car_state.getStandstill();
+  longOverride = car_control.getCruiseControl().getOverride();
+  smartCruiseControlVisionEnabled = lp_sp.getSmartCruiseControl().getVision().getEnabled();
+  smartCruiseControlVisionActive = lp_sp.getSmartCruiseControl().getVision().getActive();
 }

 void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
  HudRenderer::draw(p, surface_rect);
  if (!reversing) {
+    // Smart Cruise Control
+    int x_offset = -260;
+    int y1_offset = -80;
+    // int y2_offset = -140;  // reserved for 2 icons
+
+    bool scc_vision_active_pulse = pulseElement(smartCruiseControlVisionFrame);
+    if ((smartCruiseControlVisionEnabled && !smartCruiseControlVisionActive) || (smartCruiseControlVisionActive && scc_vision_active_pulse)) {
+      drawSmartCruiseControlOnroadIcon(p, surface_rect, x_offset, y1_offset, "SCC-V");
+    }
+
+    if (smartCruiseControlVisionActive) {
+      smartCruiseControlVisionFrame++;
+    } else {
+      smartCruiseControlVisionFrame = 0;
+    }
+
    // Bottom Dev UI
    if (devUiInfo == 2) {
      QRect rect_bottom(surface_rect.left(), surface_rect.bottom() - 60, surface_rect.width(), 61);
@@ -112,6 +133,48 @@ void HudRendererSP::drawText(QPainter &p, int x, int y, const QString &text, QCo
  p.drawText(real_rect.x(), real_rect.bottom(), text);
 }

+bool HudRendererSP::pulseElement(int frame) {
+  if (frame % UI_FREQ < (UI_FREQ / 2.5)) {
+    return false;
+  }
+
+  return true;
+}
+
+void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name) {
+  int x = surface_rect.center().x();
+  int y = surface_rect.height() / 4;
+
+  QString text = QString::fromStdString(name);
+  QFont font = InterFont(36, QFont::Bold);
+  p.setFont(font);
+
+  QFontMetrics fm(font);
+
+  int padding_v = 5;
+  int box_width = 160;
+  int box_height = fm.height() + padding_v * 2;
+
+  QRectF bg_rect(x - (box_width / 2) + x_offset,
+                 y - (box_height / 2) + y_offset,
+                 box_width, box_height);
+
+  QPainterPath boxPath;
+  boxPath.addRoundedRect(bg_rect, 10, 10);
+
+  int text_w = fm.horizontalAdvance(text);
+  qreal baseline_y = bg_rect.top() + padding_v + fm.ascent();
+  qreal text_x = bg_rect.center().x() - (text_w / 2.0);
+
+  QPainterPath textPath;
+  textPath.addText(QPointF(text_x, baseline_y), font, text);
+  boxPath = boxPath.subtracted(textPath);
+
+  p.setPen(Qt::NoPen);
+  p.setBrush(longOverride ? QColor(0x91, 0x9b, 0x95, 0xf1) : QColor(0, 0xff, 0, 0xff));
+  p.drawPath(boxPath);
+}
+
 int HudRendererSP::drawRightDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color) {

  p.setFont(InterFont(28, QFont::Bold));
@@ -26,6 +26,8 @@ private:
  int drawBottomDevUIElement(QPainter &p, int x, int y, const QString &value, const QString &label, const QString &units, QColor &color);
  void drawBottomDevUI(QPainter &p, int x, int y);
  void drawStandstillTimer(QPainter &p, int x, int y);
+  bool pulseElement(int frame);
+  void drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name);

  bool lead_status;
  float lead_d_rel;
@@ -57,4 +59,8 @@ private:
  bool standstillTimer;
  bool isStandstill;
  float standstillElapsedTime;
+  bool longOverride;
+  bool smartCruiseControlVisionEnabled;
+  bool smartCruiseControlVisionActive;
+  int smartCruiseControlVisionFrame;
 };