some more

#	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: |

CHANGELOG.md

@@ -1,6 +1,30 @@
-sunnypilot Version 2025.002.000 (2025-xx-xx)
+sunnypilot Version 2025.003.000 (20xx-xx-xx)
 ========================
 
+sunnypilot Version 2025.002.000 (2025-11-06)
+========================
+* What's Changed (sunnypilot/sunnypilot)
+  * models: bump model json to v8 by @Discountchubbs
+  * Bug: Model UI Crash Fix by @nayan8teen
+  * controlsd: add `CP_SP` to `get_pid_accel_limits` by @THERoenPR
+  * sunnylink: update uploader button logic to support novice tier and above by @devtekve
+  * Tesla: Coop Steering by @AmyJeanes
+  * ui: update discord references and add forum widget by @devtekve
+  * ui: Fix spacing in sunnylink panel by @devtekve
+  * docs: Update README installation branches and discord links by @mpurnell1 in
+  * stats: sunnylink integration by @devtekve
+  * bug: Fix initial registration for sunnylink by @devtekve
+* What's Changed (sunnypilot/opendbc)
+  * Honda: add brake hold messages for Clarity by @mvl-boston
+  * interface: add `CP_SP` to `get_pid_accel_limits` method signature by @roenthomas
+  * Honda: use fixed accel min/max constants for Gas Interceptor by @roenthomas
+  * Tesla: Coop Steering by @AmyJeanes
+* New Contributors (sunnypilot/sunnypilot)
+  * @THERoenPR made their first contribution in "controlsd: add `CP_SP` to `get_pid_accel_limits`"
+  * @AmyJeanes made their first contribution in "Tesla: Coop Steering"
+  * @mpurnell1 made their first contribution in "docs: Update README installation branches and discord links"
+* Full Changelog: https://github.com/sunnypilot/sunnypilot/compare/v2025.001.000...v2025.002.000
+
 sunnypilot Version 2025.001.000 (2025-10-25)
 ========================
 * 🛠️ Major rewrite

README.md

@@ -3,11 +3,9 @@
 ## 🌞 What is sunnypilot?
 [sunnypilot](https://github.com/sunnyhaibin/sunnypilot) is a fork of comma.ai's openpilot, an open source driver assistance system. sunnypilot offers the user a unique driving experience for over 300+ supported car makes and models with modified behaviors of driving assist engagements. sunnypilot complies with comma.ai's safety rules as accurately as possible.
 
-## 💭 Join our Discord
-Join the official sunnypilot Discord server to stay up to date with all the latest features and be a part of shaping the future of sunnypilot!
-* https://discord.gg/sunnypilot
-
-  ![](https://dcbadge.vercel.app/api/server/wRW3meAgtx?style=flat) ![Discord Shield](https://discordapp.com/api/guilds/880416502577266699/widget.png?style=shield)
+## 💭 Join our Community Forum
+Join the official sunnypilot community forum to stay up to date with all the latest features and be a part of shaping the future of sunnypilot!
+* https://community.sunnypilot.ai/
 
 ## Documentation
 https://docs.sunnypilot.ai/ is your one stop shop for everything from features to installation to FAQ about the sunnypilot
@@ -16,13 +14,13 @@ https://docs.sunnypilot.ai/ is your one stop shop for everything from features t
 * A supported device to run this software
     * a [comma three](https://comma.ai/shop/products/three) or a [C3X](https://comma.ai/shop/comma-3x)
 * This software
-* One of [the 300+ supported cars](https://github.com/commaai/openpilot/blob/master/docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run sunnypilot.
+* One of [the 325+ supported cars](https://github.com/sunnypilot/sunnypilot/blob/master/docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, Ford, and more. If your car is not supported but has adaptive cruise control and lane-keeping assist, it's likely able to run sunnypilot.
 * A [car harness](https://comma.ai/shop/products/car-harness) to connect to your car
 
 Detailed instructions for [how to mount the device in a car](https://comma.ai/setup).
 
 ## Installation
-Please refer to [Recommended Branches](#-recommended-branches) to find your preferred/supported branch. This guide will assume you want to install the latest `staging-c3-new` branch.
+Please refer to [Recommended Branches](#recommended-branches) to find your preferred/supported branch. This guide will assume you want to install the latest `staging` branch.
 
 ### If you want to use our newest branches (our rewrite)
 > [!TIP]
@@ -31,28 +29,28 @@ Please refer to [Recommended Branches](#-recommended-branches) to find your pref
 * sunnypilot not installed or you installed a version before 0.8.17?
   1. [Factory reset/uninstall](https://github.com/commaai/openpilot/wiki/FAQ#how-can-i-reset-the-device) the previous software if you have another software/fork installed.
   2. After factory reset/uninstall and upon reboot, select `Custom Software` when given the option.
-  3. Input the installation URL per [Recommended Branches](#-recommended-branches). Example: ```https://staging-c3-new.sunnypilot.ai```.
+  3. Input the installation URL per [Recommended Branches](#recommended-branches). Example: ```https://staging.sunnypilot.ai```.
   4. Complete the rest of the installation following the onscreen instructions.
 
 * sunnypilot already installed and you installed a version after 0.8.17?
-  1. On the comma three, go to `Settings` ▶️ `Software`.
+  1. On the comma three/3X, go to `Settings` ▶️ `Software`.
   2. At the `Download` option, press `CHECK`. This will fetch the list of latest branches from sunnypilot.
   3. At the `Target Branch` option, press `SELECT` to open the Target Branch selector.
-  4. Scroll to select the desired branch per  Recommended Branches (see below). Example: `staging-c3-new`
+  4. Scroll to select the desired branch per  Recommended Branches (see below). Example: `staging`
 
-
-|      Branch      |                 Installation URL              |
-|:----------------:|:---------------------------------------------:|
-| `staging-c3-new` | `https://staging-c3-new.sunnypilot.ai`        |
-|   `dev-c3-new`   | `https://dev-c3-new.sunnypilot.ai`            |
-| `custom-branch`  | `https://install.sunnypilot.ai/{branch_name}` |
-| `release-c3-new` |            **Not yet available**.             |
+### Recommended Branches
+|      Branch     |                 Installation URL              |
+|:---------------:|:---------------------------------------------:|
+| `release`       | `https://release.sunnypilot.ai`               |
+| `staging`       | `https://staging.sunnypilot.ai`               |
+|   `dev`         | `https://dev.sunnypilot.ai`                   |
+| `custom-branch` | `https://install.sunnypilot.ai/{branch_name}` |
 
 > [!TIP]
-> You can use sunnypilot/targetbranch as an install URL. Example: 'sunnypilot/staging-c3-new'.
+> You can use sunnypilot/targetbranch as an install URL. Example: 'sunnypilot/staging'.
 
 > [!NOTE]
-> Do you require further assistance with software installation? Join the [sunnypilot Discord server](https://discord.sunnypilot.com) and message us in the `#installation-help` channel.
+> Do you require further assistance with software installation? Join the [sunnypilot community forum](https://community.sunnypilot.ai/new-topic?category=general/qa) and create a topic in the General/Q&A Category channel.
 
 
 <details>

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

@@ -216,6 +216,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"HyundaiLongitudinalTuning", {PERSISTENT | BACKUP, INT, "0"}},
     {"SubaruStopAndGo", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"SubaruStopAndGoManualParkingBrake", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"TeslaCoopSteering", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},

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/widgets/wifi.cc

@@ -11,17 +11,18 @@ WiFiPromptWidget::WiFiPromptWidget(QWidget *parent) : QFrame(parent) {
   main_layout->setContentsMargins(56, 40, 56, 40);
   main_layout->setSpacing(42);  
   
-  QLabel *title = new QLabel(tr("<span style='font-family: \"Noto Color Emoji\";'>🔥</span> Firehose Mode <span style='font-family: Noto Color Emoji;'>🔥</span>"));
-  title->setStyleSheet("font-size: 64px; font-weight: 500;");
+  community_popup = new SunnylinkCommunityPopup(this);
+  QLabel *title = new QLabel(tr("sunnypilot Community"));
+  title->setStyleSheet("font-size: 56px; font-weight: 500;");
   main_layout->addWidget(title);
 
-  QLabel *desc = new QLabel(tr("Maximize your training data uploads to improve openpilot's driving models."));
+  QLabel *desc = new QLabel(tr("Need help or have ideas?<br><b>Join</b> our community now!"));
   desc->setStyleSheet("font-size: 40px; font-weight: 400;");
   desc->setWordWrap(true);
   main_layout->addWidget(desc);
 
-  QPushButton *settings_btn = new QPushButton(tr("Open"));
-  connect(settings_btn, &QPushButton::clicked, [=]() { emit openSettings(1, "FirehosePanel"); });
+  QPushButton *settings_btn = new QPushButton(tr("Learn More"));
+  connect(settings_btn, &QPushButton::clicked, [=]() { community_popup->exec(); });
   settings_btn->setStyleSheet(R"(
     QPushButton {
       font-size: 48px;

selfdrive/ui/qt/widgets/wifi.h

@@ -3,12 +3,17 @@
 #include <QFrame>
 #include <QWidget>
 
+#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink/community_widget.h"
+
 class WiFiPromptWidget : public QFrame {
   Q_OBJECT
 
 public:
   explicit WiFiPromptWidget(QWidget* parent = 0);
 
+private:
+  SunnylinkCommunityPopup *community_popup;
+
 signals:
   void openSettings(int index = 0, const QString &param = "");
 };

selfdrive/ui/sunnypilot/SConscript

@@ -36,6 +36,7 @@ qt_src = [
   "sunnypilot/qt/offroad/settings/software_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnylink_panel.cc",
   "sunnypilot/qt/offroad/settings/sunnylink/sponsor_widget.cc",
+  "sunnypilot/qt/offroad/settings/sunnylink/community_widget.cc",
   "sunnypilot/qt/offroad/settings/trips_panel.cc",
   "sunnypilot/qt/offroad/settings/vehicle_panel.cc",
   "sunnypilot/qt/offroad/settings/visuals_panel.cc",

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink/community_widget.cc

@@ -0,0 +1,139 @@
+/**
+ * Copyright (c) 2025-, sunnypilot contributors.
+ *
+ * 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 "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink/community_widget.h"
+
+#include "selfdrive/ui/sunnypilot/ui.h"
+#include "selfdrive/ui/sunnypilot/qt/util.h"
+
+using qrcodegen::QrCode;
+
+// --- SunnylinkCommunityQRWidget ---
+
+SunnylinkCommunityQRWidget::SunnylinkCommunityQRWidget(QWidget* parent)
+    : QWidget(parent) {}
+
+void SunnylinkCommunityQRWidget::showEvent(QShowEvent *event) {
+  updateQrCode(SUNNYLINK_COMMUNITY_URL);
+  update();
+}
+
+void SunnylinkCommunityQRWidget::updateQrCode(const QString &text) {
+  QrCode qr = QrCode::encodeText(text.toUtf8().data(), QrCode::Ecc::LOW);
+  qint32 sz = qr.getSize();
+  QImage im(sz, sz, QImage::Format_RGB32);
+
+  QRgb black = qRgb(0, 0, 0);
+  QRgb white = qRgb(255, 255, 255);
+  for (int y = 0; y < sz; y++) {
+    for (int x = 0; x < sz; x++) {
+      im.setPixel(x, y, qr.getModule(x, y) ? black : white);
+    }
+  }
+
+  int final_sz = ((width() / sz) - 1) * sz;
+  img = QPixmap::fromImage(im.scaled(final_sz, final_sz, Qt::KeepAspectRatio), Qt::MonoOnly);
+}
+
+void SunnylinkCommunityQRWidget::paintEvent(QPaintEvent *e) {
+  QPainter p(this);
+  p.fillRect(rect(), Qt::white);
+
+  if (!img.isNull()) {
+    QSize s = (size() - img.size()) / 2;
+    p.drawPixmap(s.width(), s.height(), img);
+  }
+}
+
+// --- SunnylinkCommunityPopup ---
+
+QStringList SunnylinkCommunityPopup::getInstructions() {
+  QStringList instructions;
+  instructions << tr("Scan the QR code and join us!");
+  return instructions;
+}
+
+SunnylinkCommunityPopup::SunnylinkCommunityPopup(QWidget* parent)
+    : DialogBase(parent) {
+  auto *mainLayout = new QVBoxLayout(this);
+  mainLayout->setContentsMargins(0, 0, 0, 0);
+  mainLayout->setSpacing(0);
+
+  // Solarized Light base3 background
+  setStyleSheet("SunnylinkCommunityPopup { background-color: #FDF6E3; }");
+
+  // Header spanning full width
+  auto headerWidget = new QWidget(this);
+  auto headerLayout = new QHBoxLayout(headerWidget);
+  headerLayout->setContentsMargins(85, 50, 85, 30);
+  headerLayout->setSpacing(30);
+
+  auto close = new QPushButton(QIcon(":/icons/close.svg"), "", this);
+  close->setIconSize(QSize(80, 80));
+  close->setStyleSheet("border: none;");
+  connect(close, &QPushButton::clicked, this, &QDialog::reject);
+  headerLayout->addWidget(close, 0, Qt::AlignLeft | Qt::AlignVCenter);
+
+  const auto title = new QLabel(tr("Join the sunnypilot Community Forum"), this);
+  // Solarized base02 for text
+  title->setStyleSheet("font-size: 65px; color: #073642;");
+  title->setWordWrap(false);
+  title->setAlignment(Qt::AlignCenter);
+  headerLayout->addWidget(title, 1);
+
+  // Spacer to balance the close button on the right
+  auto spacer = new QWidget(this);
+  spacer->setFixedSize(80, 80);
+  headerLayout->addWidget(spacer, 0);
+
+  mainLayout->addWidget(headerWidget);
+
+  // Two-column content layout
+  auto contentLayout = new QHBoxLayout();
+  contentLayout->setContentsMargins(0, 0, 0, 0);
+  contentLayout->setSpacing(0);
+  mainLayout->addLayout(contentLayout, 66);
+
+  // Left side: description
+  auto leftLayout = new QVBoxLayout();
+  leftLayout->setContentsMargins(85, 40, 50, 70);
+  leftLayout->setSpacing(35);
+  contentLayout->addLayout(leftLayout, 40);
+
+  // Hype / intro paragraph
+  const auto desc = new QLabel(tr(
+    "We're excited to announce our <b>sunnypilot Community Forum</b><br><br>"
+    "Over the years, Discord just hasn't scaled well for our growing community.<br>"
+    "It's noisy, unsearchable, and great discussions disappear too easily.<br>"
+    "Our new community forum aims to fix that by making it easier to <b>find answers, share ideas, track feedback, report bugs, help newcomers</b> and more!<br><br>"
+    "<b>Here's what's waiting for you:</b><br>"
+    "• Fully <b>indexable</b> and discoverable through search engines 🔎<br>"
+    "• <b>AI-powered</b>🤖 topic and chat summaries, spam detection, and more<br>"
+    "• A <b>trust-level system</b>✅ that rewards meaningful contributions<br>"
+    "• Designed to work <b>on your own time</b>.🧘<br><br>"
+    "Scan the QR code on the right and join the discussion!"
+  ), this);
+  // Solarized base01 for body text
+  desc->setStyleSheet("font-size: 40px; color: #586E75;");
+  desc->setWordWrap(true);
+  leftLayout->addWidget(desc);
+
+  leftLayout->addStretch();
+
+  // Right side: QR code and instructions
+  auto rightLayout = new QVBoxLayout();
+  rightLayout->setContentsMargins(50, 40, 85, 70);
+  rightLayout->setSpacing(40);
+  contentLayout->addLayout(rightLayout, 1);
+
+  // QR code (smaller, fixed size)
+  auto *qr = new SunnylinkCommunityQRWidget(this);
+  qr->setFixedSize(500, 500);
+  rightLayout->addStretch();
+  rightLayout->addWidget(qr, 0, Qt::AlignCenter);
+  rightLayout->addStretch();
+}

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink/community_widget.h

@@ -0,0 +1,40 @@
+/**
+* Copyright (c) 2025-, sunnypilot contributors.
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <QrCode.hpp>
+#include <QtCore/qjsonobject.h>
+
+#include "common/util.h"
+#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h"
+
+const QString SUNNYLINK_COMMUNITY_URL = "https://community.sunnypilot.ai/sp-qr";
+
+class SunnylinkCommunityQRWidget : public QWidget {
+  Q_OBJECT
+
+public:
+  explicit SunnylinkCommunityQRWidget(QWidget* parent = nullptr);
+  void paintEvent(QPaintEvent*) override;
+
+private:
+  QPixmap img;
+  void updateQrCode(const QString &text);
+  void showEvent(QShowEvent *event) override;
+};
+
+// Popup widget
+class SunnylinkCommunityPopup : public DialogBase {
+  Q_OBJECT
+
+public:
+  explicit SunnylinkCommunityPopup(QWidget* parent = nullptr);
+
+private:
+  static QStringList getInstructions();
+};

selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink/sponsor_widget.cc

@@ -79,11 +79,11 @@ QStringList SunnylinkSponsorPopup::getInstructions(bool sponsor_pair) {
     instructions << tr("Scan the QR code to login to your GitHub account")
                  << tr("Follow the prompts to complete the pairing process")
                  << tr("Re-enter the \"sunnylink\" panel to verify sponsorship status")
-                 << tr("If sponsorship status was not updated, please contact a moderator on Discord at https://discord.gg/sunnypilot");
+                 << tr("If sponsorship status was not updated, please contact a moderator on our forum at https://community.sunnypilot.ai");
   } else {
     instructions << tr("Scan the QR code to visit sunnyhaibin's GitHub Sponsors page")
                  << tr("Choose your sponsorship tier and confirm your support")
-                 << tr("Join our community on Discord at https://discord.gg/sunnypilot and reach out to a moderator to confirm your sponsor status");
+                 << tr("Join our Community Forum at https://community.sunnypilot.ai and reach out to a moderator if you have issues");
   }
   return instructions;
 }

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

@@ -90,7 +90,7 @@ SunnylinkPanel::SunnylinkPanel(QWidget *parent) : QFrame(parent) {
   QString sunnylinkUploaderDesc = tr("Enable sunnylink uploader to allow sunnypilot to upload your driving data to sunnypilot servers. (only for highest tiers, and does NOT bring ANY benefit to you. We are just testing data volume.)");
   sunnylinkUploaderEnabledBtn = new ParamControlSP(
     "EnableSunnylinkUploader",
-    tr("Enable sunnylink uploader (just for testing infrastructure)"),
+    tr("Enable sunnylink uploader (infrastructure test)"),
     sunnylinkUploaderDesc,
     "", nullptr, true);
   list->addItem(sunnylinkUploaderEnabledBtn);

selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/tesla_settings.cc

@@ -8,7 +8,41 @@
 #include "selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/tesla_settings.h"
 
 TeslaSettings::TeslaSettings(QWidget *parent) : BrandSettingsInterface(parent) {
+  constexpr int coopSteeringMinKmh = 23; // minimum speed for cooperative steering (enforced by Tesla firmware)
+  constexpr int oemSteeringMinKmh = 48; // minimum speed for OEM lane departure avoidance (enforced by Tesla firmware)
+  bool is_metric = params.getBool("IsMetric");
+  QString unit = is_metric ? "km/h" : "mph";
+  int display_value_coop;
+  int display_value_oem;
+  if (is_metric) {
+    display_value_coop = coopSteeringMinKmh;
+    display_value_oem = oemSteeringMinKmh;
+  } else {
+    display_value_coop = static_cast<int>(std::round(coopSteeringMinKmh * KM_TO_MILE));
+    display_value_oem = static_cast<int>(std::round(oemSteeringMinKmh * KM_TO_MILE));
+  }
+  const QString coop_desc = QString("<b>%1</b><br><br>"
+                                    "%2<br>"
+                                    "%3<br>")
+                            .arg(tr("Warning: May experience steering oscillations below %5 %6 during turns, recommend disabling this feature if you experience these."))
+                            .arg(tr("Allows the driver to provide limited steering input while openpilot is engaged."))
+                            .arg(tr("Only works above %4 %6."))
+                            .arg(display_value_coop)
+                            .arg(display_value_oem)
+                            .arg(unit);
+
+  coopSteeringToggle = new ParamControlSP(
+    "TeslaCoopSteering",
+    tr("Cooperative Steering (Beta)"),
+    coop_desc,
+    "",
+    this
+  );
+  list->addItem(coopSteeringToggle);
+  coopSteeringToggle->showDescription();
+  coopSteeringToggle->setConfirmation(true, false);
 }
 
 void TeslaSettings::updateSettings() {
+  coopSteeringToggle->setEnabled(offroad);
 }

selfdrive/ui/sunnypilot/qt/offroad/settings/vehicle/tesla_settings.h

@@ -22,5 +22,5 @@ public:
   void updateSettings() override;
 
 private:
-  bool offroad = false;
+  ParamControlSP *coopSteeringToggle = nullptr;
 };

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;

sunnypilot/common/version.h

@@ -1 +1 @@
-#define SUNNYPILOT_VERSION "2025.002.000"
+#define SUNNYPILOT_VERSION "2025.003.000"

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
 
 
 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 = route['cumulative_distances'][self.closest_idx]
 
     # 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,
@@ -109,30 +114,41 @@ 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:
+      current_coord = route['geometry'][self.closest_idx - 1]
+      future_coord = route['geometry'][self.closest_idx + 1]
+      route_bearing = bearing_between_two_points(current_coord, future_coord)
+      current_bearing_normalized = (bearing + 360) % 360
+      bearing_difference = abs(current_bearing_normalized - route_bearing)
+
+      if min(bearing_difference, 360 - bearing_difference) > 91:
+        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()
@@ -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/car/interfaces.py

@@ -15,6 +15,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.helpers import set_
 
 import openpilot.system.sentry as sentry
 
+from sunnypilot.sunnylink.statsd import STATSLOGSP
+
 
 def log_fingerprint(CP: structs.CarParams) -> None:
   if CP.carFingerprint == "MOCK":
@@ -100,6 +102,9 @@ def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None:
   _initialize_torque_lateral_control(CI, CP, enforce_torque, nnlc_enabled)
   _cleanup_unsupported_params(CP, CP_SP)
 
+  STATSLOGSP.raw('sunnypilot.car_params', CP.to_dict())
+  # STATSLOGSP.raw('sunnypilot_params.car_params_sp', CP_SP.to_dict()) # https://github.com/sunnypilot/opendbc/pull/361
+
 
 def initialize_params(params) -> list[dict[str, Any]]:
   keys: list = []
@@ -115,4 +120,9 @@ def initialize_params(params) -> list[dict[str, Any]]:
     "SubaruStopAndGoManualParkingBrake",
   ])
 
+  # tesla
+  keys.extend([
+    "TeslaCoopSteering",
+  ])
+
   return [{k: params.get(k, return_default=True)} for k in keys]

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

sunnypilot/sunnylink/athena/sunnylinkd.py

@@ -16,8 +16,9 @@ from functools import partial
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_core_affinity
 from openpilot.common.swaglog import cloudlog
+from openpilot.system.hardware.hw import Paths
 from openpilot.system.athena.athenad import ws_send, jsonrpc_handler, \
-  recv_queue, UploadQueueCache, upload_queue, cur_upload_items, backoff, ws_manage, log_handler, start_local_proxy_shim, upload_handler
+  recv_queue, UploadQueueCache, upload_queue, cur_upload_items, backoff, ws_manage, log_handler, start_local_proxy_shim, upload_handler, stat_handler
 from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutException,
                        create_connection, WebSocketConnectionClosedException)
 
@@ -33,9 +34,6 @@ SUNNYLINK_RECONNECT_TIMEOUT_S = 70  # FYI changing this will also would require
 DISALLOW_LOG_UPLOAD = threading.Event()
 
 params = Params()
-sunnylink_dongle_id = params.get("SunnylinkDongleId")
-sunnylink_api = SunnylinkApi(sunnylink_dongle_id)
-
 
 def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
   cloudlog.info("sunnylinkd.handle_long_poll started")
@@ -51,7 +49,7 @@ def handle_long_poll(ws: WebSocket, exit_event: threading.Event | None) -> None:
               threading.Thread(target=ws_queue, args=(end_event,), name='ws_queue'),
               threading.Thread(target=upload_handler, args=(end_event,), name='upload_handler'),
               # threading.Thread(target=sunny_log_handler, args=(end_event, comma_prime_cellular_end_event), name='log_handler'),
-              # threading.Thread(target=stat_handler, args=(end_event,), name='stat_handler'),
+              threading.Thread(target=stat_handler, args=(end_event, Paths.stats_sp_root(), True), name='stat_handler'),
             ] + [
               threading.Thread(target=jsonrpc_handler, args=(end_event, partial(startLocalProxy, end_event),), name=f'worker_{x}')
               for x in range(HANDLER_THREADS)
@@ -132,6 +130,8 @@ def ws_ping(ws: WebSocket, end_event: threading.Event) -> None:
 
 
 def ws_queue(end_event: threading.Event) -> None:
+  sunnylink_dongle_id = params.get("SunnylinkDongleId")
+  sunnylink_api = SunnylinkApi(sunnylink_dongle_id)
   resume_requested = False
   tries = 0
 
@@ -233,6 +233,9 @@ def saveParams(params_to_update: dict[str, str], compression: bool = False) -> N
 
 
 def startLocalProxy(global_end_event: threading.Event, remote_ws_uri: str, local_port: int) -> dict[str, int]:
+  sunnylink_dongle_id = params.get("SunnylinkDongleId")
+  sunnylink_api = SunnylinkApi(sunnylink_dongle_id)
+
   cloudlog.debug("athena.startLocalProxy.starting")
   ws = create_connection(
     remote_ws_uri,
@@ -254,6 +257,8 @@ def main(exit_event: threading.Event = None):
     cloudlog.info("Waiting for sunnylink registration to complete")
     time.sleep(10)
 
+  sunnylink_dongle_id = params.get("SunnylinkDongleId")
+  sunnylink_api = SunnylinkApi(sunnylink_dongle_id)
   UploadQueueCache.initialize(upload_queue)
 
   ws_uri = f"{SUNNYLINK_ATHENA_HOST}"

sunnypilot/sunnylink/statsd.py

@@ -0,0 +1,278 @@
+#!/usr/bin/env python3
+import base64
+import json
+import os
+import threading
+import traceback
+
+import zmq
+import time
+import uuid
+from pathlib import Path
+from collections import defaultdict
+from datetime import datetime, UTC
+
+from openpilot.common.params import Params
+from cereal.messaging import SubMaster
+from openpilot.system.hardware.hw import Paths
+from openpilot.common.swaglog import cloudlog
+from openpilot.system.hardware import HARDWARE
+from openpilot.common.file_helpers import atomic_write_in_dir
+from openpilot.system.version import get_build_metadata
+from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET, STATS_FLUSH_TIME_S
+from openpilot.system.statsd import METRIC_TYPE, StatLogSP
+from openpilot.common.realtime import Ratekeeper
+
+STATSLOGSP = StatLogSP(intercept=False)
+
+def sp_stats(end_event):
+  """Collect sunnypilot-specific statistics and send as raw metrics."""
+  rk = Ratekeeper(.1, print_delay_threshold=None)
+  statlogsp = STATSLOGSP
+  params = Params()
+
+  def flatten_dict(d, parent_key='', sep='.'):
+    items = {}
+    if isinstance(d, dict):
+      for k, v in d.items():
+        new_key = f"{parent_key}{sep}{k}" if parent_key else k
+        items.update(flatten_dict(v, new_key, sep=sep))
+    elif isinstance(d, (list, tuple)):
+      for i, v in enumerate(d):
+        new_key = f"{parent_key}[{i}]"
+        items.update(flatten_dict(v, new_key, sep=sep))
+    else:
+      items[parent_key] = d
+    return items
+
+  # Collect sunnypilot parameters
+  stats_dict = {}
+
+  param_keys = [
+    'SunnylinkEnabled',
+    'AutoLaneChangeBsmDelay',
+    'AutoLaneChangeTimer',
+    'CarPlatformBundle',
+    'CurrentRoute',
+    'DevUIInfo',
+    'EnableCopyparty',
+    'IntelligentCruiseButtonManagement',
+    'QuietMode',
+    'RainbowMode',
+    'ShowAdvancedControls',
+    'Mads',
+    'MadsMainCruiseAllowed',
+    'MadsSteeringMode',
+    'MadsUnifiedEngagementMode',
+    'ModelManager_ActiveBundle',
+    'ModelManager_Favs',
+    'EnableSunnylinkUploader',
+    'SunnylinkEnabled',
+    'InstallDate',
+    'UptimeOffroad',
+    'UptimeOnroad',
+  ]
+
+  while not end_event.is_set():
+    try:
+      for key in param_keys:
+
+        try:
+          value = params.get(key)
+        except Exception as e:
+          stats_dict[key] = e
+          continue
+
+        if value is None:
+          continue
+
+        if isinstance(value, (dict, list, tuple)):
+          stats_dict.update(flatten_dict(value, key))
+        else:
+          stats_dict[key] = value
+
+      if stats_dict:
+        statlogsp.raw('sunnypilot.device_params', stats_dict)
+    except Exception as e:
+      cloudlog.error(f"Exception {e}")
+    finally:
+      rk.keep_time()
+
+
+def stats_main(end_event):
+  comma_dongle_id = Params().get("DongleId")
+  sunnylink_dongle_id = Params().get("SunnylinkDongleId")
+
+  def get_influxdb_line(measurement: str, value: float | dict[str, float], timestamp: datetime, tags: dict) -> str:
+    res = f"{measurement}"
+    for k, v in tags.items():
+      res += f",{k}={str(v)}"
+    res += " "
+
+    if isinstance(value, float):
+      value = {'value': value}
+
+    for k, v in value.items():
+      res += f"{k}={str(v)},"
+
+    res += f"sunnylink_dongle_id=\"{sunnylink_dongle_id}\",comma_dongle_id=\"{comma_dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n"
+    return res
+
+  def get_influxdb_line_raw(measurement: str, value: dict, timestamp: datetime, tags: dict) -> str:
+    res = f"{measurement}"
+    try:
+      custom_tags = ""
+      for k, v in tags.items():
+        custom_tags += f",{k}={str(v)}"
+      res += custom_tags
+
+      fields = ""
+      for k, v in value.items():
+        # Skip complex types - only keep simple scalar values
+        if isinstance(v, (dict, list, bytes, bytearray)):
+          continue
+
+        fields += f"{k}={json.dumps(v)},"
+
+      res += f" {fields}"
+    except Exception as e:
+      cloudlog.error(f"Unable to get influxdb line for: {value}")
+      res += f",invalid=1 reason={e},"
+
+    res += f"sunnylink_dongle_id=\"{sunnylink_dongle_id}\",comma_dongle_id=\"{comma_dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n"
+    return res
+
+  # open statistics socket
+  ctx = zmq.Context.instance()
+  sock = ctx.socket(zmq.PULL)
+  sock.bind(f"{STATS_SOCKET}_sp")
+
+  STATS_DIR = Paths.stats_sp_root()
+
+  # initialize stats directory
+  Path(STATS_DIR).mkdir(parents=True, exist_ok=True)
+
+  build_metadata = get_build_metadata()
+
+  # initialize tags
+  tags = {
+    'started': False,
+    'version': build_metadata.openpilot.version,
+    'branch': build_metadata.channel,
+    'dirty': build_metadata.openpilot.is_dirty,
+    'origin': build_metadata.openpilot.git_normalized_origin,
+    'deviceType': HARDWARE.get_device_type(),
+  }
+
+  # subscribe to deviceState for started state
+  sm = SubMaster(['deviceState'])
+
+  idx = 0
+  boot_uid = str(uuid.uuid4())[:8]
+  last_flush_time = time.monotonic()
+  gauges = {}
+  samples: dict[str, list[float]] = defaultdict(list)
+  raws: dict = defaultdict()
+  try:
+    while not end_event.is_set():
+      started_prev = sm['deviceState'].started
+      sm.update()
+
+      # Update metrics
+      while True:
+        try:
+          metric = sock.recv_string(zmq.NOBLOCK)
+          try:
+            metric_type = metric.split('|')[1]
+            metric_name = metric.split(':')[0]
+            metric_value_raw = metric.split('|')[0].split(':')[1]
+
+            if metric_type == METRIC_TYPE.GAUGE:
+              metric_value = float(metric_value_raw)
+              gauges[metric_name] = metric_value
+            elif metric_type == METRIC_TYPE.SAMPLE:
+              metric_value = float(metric_value_raw)
+              samples[metric_name].append(metric_value)
+            elif metric_type == METRIC_TYPE.RAW:
+              raws[metric_name] = metric_value_raw
+            else:
+              cloudlog.event("unknown metric type", metric_type=metric_type)
+          except Exception:
+            print(traceback.format_exc())
+            cloudlog.event("malformed metric", metric=metric)
+        except zmq.error.Again:
+          break
+
+      # flush when started state changes or after FLUSH_TIME_S
+      if (time.monotonic() > last_flush_time + STATS_FLUSH_TIME_S) or (sm['deviceState'].started != started_prev):
+        result = ""
+        current_time = datetime.now(UTC)
+        tags['started'] = sm['deviceState'].started
+
+        for key, value in raws.items():
+          decoded_value = json.loads(base64.b64decode(value).decode('utf-8'))
+          result += get_influxdb_line_raw(key, decoded_value, current_time, tags)
+
+        for key, value in gauges.items():
+          result += get_influxdb_line(f"gauge.{key}", value, current_time, tags)
+
+        for key, values in samples.items():
+          values.sort()
+          sample_count = len(values)
+          sample_sum = sum(values)
+
+          stats = {
+            'count': sample_count,
+            'min': values[0],
+            'max': values[-1],
+            'mean': sample_sum / sample_count,
+          }
+          for percentile in [0.05, 0.5, 0.95]:
+            value = values[int(round(percentile * (sample_count - 1)))]
+            stats[f"p{int(percentile * 100)}"] = value
+
+          result += get_influxdb_line(f"sample.{key}", stats, current_time, tags)
+
+        # clear intermediate data
+        gauges.clear()
+        samples.clear()
+        last_flush_time = time.monotonic()
+
+        # check that we aren't filling up the drive
+        if len(os.listdir(STATS_DIR)) < STATS_DIR_FILE_LIMIT:
+          if len(result) > 0:
+            stats_path = os.path.join(STATS_DIR, f"{boot_uid}_{idx}")
+            with atomic_write_in_dir(stats_path) as f:
+              f.write(result)
+            idx += 1
+        else:
+          cloudlog.error("stats dir full")
+  finally:
+    sock.close()
+    ctx.term()
+
+
+def main():
+  rk = Ratekeeper(1, print_delay_threshold=None)
+  end_event = threading.Event()
+
+  threads = [
+    threading.Thread(target=stats_main, args=(end_event,)),
+    threading.Thread(target=sp_stats, args=(end_event,)),
+  ]
+
+  for t in threads:
+    t.start()
+
+  try:
+    while all(t.is_alive() for t in threads):
+      rk.keep_time()
+  finally:
+    end_event.set()
+
+  for t in threads:
+    t.join()
+
+
+if __name__ == "__main__":
+  main()

system/athena/athenad.py

@@ -744,26 +744,40 @@ def log_handler(end_event: threading.Event, log_attr_name=LOG_ATTR_NAME) -> None
       cloudlog.exception("athena.log_handler.exception")
 
 
-def stat_handler(end_event: threading.Event) -> None:
-  STATS_DIR = Paths.stats_root()
+def stat_handler(end_event: threading.Event, stats_dir=None, is_sunnylink=False) -> None:
+  stats_dir = stats_dir or Paths.stats_root()
   last_scan = 0.0
 
   while not end_event.is_set():
     curr_scan = time.monotonic()
     try:
       if curr_scan - last_scan > 10:
-        stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(STATS_DIR)))
+        stat_filenames = list(filter(lambda name: not name.startswith(tempfile.gettempprefix()), os.listdir(stats_dir)))
         if len(stat_filenames) > 0:
-          stat_path = os.path.join(STATS_DIR, stat_filenames[0])
+          stat_path = os.path.join(stats_dir, stat_filenames[0])
           with open(stat_path) as f:
+            payload = f.read()
+            is_compressed = False
+
+            # Log the current size of the file
+            if is_sunnylink:
+              # Compress and encode the data if it exceeds the maximum size
+              compressed_data = gzip.compress(payload.encode())
+              payload = base64.b64encode(compressed_data).decode()
+              is_compressed = True
+
             jsonrpc = {
               "method": "storeStats",
               "params": {
-                "stats": f.read()
+                "stats": payload
               },
               "jsonrpc": "2.0",
               "id": stat_filenames[0]
             }
+
+            if is_sunnylink and is_compressed:
+              jsonrpc["params"]["compressed"] = is_compressed
+
             low_priority_send_queue.put_nowait(json.dumps(jsonrpc))
           os.remove(stat_path)
         last_scan = curr_scan

system/hardware/hw.py

@@ -55,6 +55,13 @@ class Paths:
     else:
       return "/data/stats/"
 
+  @staticmethod
+  def stats_sp_root() -> str:
+    if PC:
+      return str(Path(Paths.comma_home()) / "stats")
+    else:
+      return "/data/stats_sp/"
+
   @staticmethod
   def config_root() -> str:
     if PC:

system/manager/process_config.py

@@ -164,6 +164,7 @@ procs = [
   # sunnylink <3
   DaemonProcess("manage_sunnylinkd", "sunnypilot.sunnylink.athena.manage_sunnylinkd", "SunnylinkdPid"),
   PythonProcess("sunnylink_registration_manager", "sunnypilot.sunnylink.registration_manager", sunnylink_need_register_shim),
+  PythonProcess("statsd_sp", "sunnypilot.sunnylink.statsd", and_(always_run, sunnylink_ready_shim)),
 ]
 
 # sunnypilot

system/statsd.py

@@ -1,11 +1,15 @@
 #!/usr/bin/env python3
+import base64
+import json
 import os
+from decimal import Decimal
+
 import zmq
 import time
 import uuid
 from pathlib import Path
 from collections import defaultdict
-from datetime import datetime, UTC
+from datetime import datetime, UTC, date
 from typing import NoReturn
 
 from openpilot.common.params import Params
@@ -21,18 +25,21 @@ from openpilot.system.loggerd.config import STATS_DIR_FILE_LIMIT, STATS_SOCKET,
 class METRIC_TYPE:
   GAUGE = 'g'
   SAMPLE = 'sa'
+  RAW = 'r'
+
 
 class StatLog:
   def __init__(self):
     self.pid = None
     self.zctx = None
     self.sock = None
+    self.stats_socket = STATS_SOCKET
 
   def connect(self) -> None:
-    self.zctx = zmq.Context()
+    self.zctx = zmq.Context.instance() or zmq.Context()
     self.sock = self.zctx.socket(zmq.PUSH)
     self.sock.setsockopt(zmq.LINGER, 10)
-    self.sock.connect(STATS_SOCKET)
+    self.sock.connect(self.stats_socket)
     self.pid = os.getpid()
 
   def __del__(self):
@@ -60,6 +67,50 @@ class StatLog:
     self._send(f"{name}:{value}|{METRIC_TYPE.SAMPLE}")
 
 
+class StatLogSP(StatLog):
+  def __init__(self, intercept=True):
+    """
+    Initializes the class instance with an optional parameter to determine
+    if statistical logging should be configured or not.
+
+    :param intercept: A boolean flag that indicates whether to initialize
+        the `comma_statlog`. If True, the `comma_statlog` attribute is
+        instantiated as a `StatLog` object. Defaults to True.
+    """
+    super().__init__()
+    self.comma_statlog = StatLog() if intercept else None
+    self.stats_socket = f"{STATS_SOCKET}_sp"
+
+  def connect(self) -> None:
+    super().connect()
+    if self.comma_statlog:
+      self.comma_statlog.connect()
+
+  def __del__(self):
+    super().__del__()
+    if self.comma_statlog:
+      self.comma_statlog.__del__()
+
+  def _send(self, metric: str) -> None:
+    super()._send(metric)
+    if self.comma_statlog:
+      self.comma_statlog._send(metric)
+
+  @staticmethod
+  def default_converter(obj):
+    if isinstance(obj, (datetime, date)):
+      return obj.isoformat()
+    if isinstance(obj, set):
+      return list(obj)
+    if isinstance(obj, Decimal):
+      return float(obj)
+    return str(obj)  # fallback for unknown types
+
+  def raw(self, name: str, value: dict) -> None:
+    encoded_dict = base64.b64encode(json.dumps(value, default=self.default_converter).encode("utf-8")).decode("utf-8")
+    self._send(f"{name}:{encoded_dict}|{METRIC_TYPE.RAW}")
+
+
 def main() -> NoReturn:
   dongle_id = Params().get("DongleId")
   def get_influxdb_line(measurement: str, value: float | dict[str, float],  timestamp: datetime, tags: dict) -> str:
@@ -180,4 +231,4 @@ def main() -> NoReturn:
 if __name__ == "__main__":
   main()
 else:
-  statlog = StatLog()
+  statlog = StatLogSP(intercept=True)