exact distance :)

#	sunnypilot/navd/navigation_desires/navigation_desires.py
#	sunnypilot/navd/navigation_helpers/nav_instructions.py
#	sunnypilot/navd/navigation_helpers/tests/test_mapbox.py
#	sunnypilot/navd/navigationd.py

.github/workflows/build-all-tinygrad-models.yaml

@@ -74,7 +74,7 @@ jobs:
         env:
           GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
         run: |
-          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
+          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
           cd gitlab_docs
           git checkout main
           git sparse-checkout set --no-cone models/
@@ -191,7 +191,7 @@ jobs:
           GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
         run: |
           echo "Cloning GitLab"
-          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
+          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
           cd gitlab_docs
           echo "checkout models/${RECOMPILED_DIR}"
           git sparse-checkout set --no-cone models/${RECOMPILED_DIR}

.github/workflows/build-single-tinygrad-model.yaml

@@ -109,7 +109,7 @@ jobs:
           GIT_SSH_COMMAND: 'ssh -o UserKnownHostsFile=~/.ssh/known_hosts'
         run: |
           echo "Cloning GitLab"
-          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/docs.sunnypilot.ai2.git gitlab_docs
+          git clone --depth 1 --filter=tree:0 --sparse git@gitlab.com:sunnypilot/public/${{ vars.MODELS_GITLAB }} gitlab_docs
           cd gitlab_docs
           echo "checkout models/${RECOMPILED_DIR}"
           git sparse-checkout set --no-cone models/${RECOMPILED_DIR}

.github/workflows/sunnypilot-build-model.yaml

@@ -156,6 +156,8 @@ jobs:
         with:
           name: models-${{ env.REF }}${{ inputs.artifact_suffix }}
           path: ${{ github.workspace }}/selfdrive/modeld/models
+      - run: |
+          rm -f ${{ github.workspace }}/selfdrive/modeld/models/{dmonitoring_model,big_driving_policy,big_driving_vision}.onnx
 
       - name: Build Model
         run: |

cereal/custom.capnp

@@ -369,6 +369,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
   leadOne @2 :LeadData;
   leadTwo @3 :LeadData;
   intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
+  speed @5 :Float32;
 
   struct Param {
     key @0 :Text;
@@ -456,14 +457,14 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
 
 struct Navigationd @0xcb9fd56c7057593a {
   upcomingTurn @0 :Text;
-  currentSpeedLimit @1 :UInt64;
+  currentSpeedLimit @1 :UInt16;
   bannerInstructions @2 :Text;
-  distanceFromRoute @3 :Float64;
+  distanceFromRoute @3 :Float32;
   allManeuvers @4 :List(Maneuver);
   valid @5 :Bool;
 
   struct Maneuver {
-    distance @0 :Float64;
+    distance @0 :Float32;
     type @1 :Text;
     modifier @2 :Text;
     instruction @3 :Text;

common/params_keys.h

@@ -172,14 +172,6 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ShowTurnSignals", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualRadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualRadarTracksDelay", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
-    {"VisualWideCam", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualStyle", {PERSISTENT | BACKUP, INT, "0"}},
-    {"VisualStyleZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualStyleOverhead", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualStyleOverheadZoom", {PERSISTENT | BACKUP, BOOL, "0"}},
-    {"VisualStyleOverheadThreshold", {PERSISTENT | BACKUP, INT, "20"}},
 
     // MADS params
     {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}},
@@ -197,6 +189,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // Navigation params
     {"AllowNavigation", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MapboxFavorites", {PERSISTENT | BACKUP, STRING}},
     {"MapboxToken", {PERSISTENT | BACKUP, STRING}},
     {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
     {"MapboxRoute", {PERSISTENT, STRING}},

selfdrive/selfdrived/selfdrived.py

@@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper):
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] + ['navigationd']
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -98,7 +98,7 @@ class SelfdriveD(CruiseHelper):
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP', 'longitudinalPlanSP', 'navigationd'] + \
+                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
                                   self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -25,11 +25,6 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   // update engageability/experimental mode button
   experimental_btn->updateState(s);
   dmon.updateState(s);
-  if (s.scene.visual_style == 0) {
-    setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
-  } else {
-    setBackgroundColor(QColor(0, 0, 0));
-  }
 }
 
 void AnnotatedCameraWidget::initializeGL() {
@@ -40,12 +35,7 @@ void AnnotatedCameraWidget::initializeGL() {
   qInfo() << "OpenGL language version:" << QString((const char*)glGetString(GL_SHADING_LANGUAGE_VERSION));
 
   prev_draw_t = millis_since_boot();
-  auto *s = uiState();
-  if (s->scene.visual_style == 0) {
-    setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
-  } else {
-    setBackgroundColor(QColor(0, 0, 0));
-  }
+  setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
 }
 
 mat4 AnnotatedCameraWidget::calcFrameMatrix() {
@@ -128,13 +118,7 @@ void AnnotatedCameraWidget::paintGL() {
       } else if (v_ego > 15) {
         wide_cam_requested = false;
       }
-      if (s->scene.visual_wide_cam == 1) {
-        wide_cam_requested = true;
-      } else if (s->scene.visual_wide_cam == 2) {
-        wide_cam_requested = false;
-      } else {
-        wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
-      }
+      wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
     }
     CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
     CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId());

selfdrive/ui/qt/onroad/model.cc

@@ -1,5 +1,4 @@
 #include "selfdrive/ui/qt/onroad/model.h"
-#include <algorithm>
 
 void ModelRenderer::draw(QPainter &painter, const QRect &surface_rect) {
   auto *s = uiState();
@@ -50,14 +49,8 @@ void ModelRenderer::update_leads(const cereal::RadarState::Reader &radar_state,
 }
 
 void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
-  auto *s = uiState();
   const auto &model_position = model.getPosition();
-  float max_distance;
-  if (s->scene.visual_style == 0) {
-    max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
-  } else {
-    max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
-  }
+  float max_distance = std::clamp(*(model_position.getX().end() - 1), MIN_DRAW_DISTANCE, MAX_DRAW_DISTANCE);
 
   // update lane lines
   const auto &lane_lines = model.getLaneLines();
@@ -65,11 +58,7 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
   int max_idx = get_path_length_idx(lane_lines[0], max_distance);
   for (int i = 0; i < std::size(lane_line_vertices); i++) {
     lane_line_probs[i] = line_probs[i];
-    if (s->scene.visual_style == 2) {
-      mapLineToPolygon(lane_lines[i], 0.075 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
-    } else {
-      mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
-    }
+    mapLineToPolygon(lane_lines[i], 0.025 * lane_line_probs[i], 0, &lane_line_vertices[i], max_idx);
   }
 
   // update road edges
@@ -77,11 +66,7 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
   const auto &edge_stds = model.getRoadEdgeStds();
   for (int i = 0; i < std::size(road_edge_vertices); i++) {
     road_edge_stds[i] = edge_stds[i];
-    if (s->scene.visual_style == 2) {
-      mapLineToPolygon(road_edges[i], 0.1, 0, &road_edge_vertices[i], max_idx);
-    } else {
-      mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
-    }
+    mapLineToPolygon(road_edges[i], 0.025, 0, &road_edge_vertices[i], max_idx);
   }
 
   // update path
@@ -94,112 +79,16 @@ void ModelRenderer::update_model(const cereal::ModelDataV2::Reader &model, const
 }
 
 void ModelRenderer::drawLaneLines(QPainter &painter) {
-  auto *s = uiState();
-  if (s->scene.visual_style == 2) {
-    QRectF r = clip_region;
+  // lanelines
+  for (int i = 0; i < std::size(lane_line_vertices); ++i) {
+    painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
+    painter.drawPolygon(lane_line_vertices[i]);
+  }
 
-    qreal horizonY = r.bottom();
-    if (!road_edge_vertices[0].isEmpty() || !road_edge_vertices[1].isEmpty()) {
-      qreal leftH  = r.top();
-      qreal rightH = r.top();
-
-      if (!road_edge_vertices[0].isEmpty()) {
-        leftH = std::numeric_limits<qreal>::max();
-        for (const QPointF &pt : road_edge_vertices[0]) {
-          if (pt.y() < leftH) leftH = pt.y();
-        }
-      }
-
-      if (!road_edge_vertices[1].isEmpty()) {
-        rightH = std::numeric_limits<qreal>::max();
-        for (const QPointF &pt : road_edge_vertices[1]) {
-          if (pt.y() < rightH) rightH = pt.y();
-        }
-      }
-
-      horizonY = std::max(leftH, rightH);
-    }
-
-    painter.fillRect(QRectF(r.left(), horizonY + 0, r.width(), r.bottom() - (horizonY + 0)), QColor("#111111"));
-
-    auto buildFill = [&](const QPolygonF &edgeRibbon, bool isLeftSide) -> QPolygonF {
-      if (edgeRibbon.isEmpty()) return {};
-
-      QMap<int, QPointF> byY;
-      for (const QPointF &pt : edgeRibbon) {
-        int yi = int(std::round(pt.y()));
-        if (!byY.contains(yi)) {
-          byY[yi] = pt;
-        } else {
-          if (isLeftSide) {
-            if (pt.x() > byY[yi].x()) byY[yi] = pt;
-          } else {
-            if (pt.x() < byY[yi].x()) byY[yi] = pt;
-          }
-        }
-      }
-      if (byY.isEmpty()) return {};
-
-      QPolygonF curve;
-      for (auto it = byY.cbegin(); it != byY.cend(); ++it) {
-        curve << it.value();
-      }
-      if (curve.size() < 2) return {};
-
-      const qreal topY = curve.first().y();
-      QPolygonF fill;
-      if (isLeftSide) {
-        fill << QPointF(r.left(), topY);
-        for (const QPointF &pt : curve) fill << pt;
-        fill << QPointF(r.left(), r.bottom());
-      } else {
-        fill << QPointF(r.right(), topY);
-        for (const QPointF &pt : curve) fill << pt;
-        fill << QPointF(r.right(), r.bottom());
-      }
-      return fill;
-    };
-
-    QPolygonF leftFill  = buildFill(road_edge_vertices[0], true);
-    QPolygonF rightFill = buildFill(road_edge_vertices[1], false);
-
-    if (!leftFill.isEmpty()) {
-      painter.setBrush(QColor("#222222"));
-      painter.drawPolygon(leftFill);
-    }
-    if (!rightFill.isEmpty()) {
-      painter.setBrush(QColor("#222222"));
-      painter.drawPolygon(rightFill);
-    }
-
-    for (int i = 0; i < std::size(lane_line_vertices); ++i) {
-      painter.setBrush(QColor::fromRgbF(0.902, 0.902, 0.902, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
-      painter.drawPolygon(lane_line_vertices[i]);
-    }
-
-    for (int i = 0; i < std::size(road_edge_vertices); ++i) {
-      painter.setBrush(QColor(0x55, 0x55, 0x55, 255));
-      painter.drawPolygon(road_edge_vertices[i]);
-    }
-
-    QLinearGradient bgGrad(r.left(), horizonY - 100, r.left(), horizonY + 100);
-    bgGrad.setColorAt(0.0, QColor("#000000"));
-    bgGrad.setColorAt(0.5, QColor("#111111"));
-    bgGrad.setColorAt(1.0, QColor("#111111"));
-    painter.fillRect(QRectF(r.left(), horizonY - 200, r.width(), 200), bgGrad);
-
-  } else {
-    // lanelines
-    for (int i = 0; i < std::size(lane_line_vertices); ++i) {
-      painter.setBrush(QColor::fromRgbF(1.0, 1.0, 1.0, std::clamp<float>(lane_line_probs[i], 0.0, 0.7)));
-      painter.drawPolygon(lane_line_vertices[i]);
-    }
-
-    // road edges
-    for (int i = 0; i < std::size(road_edge_vertices); ++i) {
-      painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
-      painter.drawPolygon(road_edge_vertices[i]);
-    }
+  // road edges
+  for (int i = 0; i < std::size(road_edge_vertices); ++i) {
+    painter.setBrush(QColor::fromRgbF(1.0, 0, 0, std::clamp<float>(1.0 - road_edge_stds[i], 0.0, 1.0)));
+    painter.drawPolygon(road_edge_vertices[i]);
   }
 }
 
@@ -286,7 +175,6 @@ QColor ModelRenderer::blendColors(const QColor &start, const QColor &end, float
 
 void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data,
                              const QPointF &vd, const QRect &surface_rect) {
-  auto *s = uiState();
   const float speedBuff = 10.;
   const float leadBuff = 40.;
   const float d_rel = lead_data.getDRel();
@@ -309,133 +197,20 @@ void ModelRenderer::drawLead(QPainter &painter, const cereal::RadarState::LeadDa
   float g_yo = sz / 10;
 
   QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}};
-  if (s->scene.visual_style == 2) {
-    painter.setBrush(QColor(0xE6, 0xE6, 0xE6, 255));
-  } else {
-    painter.setBrush(QColor(218, 202, 37, 255));
-  }
+  painter.setBrush(QColor(218, 202, 37, 255));
   painter.drawPolygon(glow, std::size(glow));
 
   // chevron
   QPointF chevron[] = {{x + (sz * 1.25), y + sz}, {x, y}, {x - (sz * 1.25), y + sz}};
-  if (s->scene.visual_style == 2) {
-    painter.setBrush(QColor(0, 0, 0, fillAlpha));
-  } else {
-    painter.setBrush(QColor(201, 34, 49, fillAlpha));
-  }
+  painter.setBrush(QColor(201, 34, 49, fillAlpha));
   painter.drawPolygon(chevron, std::size(chevron));
 }
 
-float mapRange(float x, float in_min, float in_max, float out_min, float out_max) {
-  if (in_min < in_max) {
-    x = std::clamp(x, in_min, in_max);
-  } else {
-    x = std::clamp(x, in_max, in_min);
-  }
-  return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
-}
-
-// Projects a point in car space to the corresponding point in full frame image space.
+// Projects a point in car to space to the corresponding point in full frame image space.
 bool ModelRenderer::mapToScreen(float in_x, float in_y, float in_z, QPointF *out) {
-  auto *s = uiState();
-  auto &sm = *(s->sm);
-  float blend_speed_mph = fabsf(sm["carState"].getCarState().getVEgo() * 2.23694f);
-
   Eigen::Vector3f input(in_x, in_y, in_z);
-
-  if ((s->scene.visual_style_zoom == 1 || s->scene.visual_style_zoom == 2) && s->scene.visual_style != 0) {
-    float zoom_start = 20.0f;
-    float zoom_end   = 50.0f;
-
-    if (s->scene.visual_style_zoom == 2) {
-      std::swap(zoom_start, zoom_end);
-    }
-
-    float IN_X_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 24.0f);
-    float IN_Y_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 1.0f, 2.0f);
-    float IN_Z_OFFSET = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
-    float PITCH_DEG   = mapRange(blend_speed_mph, zoom_start, zoom_end, 0.0f, 5.0f);
-
-    input = Eigen::Vector3f(in_x + IN_X_OFFSET, in_y / IN_Y_OFFSET, in_z + IN_Z_OFFSET);
-    Eigen::AngleAxisf pitch_rot(PITCH_DEG * M_PI / 180.0f, Eigen::Vector3f::UnitY());
-    input = pitch_rot * input;
-  }
-
   auto pt = car_space_transform * input;
-  bool normal_valid = (pt.z() > 1e-3f &&
-                       std::isfinite(pt.x()) && std::isfinite(pt.y()));
-  QPointF normal_view;
-  if (normal_valid) {
-    normal_view = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
-  }
-
-  const float base_scale_x = 20.0f;
-  const float base_scale_y = 15.0f;
-  const float y_offset = 450.0f;
-
-  float factor_scale_x = 0.0f;
-  if (blend_speed_mph > 0.0f) {
-    if (s->scene.visual_style_overhead_zoom == 1) {
-      factor_scale_x = mapRange(blend_speed_mph, 0.0f, 50.0f, 30.0f, 0.0f);
-    } else if (s->scene.visual_style_overhead_zoom == 2) {
-      factor_scale_x = mapRange(blend_speed_mph, 50.0f, 0.0f, 30.0f, 0.0f);
-    }
-  }
-
-  float scale_x = base_scale_x + factor_scale_x;
-  float scale_y = base_scale_y;
-
-  QPointF topdown_view(
-    clip_region.center().x() + in_y * scale_x,
-    (clip_region.bottom() - y_offset) - in_x * scale_y
-  );
-
-  if ((s->scene.visual_style_overhead == 1 || s->scene.visual_style_overhead == 2) && s->scene.visual_style != 0) {
-    static float blend = 0.0f;
-    static float target_blend = 0.0f;
-    static double last_t = millis_since_boot();
-
-    const bool inverted = (s->scene.visual_style_overhead == 2);
-    const float threshold = s->scene.visual_style_overhead_threshold;
-    const float hysteresis = 5.0f;
-
-    if (!inverted) {
-      if (target_blend < 0.5f && blend_speed_mph > threshold) {
-        target_blend = 1.0f;
-      } else if (target_blend > 0.5f && blend_speed_mph < threshold - hysteresis) {
-        target_blend = 0.0f;
-      }
-    } else {
-      if (target_blend < 0.5f && blend_speed_mph < threshold) {
-        target_blend = 1.0f;
-      } else if (target_blend > 0.5f && blend_speed_mph > threshold + hysteresis) {
-        target_blend = 0.0f;
-      }
-    }
-
-    double now = millis_since_boot();
-    double dt = (now - last_t) / 1000.0;
-    last_t = now;
-
-    const float transition_time = 1.50f;
-    float step = dt / transition_time;
-
-    if (blend < target_blend) {
-      blend = std::min(blend + step, target_blend);
-    } else if (blend > target_blend) {
-      blend = std::max(blend - step, target_blend);
-    }
-
-    if (!normal_valid) return false;
-    *out = QPointF(
-      (1 - blend) * normal_view.x() + blend * topdown_view.x(),
-      (1 - blend) * normal_view.y() + blend * topdown_view.y()
-    );
-  } else {
-    if (!normal_valid) return false;
-    *out = normal_view;
-  }
-
+  *out = QPointF(pt.x() / pt.z(), pt.y() / pt.z());
   return clip_region.contains(*out);
 }
 

selfdrive/ui/qt/widgets/cameraview.cc

@@ -196,7 +196,6 @@ mat4 CameraWidget::calcFrameMatrix() {
 }
 
 void CameraWidget::paintGL() {
-  auto *s = uiState();
   glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
   glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
@@ -249,9 +248,7 @@ void CameraWidget::paintGL() {
 
   glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
   glEnableVertexAttribArray(0);
-  if (s->scene.visual_style == 0) {
-    glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
-  }
+  glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
   glDisableVertexAttribArray(0);
   glBindVertexArray(0);
   glBindTexture(GL_TEXTURE_2D, 0);

selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.cc

@@ -36,7 +36,7 @@ DisplayPanel::DisplayPanel(QWidget *parent) : QWidget(parent) {
   interactivityTimeout =  new OptionControlSP("InteractivityTimeout", tr("Interactivity Timeout"),
                                    tr("Apply a custom timeout for settings UI."
                                       "\nThis is the time after which settings UI closes automatically if user is not interacting with the screen."),
-                                   "", {999999, 1000000}, 1000000, true, nullptr, false);
+                                   "", {0, 120}, 10, true, nullptr, false);
 
   connect(interactivityTimeout, &OptionControlSP::updateLabels, [=]() {
     refresh();

selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.cc

@@ -16,7 +16,7 @@ NavigationPanel::NavigationPanel(QWidget* parent) : QWidget(parent) {
   main_layout->addWidget(scroller);
 
   // Mapbox Token
-  mapbox_token = new ButtonControl(tr("Mapbox Token"), tr("Edit"), tr("Enter your Mapbox API token"));
+  mapbox_token = new ButtonControl(tr("Mapbox token"), tr("Edit"), tr("Enter your mapbox public token"));
   QObject::connect(mapbox_token, &ButtonControl::clicked, [=]() {
     QString current = QString::fromStdString(params.get("MapboxToken"));
     QString token = InputDialog::getText(tr("Enter Mapbox Token"), this, "", false, -1, current);
@@ -28,7 +28,7 @@ NavigationPanel::NavigationPanel(QWidget* parent) : QWidget(parent) {
   list->addItem(mapbox_token);
 
   // Mapbox Route
-  mapbox_route = new ButtonControl(tr("Mapbox Route"), tr("Edit"), tr("Enter Mapbox route data"));
+  mapbox_route = new ButtonControl(tr("Mapbox route"), tr("Edit"), tr(""));
   QObject::connect(mapbox_route, &ButtonControl::clicked, [=]() {
     QString current = QString::fromStdString(params.get("MapboxRoute"));
     QString route = InputDialog::getText(tr("Enter Mapbox Route"), this, "", false, -1, current);
@@ -39,23 +39,134 @@ NavigationPanel::NavigationPanel(QWidget* parent) : QWidget(parent) {
   });
   list->addItem(mapbox_route);
 
-  // Allow Navigation
-  allow_navigation = new ParamControlSP("AllowNavigation", tr("Allow Navigation"), tr("Enable navigation features and start navigationd"), "", this);
+  // Clear Route
+  clear_route = new ButtonControl(tr("Clear current route"), tr("Clear"), tr(""));
+  QObject::connect(clear_route, &ButtonControl::clicked, [=]() {
+    params.remove("MapboxRoute");
+    refresh();
+  });
+  list->addItem(clear_route);
+
+  // Favs selector
+  favorites_selector = new MultiButtonControlSP("Favorites", "Select favorite route", "", {"Home", "Work", "Favs"}, 470);
+  QObject::connect(favorites_selector, &MultiButtonControlSP::buttonClicked, [=](int id) {
+    QString favs_str = QString::fromStdString(params.get("MapboxFavorites"));
+    QJsonDocument doc = QJsonDocument::fromJson(favs_str.toUtf8());
+    QJsonObject obj = doc.object();
+    QString route;
+    if (id == 0) route = obj["home"].toString();
+    else if (id == 1) route = obj["work"].toString();
+    else if (id == 2) {
+      QJsonObject favorites_obj = obj["favorites"].toObject();
+      if (favorites_obj.isEmpty()) {
+        ConfirmationDialog::alert(tr("No custom favorites set"), this);
+        return;
+      }
+      QStringList favNames = favorites_obj.keys();
+      const QString selected = MultiOptionDialog::getSelection(tr("Select Favorite"), favNames, "", this);
+      if (!selected.isEmpty()) {
+        route = favorites_obj[selected].toString();
+      }
+    }
+    if (!route.isEmpty()) {
+      params.put("MapboxRoute", route.toStdString());
+      refresh();
+    }
+  });
+  list->addItem(favorites_selector);
+
+  struct FavInfo {
+    QString key;
+    QString title;
+    QString desc;
+  };
+  std::vector<FavInfo> favs = {
+    {"home", tr("Set Home"), tr("")},
+    {"work", tr("Set Work"), tr("")}
+  };
+  set_buttons.resize(favs.size());
+  for (size_t i = 0; i < favs.size(); ++i) {
+    const auto& fav = favs[i];
+    set_buttons[i] = new ButtonControl(fav.title, tr("Set"), fav.desc);
+    QObject::connect(set_buttons[i], &ButtonControl::clicked, [this, fav]() {
+      QString favs_str = QString::fromStdString(params.get("MapboxFavorites"));
+      QJsonDocument doc = QJsonDocument::fromJson(favs_str.toUtf8());
+      QJsonObject obj = doc.object();
+      QString current = obj[fav.key].toString();
+      QString route = InputDialog::getText(tr("Set %1 Route").arg(fav.title.mid(4)), this, "", false, -1, current);  // mid(4) to remove "Set "
+      if (!route.isEmpty()) {
+        obj[fav.key] = route;
+        QJsonDocument new_doc(obj);
+        params.put("MapboxFavorites", new_doc.toJson(QJsonDocument::Compact).toStdString());
+        refresh();
+      }
+    });
+    list->addItem(set_buttons[i]);
+  }
+
+  add_fav = new ButtonControl(tr("Add Favorite"), tr("Add"), tr("Add a new custom favorite"));
+  QObject::connect(add_fav, &ButtonControl::clicked, [=]() {
+    QString name = InputDialog::getText(tr("Favorite Name"), this, "", false, -1, "");
+    if (name.isEmpty()) return;
+
+    QString route = InputDialog::getText(tr("Favorite Route"), this, "", false, -1, "");
+    if (route.isEmpty()) return;
+
+    QString favs_str = QString::fromStdString(params.get("MapboxFavorites"));
+    QJsonDocument doc = QJsonDocument::fromJson(favs_str.toUtf8());
+    QJsonObject obj = doc.object();
+    QJsonObject favorites_obj = obj["favorites"].toObject();
+    favorites_obj[name] = route;
+    obj["favorites"] = favorites_obj;
+    QJsonDocument new_doc(obj);
+    params.put("MapboxFavorites", new_doc.toJson(QJsonDocument::Compact).toStdString());
+    refresh();
+  });
+  list->addItem(add_fav);
+
+  remove_fav = new ButtonControl(tr("Remove Favorite"), tr("Remove"), tr("Remove a custom favorite"));
+  QObject::connect(remove_fav, &ButtonControl::clicked, [=]() {
+    QString favs_str = QString::fromStdString(params.get("MapboxFavorites"));
+    QJsonDocument doc = QJsonDocument::fromJson(favs_str.toUtf8());
+    QJsonObject obj = doc.object();
+    QJsonObject favorites_obj = obj["favorites"].toObject();
+    if (favorites_obj.isEmpty()) {
+      ConfirmationDialog::alert(tr("No custom favorites to remove"), this);
+      return;
+    }
+    QStringList favNames = favorites_obj.keys();
+    const QString selected = MultiOptionDialog::getSelection(tr("Remove Favorite"), favNames, "", this);
+    if (!selected.isEmpty()) {
+      favorites_obj.remove(selected);
+      obj["favorites"] = favorites_obj;
+      QJsonDocument new_doc(obj);
+      params.put("MapboxFavorites", new_doc.toJson(QJsonDocument::Compact).toStdString());
+      refresh();
+    }
+  });
+  list->addItem(remove_fav);
+
+  // Dumb params
+  allow_navigation = new ParamControlSP("AllowNavigation", tr("Allow navigation"), tr("Enable navigation features and start navigationd"), "", this);
   QObject::connect(allow_navigation, &ParamControlSP::toggleFlipped, this, &NavigationPanel::updateNavigationVisibility);
   list->addItem(allow_navigation);
 
-  // Mapbox Recompute
-  mapbox_recompute = new ParamControlSP("MapboxRecompute", tr("Mapbox Recompute"), tr("Enable automatic route recomputation"), "", this);
+  mapbox_recompute = new ParamControlSP("MapboxRecompute", tr("Mapbox recompute"), tr("Enable automatic route recomputation"), "", this);
   list->addItem(mapbox_recompute);
 
-  // Nav Allowed
-  nav_allowed = new ParamControlSP("NavDesiresAllowed", tr("Navigation Allowed"), tr("Allow navigation to automatically take turns"), "", this);
+  nav_allowed = new ParamControlSP("NavDesiresAllowed", tr("Navigation allowed"), tr("Allow navigation to automatically take turns"), "", this);
   list->addItem(nav_allowed);
 }
 
 void NavigationPanel::updateNavigationVisibility(bool state) {
   mapbox_recompute->setVisible(state);
   nav_allowed->setVisible(state);
+  favorites_selector->setVisible(state);
+  for (auto btn : set_buttons) {
+    btn->setVisible(state);
+  }
+  add_fav->setVisible(state);
+  remove_fav->setVisible(state);
 }
 
 void NavigationPanel::showEvent(QShowEvent *event) {

selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.h

@@ -7,13 +7,12 @@
 
 #pragma once
 
-#include "selfdrive/ui/qt/offroad/settings.h"
-#include "selfdrive/ui/qt/widgets/controls.h"
-#include "selfdrive/ui/qt/widgets/input.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
-#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
-#include "selfdrive/ui/sunnypilot/qt/util.h"
+#include <QJsonDocument>
+#include <QJsonObject>
+#include <vector>
 
+#include "selfdrive/ui/qt/offroad/settings.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h"
 
 class NavigationPanel : public QWidget {
   Q_OBJECT
@@ -33,6 +32,11 @@ private:
   ParamControlSP* allow_navigation;
   ButtonControl* mapbox_token;
   ButtonControl* mapbox_route;
+  ButtonControl* clear_route;
   ParamControlSP* mapbox_recompute;
   ParamControlSP* nav_allowed;
+  MultiButtonControlSP* favorites_selector;
+  std::vector<ButtonControl*> set_buttons;
+  ButtonControl* add_fav;
+  ButtonControl* remove_fav;
 };

selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.cc

@@ -11,18 +11,6 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
   param_watcher = new ParamWatcher(this);
   connect(param_watcher, &ParamWatcher::paramChanged, [=](const QString &param_name, const QString &param_value) {
     paramsRefresh();
-    if (param_name == "VisualStyle") {
-      visual_style_value = param_value.toInt();
-    } else if (param_name == "VisualStyleOverhead") {
-      visual_style_overhead_value = param_value.toInt();
-    } else if (param_name == "VisualRadarTracks") {
-      bool radar_tracks_enabled = param_value.toInt() != 0;
-      visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
-    }
-    visual_style_zoom_settings->setVisible(visual_style_value != 0);
-    visual_style_overhead_settings->setVisible(visual_style_value != 0);
-    visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
-    visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
   });
 
   main_layout = new QStackedLayout(this);
@@ -102,13 +90,6 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
       "",
       false,
     },
-    {
-      "VisualRadarTracks",
-      tr("Show Radar Tracks"),
-      tr("Shows what the cars radar sees."),
-      "",
-      false,
-    },
   };
 
   // Add regular toggles first
@@ -135,111 +116,6 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
     param_watcher->addParam(param);
   }
 
-  // Visuals: Radar Tracks Delay
-  visual_radar_tracks_delay_settings = new OptionControlSP("VisualRadarTracksDelay", tr("Adjust Visual Radar Tracks Delay"),
-                                      tr("Delays radar tracks to better match what you see through the camera."),
-                                      "", {0, 100}, 10, false, nullptr, true);
-
-  connect(visual_radar_tracks_delay_settings, &OptionControlSP::updateLabels, [=]() {
-    float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
-    visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
-  });
-
-  float radar_tracks_delay_value = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
-  visual_radar_tracks_delay_settings->setLabel(QString::number(radar_tracks_delay_value, 'f', 1) + " s");
-
-  list->addItem(visual_radar_tracks_delay_settings);
-
-  // Wide Cam
-  std::vector<QString> visual_wide_cam_settings_texts{tr("Auto"), tr("On"), tr("Off")};
-  visual_wide_cam_settings = new ButtonParamControlSP(
-    "VisualWideCam", tr("Wide Cam"), tr("Override the wide cam view regardless of experimental mode status."),
-    "",
-    visual_wide_cam_settings_texts,
-    250);
-  list->addItem(visual_wide_cam_settings);
-
-  // Visual Style
-  std::vector<QString> visual_style_settings_texts{tr("Default"), tr("Minimal"), tr("Vision")};
-  visual_style_settings = new ButtonParamControlSP(
-    "VisualStyle", tr("Visual Style"),
-    tr(
-      "Switch between different on-road visualization layouts."
-      "<ul style='margin-left: 10px; margin-top: 4px;'>"
-      "<li><b>Default:</b> Standard OpenPilot layout with camera and path view.</li>"
-      "<li><b>Minimal:</b> Clean interface without camera feed or extra elements.</li>"
-      "<li><b>Vision:</b> Experimental layout that focuses on model perception and environment.</li>"
-      "</ul>"
-    ),
-    "",
-    visual_style_settings_texts,
-    380);
-  list->addItem(visual_style_settings);
-
-  // Visual Style Zoom
-  std::vector<QString> visual_style_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
-  visual_style_zoom_settings = new ButtonParamControlSP(
-    "VisualStyleZoom", tr("Visual Style Zoom"),
-    tr(
-      "Enables dynamic zooming based on driving speed in the selected visual style."
-      "<ul style='margin-left: 10px; margin-top: 4px;'>"
-      "<li><b>Disabled:</b> Keeps the zoom fixed.</li>"
-      "<li><b>Enabled:</b> Zooms in at low speed and out at high speed.</li>"
-      "<li><b>Inverted:</b> Reverses the zoom behavior.</li>"
-      "</ul>"
-    ),
-    "",
-    visual_style_zoom_settings_texts,
-    380);
-  list->addItem(visual_style_zoom_settings);
-
-  // Visual Style Overhead
-  std::vector<QString> visual_style_overhead_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
-  visual_style_overhead_settings = new ButtonParamControlSP(
-    "VisualStyleOverhead", tr("Visual Style Overhead"),
-    tr(
-      "Toggles an overhead (top-down) camera view for a 2D-style perspective."
-      "<ul style='margin-left: 10px; margin-top: 4px;'>"
-      "<li><b>Disabled:</b> Keeps the standard forward 3D view.</li>"
-      "<li><b>Enabled:</b> Switches to overhead view when active.</li>"
-      "<li><b>Inverted:</b> Reverses when the transition happens.</li>"
-      "</ul>"
-    ),
-    "",
-    visual_style_overhead_settings_texts,
-    380);
-  list->addItem(visual_style_overhead_settings);
-
-  // Visual Style Overhead Zoom
-  std::vector<QString> visual_style_overhead_zoom_settings_texts{tr("Disabled"), tr("Enabled"), tr("Inverted")};
-  visual_style_overhead_zoom_settings = new ButtonParamControlSP(
-    "VisualStyleOverheadZoom", tr("Visual Style Overhead Zoom"),
-    tr(
-      "Controls zooming behavior while in overhead mode."
-      "<ul style='margin-left: 10px; margin-top: 4px;'>"
-      "<li><b>Disabled:</b> Keeps a fixed zoom level in overhead mode.</li>"
-      "<li><b>Enabled:</b> Zooms dynamically based on speed while overhead.</li>"
-      "<li><b>Inverted:</b> Opposite zoom direction.</li>"
-      "</ul>"
-    ),
-    "",
-    visual_style_overhead_zoom_settings_texts,
-    380);
-  list->addItem(visual_style_overhead_zoom_settings);
-
-  // Visual Style Overhead Threshold
-  visual_style_overhead_threshold_settings = new OptionControlSP(
-    "VisualStyleOverheadThreshold", tr("Visual Style Overhead Threshold"),
-    tr("Sets the speed (in mph) where the display transitions between normal and overhead view."),
-    "", {10, 80}, 5, false, nullptr, false);
-  auto updateThresholdLabel = [=]() {
-    int mph = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
-    visual_style_overhead_threshold_settings->setLabel(QString("%1 mph").arg(mph));
-  };
-  connect(visual_style_overhead_threshold_settings, &OptionControlSP::updateLabels, updateThresholdLabel);
-  updateThresholdLabel();
-  list->addItem(visual_style_overhead_threshold_settings);
-
   // Visuals: Display Metrics below Chevron
   std::vector<QString> chevron_info_settings_texts{tr("Off"), tr("Distance"), tr("Speed"), tr("Time"), tr("All")};
   chevron_info_settings = new ButtonParamControlSP(
@@ -260,19 +136,6 @@ VisualsPanel::VisualsPanel(QWidget *parent) : QWidget(parent) {
     380);
   list->addItem(dev_ui_settings);
 
-  bool radar_tracks_enabled = QString::fromStdString(params.get("VisualRadarTracks")).toInt() != 0;
-  visual_radar_tracks_delay_settings->setVisible(radar_tracks_enabled);
-  param_watcher->addParam("VisualRadarTracks");
-
-  visual_style_value = QString::fromStdString(params.get("VisualStyle")).toInt();
-  visual_style_overhead_value = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
-  visual_style_zoom_settings->setVisible(visual_style_value != 0);
-  visual_style_overhead_settings->setVisible(visual_style_value != 0);
-  visual_style_overhead_zoom_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
-  visual_style_overhead_threshold_settings->setVisible(visual_style_value != 0 && visual_style_overhead_value != 0);
-  param_watcher->addParam("VisualStyle");
-  param_watcher->addParam("VisualStyleOverhead");
-
   sunnypilotScroller = new ScrollViewSP(list, this);
   vlayout->addWidget(sunnypilotScroller);
 
@@ -328,19 +191,4 @@ void VisualsPanel::paramsRefresh() {
   if (dev_ui_settings) {
     dev_ui_settings->refresh();
   }
-  if (visual_wide_cam_settings) {
-    visual_wide_cam_settings->refresh();
-  }
-  if (visual_style_settings) {
-    visual_style_settings->refresh();
-  }
-  if (visual_style_zoom_settings) {
-    visual_style_zoom_settings->refresh();
-  }
-  if (visual_style_overhead_settings) {
-    visual_style_overhead_settings->refresh();
-  }
-  if (visual_style_overhead_zoom_settings) {
-    visual_style_overhead_zoom_settings->refresh();
-  }
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/visuals_panel.h

@@ -32,14 +32,4 @@ protected:
  ButtonParamControlSP *dev_ui_settings;
 
  bool has_longitudinal_control = false;
-
- OptionControlSP *visual_radar_tracks_delay_settings;
- ButtonParamControlSP *visual_wide_cam_settings;
- int visual_style_value = 0;
- int visual_style_overhead_value = 0;
- ButtonParamControlSP *visual_style_settings;
- ButtonParamControlSP *visual_style_zoom_settings;
- ButtonParamControlSP *visual_style_overhead_settings;
- ButtonParamControlSP *visual_style_overhead_zoom_settings;
- OptionControlSP *visual_style_overhead_threshold_settings;
 };

selfdrive/ui/sunnypilot/qt/onroad/hud.cc

@@ -176,7 +176,12 @@ void HudRendererSP::updateState(const UIState &s) {
             navigationDistance = QString::number(std::round(dist / 50.0) * 50) + " m";
           }
         } else {
-          navigationDistance = QString::number(dist / 1000, 'f', 1) + " km";
+          float dist_km = dist / 1000;
+          if (dist_km >= 10.0) {
+            navigationDistance = QString::number(std::floor(dist_km)) + " km";
+          } else {
+            navigationDistance = QString::number(dist_km, 'f', 1) + " km";
+          }
         }
       } else {
         float dist_ft = dist * 3.28084f;
@@ -188,7 +193,12 @@ void HudRendererSP::updateState(const UIState &s) {
             navigationDistance = QString::number((std::round(dist_ft / 50.0) * 50)) + " ft";
           }
         } else {
-          navigationDistance = QString::number(dist_ft / 5280, 'f', 1) + " mi";
+          float dist_mi = dist_ft / 5280;
+          if (dist_mi >= 10.0) {
+            navigationDistance = QString::number(std::floor(dist_mi)) + " mi";
+          } else {
+            navigationDistance = QString::number(dist_mi, 'f', 1) + " mi";
+          }
         }
       }
 
@@ -1086,10 +1096,15 @@ void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) {
 
   const int container_width = 1080;
   const int container_height = 225;
-  const int container_x = (surface_rect.width() - container_width) / 2;
+  int container_x = (surface_rect.width() - container_width) / 2;
   const int container_y = 62;
   const int border_radius = 42;
 
+
+  if (speedLimitAssistState == cereal::LongitudinalPlanSP::SpeedLimit::AssistState::PRE_ACTIVE) {
+    container_x += 190;
+  }
+
   QRect container_rect(container_x, container_y, container_width, container_height);
 
   p.setPen(Qt::NoPen);
@@ -1112,7 +1127,7 @@ void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) {
   // Distance
   p.setFont(InterFont(48, QFont::Bold));
   p.setPen(Qt::white);
-  QRect distance_rect(icon_x, icon_y + icon_size, icon_size, 38);
+  QRect distance_rect(icon_x - 25, icon_y + icon_size, icon_size + 50, 38);
   p.drawText(distance_rect, Qt::AlignCenter, navigationDistance);
 
   const int then_section_width = 180;

selfdrive/ui/sunnypilot/qt/onroad/model.cc

@@ -8,12 +8,6 @@
 #include "selfdrive/ui/sunnypilot/qt/onroad/model.h"
 
 
-void ModelRendererSP::drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius) {
-  painter.setBrush(QColor(255, 255, 255, 200));
-  painter.setPen(Qt::NoPen);
-  painter.drawEllipse(pos, radius, radius);
-}
-
 void ModelRendererSP::update_model(const cereal::ModelDataV2::Reader &model, const cereal::RadarState::LeadData::Reader &lead) {
   ModelRenderer::update_model(model, lead);
   const auto &model_position = model.getPosition();
@@ -73,26 +67,6 @@ void ModelRendererSP::draw(QPainter &painter, const QRect &surface_rect) {
     const bool right_blindspot = car_state.getRightBlindspot();
     drawBlindspot(painter, surface_rect, left_blindspot, right_blindspot);
   }
-
-  if (s->scene.visual_radar_tracks) {
-    if (sm.alive("liveTracks") && sm.rcv_frame("liveTracks") >= s->scene.started_frame) {
-      const auto &tracks = sm["liveTracks"].getLiveTracks().getPoints();
-      for (const auto &track : tracks) {
-        if (!std::isfinite(track.getDRel()) || !std::isfinite(track.getYRel())) continue;
-        float t_lag = s->scene.visual_radar_tracks_delay;
-        float d_pred = track.getDRel();
-        float y_pred = track.getYRel();
-        if (t_lag > 0.0f) {
-          d_pred += track.getVRel() * t_lag + 0.5f * track.getARel() * t_lag * t_lag;
-        }
-        QPointF screen_pt;
-        if (mapToScreen(d_pred, -y_pred, path_offset_z, &screen_pt)) {
-          drawRadarPoint(painter, screen_pt, track.getVRel(), 10.0f);
-        }
-      }
-    }
-  }
-
   drawLeadStatus(painter, surface_rect.height(), surface_rect.width());
 
   painter.restore();

selfdrive/ui/sunnypilot/qt/onroad/model.h

@@ -28,6 +28,4 @@ private:
 
   // Lead status animation
   float lead_status_alpha = 0.0f;
-
-  void drawRadarPoint(QPainter &painter, const QPointF &pos, float v_rel, float radius = 10.0f);
 };

selfdrive/ui/sunnypilot/ui.cc

@@ -29,7 +29,7 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "modelManagerSP", "selfdriveStateSP", "longitudinalPlanSP", "backupManagerSP",
     "carControl", "gpsLocationExternal", "gpsLocation", "liveTorqueParameters",
-    "carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd", "liveTracks"
+    "carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd"
   });
 
   // update timer
@@ -44,14 +44,6 @@ UIStateSP::UIStateSP(QObject *parent) : UIState(parent) {
   });
   param_watcher->addParam("DevUIInfo");
   param_watcher->addParam("StandstillTimer");
-  param_watcher->addParam("VisualRadarTracks");
-  param_watcher->addParam("VisualRadarTracksDelay");
-  param_watcher->addParam("VisualWideCam");
-  param_watcher->addParam("VisualStyle");
-  param_watcher->addParam("VisualStyleZoom");
-  param_watcher->addParam("VisualStyleOverhead");
-  param_watcher->addParam("VisualStyleOverheadZoom");
-  param_watcher->addParam("VisualStyleOverheadThreshold");
 }
 
 // This method overrides completely the update method from the parent class intentionally.
@@ -84,17 +76,6 @@ void ui_update_params_sp(UIStateSP *s) {
   s->scene.chevron_info = std::atoi(params.get("ChevronInfo").c_str());
   s->scene.blindspot_ui = params.getBool("BlindSpot");
   s->scene.rainbow_mode = params.getBool("RainbowMode");
-
-  s->scene.visual_radar_tracks = QString::fromStdString(params.get("VisualRadarTracks")).toInt();
-  s->scene.visual_radar_tracks_delay = QString::fromStdString(params.get("VisualRadarTracksDelay")).toFloat();
-
-  s->scene.visual_wide_cam = QString::fromStdString(params.get("VisualWideCam")).toInt();
-
-  s->scene.visual_style = QString::fromStdString(params.get("VisualStyle")).toInt();
-  s->scene.visual_style_zoom = QString::fromStdString(params.get("VisualStyleZoom")).toInt();
-  s->scene.visual_style_overhead = QString::fromStdString(params.get("VisualStyleOverhead")).toInt();
-  s->scene.visual_style_overhead_zoom = QString::fromStdString(params.get("VisualStyleOverheadZoom")).toInt();
-  s->scene.visual_style_overhead_threshold = QString::fromStdString(params.get("VisualStyleOverheadThreshold")).toInt();
 }
 
 void UIStateSP::reset_onroad_sleep_timer(OnroadTimerStatusToggle toggleTimerStatus) {

selfdrive/ui/sunnypilot/ui_scene.h

@@ -21,12 +21,4 @@ typedef struct UISceneSP : UIScene {
   int chevron_info;
   bool blindspot_ui;
   bool rainbow_mode;
-  int visual_radar_tracks = 0;
-  float visual_radar_tracks_delay = 0;
-  int visual_wide_cam = 0;
-  int visual_style = 0;
-  int visual_style_zoom = 0;
-  int visual_style_overhead = 0;
-  int visual_style_overhead_zoom = 0;
-  int visual_style_overhead_threshold = 20.0;
 } UISceneSP;

sunnypilot/navd/helpers.py

@@ -72,6 +72,15 @@ class Coordinate:
     return x * EARTH_MEAN_RADIUS
 
 
+def bearing_between_two_points(point_one: Coordinate, point_two: Coordinate) -> float:
+  dlon = math.radians(point_two.longitude - point_one.longitude)
+  bearing_radians = math.atan2(math.sin(dlon)* math.cos(point_two.latitude), math.cos(point_one.latitude) * math.sin(point_two.latitude) -
+                               math.sin(point_one.latitude) * math.cos(point_two.latitude) * math.cos(dlon))
+  bearing_degrees = math.degrees(bearing_radians)
+  bearing_normalized = (bearing_degrees + 360) % 360
+  return bearing_normalized
+
+
 def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate):
   if a.distance_to(b) < 0.01:
     return a.distance_to(p)

sunnypilot/navd/navigation_desires/navigation_desires.py

@@ -31,14 +31,12 @@ class NavigationDesires:
     self.desire = log.Desire.none
     if self.nav_allowed and nav_msg.valid and lateral_active:
       upcoming = nav_msg.upcomingTurn
-      if upcoming == 'slightLeft' and (not CS.leftBlindspot or CS.vEgo < self._turn_speed_limit):
+      if upcoming == 'slightLeft' and not CS.rightBlinker and not CS.leftBlindspot and CS.steeringPressed and CS.steeringTorque > 0:
         self.desire = log.Desire.keepLeft
-      elif upcoming == 'slightRight' and (not CS.rightBlindspot or CS.vEgo < self._turn_speed_limit):
+      elif upcoming == 'slightRight' and not CS.leftBlinker and not CS.rightBlindspot and CS.steeringPressed and CS.steeringTorque < 0:
         self.desire = log.Desire.keepRight
-      elif (upcoming == 'left' and CS.steeringPressed and CS.steeringTorque > 0 and not CS.rightBlinker
-            and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit):
+      elif upcoming == 'left' and not CS.rightBlinker and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit:
         self.desire = log.Desire.turnLeft
-      elif (upcoming == 'right' and CS.steeringPressed and CS.steeringTorque < 0 and not CS.leftBlinker
-            and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit):
+      elif upcoming == 'right' and not CS.leftBlinker and not CS.rightBlindspot and CS.vEgo < self._turn_speed_limit:
         self.desire = log.Desire.turnRight
     return self.desire

sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py

@@ -22,20 +22,20 @@ def make_car(vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False,
   )
 
 NAVIGATION_PARAMS: list[tuple] = [
-  ('slightLeft', make_car(), log.Desire.keepLeft),
-  ('slightRight', make_car(), log.Desire.keepRight),
+  ('slightLeft', make_car(steeringPressed=True, steeringTorque=1), log.Desire.keepLeft),
+  ('slightRight', make_car(steeringPressed=True, steeringTorque=-1), log.Desire.keepRight),
   ('slightLeft', make_car(vEgo=9, leftBlindspot=True), log.Desire.none),
   ('slightRight', make_car(vEgo=9, rightBlindspot=True), log.Desire.none),
-  ('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, steeringPressed=True, steeringTorque=1), log.Desire.turnLeft),
+  ('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False), log.Desire.turnLeft),
   ('left', make_car(vEgo=5, leftBlinker=False, rightBlinker=True), log.Desire.none),
-  ('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False, steeringPressed=True, steeringTorque=-1), log.Desire.turnRight),
+  ('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False), log.Desire.turnRight),
   ('right', make_car(vEgo=6, rightBlinker=True, rightBlindspot=True), log.Desire.none),
   ('left', make_car(vEgo=9, leftBlinker=True), log.Desire.none),
 ]
 
 INTEGRATION_PARAMS: list[tuple] = [(carstate, upcoming, log.Desire.none, expected) for upcoming, carstate, expected in NAVIGATION_PARAMS] + [
-  (make_car(vEgo=9, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
-  (make_car(vEgo=9, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
+  (make_car(vEgo=6, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
+  (make_car(vEgo=5, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
   (make_car(vEgo=9, leftBlinker=True), 'slightLeft', log.Desire.laneChangeLeft, log.Desire.laneChangeLeft),
   (make_car(vEgo=9, rightBlinker=True), 'slightRight', log.Desire.laneChangeRight, log.Desire.laneChangeRight),
   (make_car(vEgo=9), 'none', log.Desire.none, log.Desire.none),

sunnypilot/navd/navigation_helpers/nav_instructions.py

@@ -4,20 +4,25 @@ Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of oth
 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.
 """
-from openpilot.common.constants import CV
+from numpy import interp
+
 from openpilot.common.params import Params
 
-from openpilot.sunnypilot.navd.helpers import Coordinate, string_to_direction
+from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, string_to_direction, distance_along_geometry
 
 
 class NavigationInstructions:
   def __init__(self):
     self.coord = Coordinate(0, 0)
     self.params = Params()
+
     self._cached_route = None
     self._route_loaded = False
     self._no_route = False
 
+    self.closest_idx: float = 0
+    self.min_distance: float = 0
+
   def get_route_progress(self, current_lat, current_lon) -> dict | None:
     route = self.get_current_route()
     if not route or not route['geometry'] or not route['steps']:
@@ -27,8 +32,8 @@ class NavigationInstructions:
     self.coord.longitude = current_lon
 
     # Find the closest point on the route relative to self
-    closest_idx, min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
-    closest_cumulative = route['cumulative_distances'][closest_idx]
+    self.closest_idx, self.min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
+    closest_cumulative = distance_along_geometry(route['geometry'], self.coord)
 
     # Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
     current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
@@ -53,7 +58,7 @@ class NavigationInstructions:
       all_maneuvers.append({'distance': distance, 'type': step['maneuver'], 'modifier': step['modifier'], 'instruction': step['instruction']})
 
     return {
-      'distance_from_route': min_distance,
+      'distance_from_route': self.min_distance,
       'current_step': current_step,
       'next_turn': next_turn,
       'current_maxspeed': current_maxspeed,
@@ -94,6 +99,7 @@ class NavigationInstructions:
         'instruction': step['instruction'],
       })
     self._cached_route = {
+      'bearings': [bearing_between_two_points(geometry[i], geometry[i+2]) for i in range(len(geometry)-2)],
       'steps': steps,
       'total_distance': route['totalDistance'],
       'total_duration': route['totalDuration'],
@@ -109,30 +115,39 @@ class NavigationInstructions:
     self._route_loaded = False
     self._no_route = False
 
-  def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon) -> str:
+  def route_bearing_misalign(self, route, bearing, v_ego) -> bool:
+    route_bearing_misalign:bool = False
+
+    if v_ego < 5.0:
+      route_bearing_misalign = False
+    elif self.closest_idx > 0 and self.closest_idx < len(route['geometry']) -1:
+      route_bearing = route['bearings'][self.closest_idx -1]
+      current_bearing_normalized = (bearing + 360) % 360
+      bearing_difference = abs(current_bearing_normalized - route_bearing)
+
+      if min(bearing_difference, 360 - bearing_difference) > 110:
+        route_bearing_misalign = True  # flag for recompute/cancellation
+    return route_bearing_misalign
+
+  def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon, v_ego: float) -> str:
     if progress and progress['next_turn']:
+      speed_breakpoints: list = [0, 5, 10, 15, 20, 25, 30, 35, 40]
+      distance_breakpoints: list = [20, 25, 30, 45, 60, 75, 90, 105, 120]
+      distance_interp = interp(v_ego, speed_breakpoints, distance_breakpoints)
+
       self.coord.latitude = current_lat
       self.coord.longitude = current_lon
       distance = self.coord.distance_to(progress['next_turn']['location'])
-      if distance <= 30.0:
+
+      if distance <= distance_interp:
         modifier = progress['next_turn']['modifier']
         return str(modifier)
     return 'none'
 
   @staticmethod
-  def get_current_speed_limit_from_progress(progress, is_metric: bool) -> int:
-    if progress and progress['current_maxspeed']:
-      speed, _ = progress['current_maxspeed']
-      if is_metric:
-        return int(speed)
-      else:
-        return int(round(speed * CV.KPH_TO_MPH))
-    return 0
-
-  @staticmethod
-  def arrived_at_destination(progress) -> bool:
-    if progress['all_maneuvers'][0]['type'] == 'arrive':
-      return True
-    elif progress['all_maneuvers'][0]['instruction'].startswith('Your destination'):
-      return True
+  def arrived_at_destination(progress, v_ego) -> bool:
+    if v_ego < 1.0:
+      maneuvers = progress['all_maneuvers'][0]
+      if maneuvers['type'] == 'arrive' or maneuvers['instruction'].startswith('Your destination'):
+        return True
     return False

sunnypilot/navd/navigation_helpers/tests/test_mapbox.py

@@ -54,17 +54,17 @@ class TestMapbox:
         assert 'modifier' in step
 
   def test_upcoming_turn_detection(self):
-    upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon)
+    upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon, v_ego=40.0)
     assert isinstance(upcoming, str)
     assert upcoming == 'none'
 
     if self.route['steps']:
       turn_lat = self.route['steps'][1]['location'].latitude
       turn_lon = self.route['steps'][1]['location'].longitude
-      close_lat = turn_lat - 0.00025  # slightly before the turn
+      close_lat = turn_lat - 0.000175  # slightly before the turn
       if self.progress and self.progress.get('next_turn'):
         expected_turn = self.progress['next_turn']['modifier']
-        upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon)
+        upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon, v_ego=0.0)
         if expected_turn:
           assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"
 
@@ -79,16 +79,20 @@ class TestMapbox:
     assert isinstance(self.progress['all_maneuvers'], list)
 
   def test_speed_limit_handling(self):
-    speed_limit_metric = self.nav.get_current_speed_limit_from_progress(self.progress, True)
-    speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(self.progress, False)
+    speed_limit_metric = self.progress['current_maxspeed'][0]
+    speed_limit_imperial = (round(speed_limit_metric * CV.KPH_TO_MPH))
     assert isinstance(speed_limit_metric, int)
     assert isinstance(speed_limit_imperial, int)
-    expected_metric = int(self.progress['current_maxspeed'][0])
-    expected_imperial = int(round(self.progress['current_maxspeed'][0] * CV.KPH_TO_MPH))
-    assert speed_limit_metric == expected_metric
-    assert speed_limit_imperial == expected_imperial
 
   def test_arrival_detection(self):
-    is_arrived = self.nav.arrived_at_destination(self.progress)
+    is_arrived = self.nav.arrived_at_destination(self.progress, 2.0)
     assert isinstance(is_arrived, bool)
     assert not is_arrived
+
+  def test_bearing_misalign(self):
+    lat = self.route['steps'][1]['location'].latitude
+    lon = self.route['steps'][1]['location'].longitude
+    self.nav.get_route_progress(lat, lon)
+    route_bearing_misaligned = self.nav.route_bearing_misalign(self.route, 45, 5.0)
+    # based on math: closest index: 7, normalized bearing: 45 route bearing: 180.5486953778888, expected differential: 135.54869538
+    assert route_bearing_misaligned

sunnypilot/navd/navigationd.py

@@ -4,7 +4,8 @@ Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of oth
 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.
 """
-import math
+from math import degrees
+from numpy import interp
 
 import cereal.messaging as messaging
 from cereal import custom
@@ -24,7 +25,7 @@ class Navigationd:
     self.mapbox = MapboxIntegration()
     self.nav_instructions = NavigationInstructions()
 
-    self.sm = messaging.SubMaster(['liveLocationKalman'])
+    self.sm = messaging.SubMaster(['carControlSP', 'liveLocationKalman'])
     self.pm = messaging.PubMaster(['navigationd'])
     self.rk = Ratekeeper(3) # 3 Hz
 
@@ -41,26 +42,23 @@ class Navigationd:
     self.frame: int = -1
     self.last_position: Coordinate | None = None
     self.last_bearing: float | None = None
-    self.is_metric: bool = False
     self.valid: bool = False
 
   def _update_params(self):
     if self.last_position is not None:
       self.frame += 1
-      if self.frame % 9 == 0:
+      if self.frame % 15 == 0:
         self.allow_navigation = self.params.get('AllowNavigation', return_default=True)
-        self.is_metric = self.params.get('IsMetric', return_default=True)
         self.new_destination = self.params.get('MapboxRoute')
         self.recompute_allowed = self.params.get('MapboxRecompute', return_default=True)
 
       self.allow_recompute: bool = (self.new_destination != self.destination and self.new_destination != '') or (
-        self.recompute_allowed and self.reroute_counter > 9 and self.route
-      )
+        self.recompute_allowed and self.reroute_counter > 9 and self.route)
 
       if self.allow_recompute:
         postvars = {'place_name': self.new_destination}
         postvars, valid_addr = self.mapbox.set_destination(postvars, self.last_position.longitude, self.last_position.latitude, self.last_bearing)
-        cloudlog.debug(f'Set new destination to: {self.new_destination}, valid: {valid_addr}')
+
         if valid_addr:
           self.destination = self.new_destination
           self.nav_instructions.clear_route_cache()
@@ -70,7 +68,7 @@ class Navigationd:
 
       if self.cancel_route_counter == 30:
         self.cancel_route_counter = 0
-        self.destination = None
+        self.params.put_nonblocking("MapboxRoute", "")
         self.nav_instructions.clear_route_cache()
         self.route = None
 
@@ -79,11 +77,14 @@ class Navigationd:
   def _update_navigation(self) -> tuple[str, dict | None, dict]:
     banner_instructions: str = ''
     nav_data: dict = {}
-    if self.allow_navigation and self.last_position is not None:
+    if self.allow_navigation and self.route and self.last_position is not None:
       if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude):
-        nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude)
-        nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric)
-        arrived = self.nav_instructions.arrived_at_destination(progress)
+        v_ego = float(max(self.sm['carControlSP'].speed, 0.0))
+        nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude,
+                                                                                          self.last_position.longitude, v_ego)
+        speed_limit, _ = progress['current_maxspeed']
+        nav_data['current_speed_limit'] = speed_limit
+        arrived = self.nav_instructions.arrived_at_destination(progress, v_ego)
 
         if progress['current_step']:
           parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step'])
@@ -91,27 +92,35 @@ class Navigationd:
             banner_instructions = parsed['maneuverPrimaryText']
 
         nav_data['distance_from_route'] = progress['distance_from_route']
-        large_distance = progress['distance_from_route'] > 100
+        speed_breakpoints: list = [0.0, 5.0, 10.0, 20.0, 40.0]
+        distance_list: list = [100.0, 125.0, 150.0, 200.0, 250.0]
+        large_distance: bool = progress['distance_from_route'] > float(interp(v_ego, speed_breakpoints, distance_list))
 
-        if large_distance:
+        route_bearing_misalign: bool = self.nav_instructions.route_bearing_misalign(self.route, self.last_bearing, v_ego)
+
+        if large_distance and not arrived:
           self.cancel_route_counter = self.cancel_route_counter + 1 if progress['distance_from_route'] > NAV_CV.QUARTER_MILE else 0
           if self.recompute_allowed:
             self.reroute_counter += 1
         elif arrived:
           self.cancel_route_counter += 1
+          self.recompute_allowed = False
+        elif route_bearing_misalign:
+          self.cancel_route_counter += 1
+          if self.recompute_allowed:
+            self.reroute_counter += 1
         else:
           self.cancel_route_counter = 0
           self.reroute_counter = 0
 
         # Don't recompute in last segment to prevent reroute loops
-        if self.route:
-          if progress['current_step_idx'] == len(self.route['steps']) - 1:
-            self.allow_recompute = False
+        if progress['current_step_idx'] == len(self.route['steps']) - 1:
+          self.recompute_allowed = False
+          self.allow_navigation = False
     else:
       banner_instructions = ''
       progress = None
       nav_data = {}
-      self.valid = False
 
     return banner_instructions, progress, nav_data
 
@@ -131,19 +140,18 @@ class Navigationd:
       else []
     )
     msg.navigationd.allManeuvers = all_maneuvers
-
     return msg
 
   def run(self):
     cloudlog.warning('navigationd init')
 
     while True:
-      self.sm.update()
+      self.sm.update(0)
       location = self.sm['liveLocationKalman']
       localizer_valid = location.positionGeodetic.valid if location else False
 
       if localizer_valid:
-        self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
+        self.last_bearing = degrees(location.calibratedOrientationNED.value[2])
         self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
 
       self._update_params()

sunnypilot/navd/tests/test_navigationd.py

@@ -65,12 +65,3 @@ class TestNavigationd:
 
       assert received_msg.bannerInstructions == msg.navigationd.bannerInstructions
       assert received_msg.valid == msg.navigationd.valid
-
-  def test_cancel_route(self):
-    nav = Navigationd()
-    nav.last_position = Coordinate(latitude=37.0, longitude=128.0)
-    nav.route = {'580 Winchester dr, oxnard, CA': True}
-    nav.cancel_route_counter = 30
-    nav._update_params()
-    assert nav.route is None
-    assert nav.destination is None

sunnypilot/selfdrive/controls/controlsd_ext.py

@@ -72,6 +72,8 @@ class ControlsExt:
 
     CC_SP.intelligentCruiseButtonManagement = sm['selfdriveStateSP'].intelligentCruiseButtonManagement
 
+    CC_SP.speed = sm['carState'].vEgo
+
     return CC_SP
 
   @staticmethod