Compare commits

...

30 Commits

Author SHA1 Message Date
discountchubbs
bf84976129 some more 2025-11-12 19:48:10 -08:00
discountchubbs
d3db789e86 some more 2025-11-12 18:53:05 -08:00
discountchubbs
ae023d8303 :( 2025-11-11 09:28:18 -08:00
discountchubbs
6b102b4e63 but why tho 2025-11-11 08:43:15 -08:00
discountchubbs
71ac12f438 # Conflicts:
#	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
2025-11-11 08:21:14 -08:00
discountchubbs
035d5fba92 meh 2025-11-10 19:22:46 -08:00
discountchubbs
729c508a73 cmath floor >= 10.0 mi/km for more apple/google maps experience 2025-11-09 14:53:46 -08:00
discountchubbs
1796598b52 for now 2025-11-09 14:38:45 -08:00
discountchubbs
ec031ffeb6 n overlap and more code diff! 2025-11-07 17:59:23 -08:00
discountchubbs
9c5ee2e12a start cleanup 2025-11-07 12:37:22 -08:00
discountchubbs
65386da6d4 yesssss 2025-11-07 09:34:52 -08:00
discountchubbs
00799154ef merge origin/nav-desires into nav-commacon 2025-11-07 07:47:16 -08:00
discountchubbs
d6731c30da play with moving stuff when not valid 2025-11-07 07:05:24 -08:00
discountchubbs
0b54b86476 rounding 2025-11-06 15:47:31 -08:00
discountchubbs
fd675d29a9 no more nav-events 2025-11-06 12:25:27 -08:00
discountchubbs
d58bbda789 merge origin/nav-events into nav-commacon 2025-11-05 19:28:38 -08:00
discountchubbs
50773a62ed fine 2025-11-05 17:47:16 -08:00
discountchubbs
f5dbdc192e YOUR DESTINATION IS ON THE RIGHT 2025-11-05 15:28:52 -08:00
discountchubbs
f9987b2d7d quick don't look 2025-11-05 14:37:44 -08:00
discountchubbs
d51af1513b add to navigationd-service , but add here first because yessss 2025-11-05 14:30:02 -08:00
discountchubbs
2807c949a9 nayan! 2025-11-05 09:58:18 -08:00
nayan
0268d4384d Move Elements 2025-11-05 11:27:49 -05:00
nayan
0e60a56e56 Update idx to allow next maneuver 2025-11-05 11:02:21 -05:00
nayan
c5445d2a8b Make it bigger. BIGGER! 2025-11-05 11:02:21 -05:00
discountchubbs
6df1edbbaf offroad panel stuffs 2025-11-05 06:57:21 -08:00
discountchubbs
a098a0805a fix index access 2025-11-05 06:08:31 -08:00
nayan
469f1e01ef LOL, keep these 2025-11-04 16:28:23 -05:00
nayan
0fc5414f7c NAV DIRECTIONS UI 2025-11-04 16:22:37 -05:00
James Vecellio
d798288b2c more 2025-11-03 06:54:03 -08:00
James Vecellio
0e02fc2449 commacon stuff 2025-11-02 18:53:53 -08:00
136 changed files with 1568 additions and 396 deletions

View File

@@ -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}

View File

@@ -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}

View File

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

View File

@@ -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

View File

@@ -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>

View File

@@ -340,7 +340,6 @@ struct OnroadEventSP @0xda96579883444c35 {
speedLimitChanged @21;
speedLimitPending @22;
e2eChime @23;
navigationBanner @24;
}
}
@@ -370,6 +369,7 @@ struct CarControlSP @0xa5cd762cd951a455 {
leadOne @2 :LeadData;
leadTwo @3 :LeadData;
intelligentCruiseButtonManagement @4 :IntelligentCruiseButtonManagement;
speed @5 :Float32;
struct Param {
key @0 :Text;
@@ -457,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;

View File

@@ -188,12 +188,12 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}},
// Navigation params
{"AllowNavigation", {PERSISTENT | BACKUP, BOOL, "0"}},
{"MapboxToken", {PERSISTENT | BACKUP, STRING}},
{"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
{"MapboxRoute", {PERSISTENT, STRING}},
{"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
{"NavAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
{"NavEvents", {PERSISTENT | BACKUP, BOOL, "0"}},
{"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
// Neural Network Lateral Control
{"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},
@@ -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"}},

View File

@@ -61,7 +61,6 @@ class DesireHelper:
def update(self, carstate, lateral_active, lane_change_prob):
self.alc.update_params()
self.lane_turn_controller.update_params()
self.navigation_desires.update(carstate, lateral_active)
v_ego = carstate.vEgo
one_blinker = carstate.leftBlinker != carstate.rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
@@ -147,6 +146,6 @@ class DesireHelper:
self.alc.update_state()
nav_desire = self.navigation_desires.get_desire()
nav_desire = self.navigation_desires.update(carstate, lateral_active)
if nav_desire != log.Desire.none and (self.desire == log.Desire.none or self.desire in (log.Desire.turnLeft, log.Desire.turnRight)):
self.desire = nav_desire

View File

@@ -27,7 +27,7 @@ def main():
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
'liveMapDataSP', 'navigationd', 'carStateSP', gps_location_service],
'liveMapDataSP', 'carStateSP', gps_location_service],
poll='carState')
while True:

View File

@@ -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,8 +98,8 @@ class SelfdriveD(CruiseHelper):
'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
'modelDataV2SP', 'longitudinalPlanSP', 'navigationd'] + \
self.camera_packets + self.sensor_packets + self.gps_packets,
'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))
@@ -293,7 +293,7 @@ class SelfdriveD(CruiseHelper):
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
direction = self.sm['modelV2'].meta.laneChangeDirection
if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
(CS.rightBlindspot and direction == LaneChangeDirection.right):
(CS.rightBlindspot and direction == LaneChangeDirection.right):
self.events.add(EventName.laneChangeBlocked)
else:
if direction == LaneChangeDirection.left:
@@ -301,7 +301,7 @@ class SelfdriveD(CruiseHelper):
else:
self.events.add(EventName.preLaneChangeRight)
elif self.sm['modelV2'].meta.laneChangeState in (LaneChangeState.laneChangeStarting,
LaneChangeState.laneChangeFinishing):
LaneChangeState.laneChangeFinishing):
self.events.add(EventName.laneChange)
# Handle lane turn
@@ -496,7 +496,7 @@ class SelfdriveD(CruiseHelper):
# All pandas not in silent mode must have controlsAllowed when openpilot is enabled
if self.enabled and any(not ps.controlsAllowed for ps in self.sm['pandaStates']
if ps.safetyModel not in IGNORED_SAFETY_MODES):
if ps.safetyModel not in IGNORED_SAFETY_MODES):
self.mismatch_counter += 1
return CS

View File

@@ -71,7 +71,6 @@ class Plant:
model = messaging.new_message('modelV2')
car_state_sp = messaging.new_message('carStateSP')
live_map_data_sp = messaging.new_message('liveMapDataSP')
navigationd = messaging.new_message('navigationd')
gps_data = messaging.new_message('gpsLocation')
a_lead = (v_lead - self.v_lead_prev)/self.ts
self.v_lead_prev = v_lead
@@ -142,7 +141,6 @@ class Plant:
'modelV2': model.modelV2,
'carStateSP': car_state_sp.carStateSP,
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
'navigationd': navigationd.navigationd,
'gpsLocation': gps_data.gpsLocation}
self.planner.update(sm)
self.acceleration = self.planner.output_a_target

View File

@@ -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;

View File

@@ -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 = "");
};

View File

@@ -30,11 +30,13 @@ qt_src = [
"sunnypilot/qt/offroad/settings/max_time_offroad.cc",
"sunnypilot/qt/offroad/settings/brightness.cc",
"sunnypilot/qt/offroad/settings/models_panel.cc",
"sunnypilot/qt/offroad/settings/navigation_panel.cc",
"sunnypilot/qt/offroad/settings/osm_panel.cc",
"sunnypilot/qt/offroad/settings/settings.cc",
"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",

View File

@@ -0,0 +1,79 @@
/**
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other 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/navigation_panel.h"
NavigationPanel::NavigationPanel(QWidget* parent) : QWidget(parent) {
QVBoxLayout* main_layout = new QVBoxLayout(this);
main_layout->setContentsMargins(50, 25, 50, 25);
list = new ListWidget(this, false);
scroller = new ScrollViewSP(list, this);
main_layout->addWidget(scroller);
// Mapbox Token
mapbox_token = new ButtonControl(tr("Mapbox Token"), tr("Edit"), tr("Enter your Mapbox API 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);
if (!token.isEmpty()) {
params.put("MapboxToken", token.toStdString());
refresh();
}
});
list->addItem(mapbox_token);
// Mapbox Route
mapbox_route = new ButtonControl(tr("Mapbox Route"), tr("Edit"), tr("Enter Mapbox route data"));
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);
if (!route.isEmpty()) {
params.put("MapboxRoute", route.toStdString());
refresh();
}
});
list->addItem(mapbox_route);
// Allow Navigation
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);
list->addItem(mapbox_recompute);
// Nav Allowed
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);
}
void NavigationPanel::showEvent(QShowEvent *event) {
refresh();
}
void NavigationPanel::refresh() {
allow_navigation->refresh();
bool nav_enabled = allow_navigation->isToggled();
updateNavigationVisibility(nav_enabled);
QString token = QString::fromStdString(params.get("MapboxToken"));
mapbox_token->setValue(token.isEmpty() ? tr("Not set") : token);
QString route = QString::fromStdString(params.get("MapboxRoute"));
mapbox_route->setValue(route.isEmpty() ? tr("Not set") : route);
mapbox_recompute->refresh();
nav_allowed->refresh();
}

View File

@@ -0,0 +1,38 @@
/**
* Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other 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 "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"
class NavigationPanel : public QWidget {
Q_OBJECT
public:
explicit NavigationPanel(QWidget* parent = nullptr);
void showEvent(QShowEvent *event) override;
void refresh();
public slots:
void updateNavigationVisibility(bool state);
private:
Params params;
ListWidget* list;
ScrollViewSP* scroller;
ParamControlSP* allow_navigation;
ButtonControl* mapbox_token;
ButtonControl* mapbox_route;
ParamControlSP* mapbox_recompute;
ParamControlSP* nav_allowed;
};

View File

@@ -15,6 +15,7 @@
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/device_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/display_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/navigation_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/software_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/sunnylink_panel.h"
#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h"
@@ -85,6 +86,7 @@ SettingsWindowSP::SettingsWindowSP(QWidget *parent) : SettingsWindow(parent) {
PanelInfo(" " + tr("Toggles"), toggles, "../../sunnypilot/selfdrive/assets/offroad/icon_toggle.png"),
PanelInfo(" " + tr("Software"), new SoftwarePanelSP(this), "../../sunnypilot/selfdrive/assets/offroad/icon_software.png"),
PanelInfo(" " + tr("Models"), new ModelsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_models.png"),
PanelInfo(" " + tr("Navigation"), new NavigationPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_map.png"),
PanelInfo(" " + tr("Steering"), new LateralPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_lateral.png"),
PanelInfo(" " + tr("Cruise"), new LongitudinalPanel(this), "../assets/icons/speed_limit.png"),
PanelInfo(" " + tr("Visuals"), new VisualsPanel(this), "../../sunnypilot/selfdrive/assets/offroad/icon_visuals.png"),

View File

@@ -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();
}

View File

@@ -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();
};

View File

@@ -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;
}

View File

@@ -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);

View File

@@ -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);
}

View File

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

View File

@@ -5,6 +5,7 @@
* See the LICENSE.md file in the root directory for more details.
*/
#include <QPainterPath>
#include <cmath>
#include "selfdrive/ui/sunnypilot/qt/onroad/hud.h"
@@ -154,6 +155,73 @@ void HudRendererSP::updateState(const UIState &s) {
allow_e2e_alerts = sm["selfdriveState"].getSelfdriveState().getAlertSize() == cereal::SelfdriveState::AlertSize::NONE &&
sm.rcv_frame("driverStateV2") > s.scene.started_frame && !reversing;
// Navigationd
if (sm.updated("navigationd")) {
auto nav = sm["navigationd"].getNavigationd();
navigationValid = nav.getValid();
if (navigationValid && nav.getAllManeuvers().size() > 0) {
int currManeuverIdx = nav.getAllManeuvers().size() > 1 ? 1 : 0;
auto maneuver = nav.getAllManeuvers()[currManeuverIdx];
navigationModifier = QString::fromStdString(maneuver.getModifier());
navigationManeuverType = QString::fromStdString(maneuver.getType());
float dist = maneuver.getDistance();
if (is_metric) {
if (dist < 1000) {
if (dist < 500) {
navigationDistance = QString::number(std::round(dist / 25.0) * 25) + " m";
}
else {
navigationDistance = QString::number(std::round(dist / 50.0) * 50) + " m";
}
} else {
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;
if (dist_ft < 1000) {
if (dist_ft <= 100){
navigationDistance = QString::number((std::round(dist_ft / 10.0) * 10)) + " ft";
}
else {
navigationDistance = QString::number((std::round(dist_ft / 50.0) * 50)) + " ft";
}
} else {
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";
}
}
}
QString instruction = QString::fromStdString(maneuver.getInstruction());
QStringList parts = instruction.split(" onto ");
if (parts.size() > 1) {
navigationStreet = parts[1].trimmed();
} else {
navigationStreet = instruction;
}
navigationStreet = navigationStreet.replace(".", "");
// Get next maneuver if available
if (nav.getAllManeuvers().size() > 2) {
auto nextManeuver = nav.getAllManeuvers()[2];
navigationNextModifier = QString::fromStdString(nextManeuver.getModifier());
navigationNextManeuverType = QString::fromStdString(nextManeuver.getType());
navigationHasNext = true;
} else {
navigationHasNext = false;
}
}
}
}
void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
@@ -287,6 +355,8 @@ void HudRendererSP::draw(QPainter &p, const QRect &surface_rect) {
}
}
drawNavigationHUD(p, surface_rect);
p.restore();
}
@@ -306,9 +376,14 @@ bool HudRendererSP::pulseElement(int frame) {
}
void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &surface_rect, int x_offset, int y_offset, std::string name) {
int x = surface_rect.center().x();
int base_x = surface_rect.center().x();
int y = surface_rect.height() / 4;
if (navigationValid) {
base_x = 618;
y = 420;
}
QString text = QString::fromStdString(name);
QFont font = InterFont(36, QFont::Bold);
p.setFont(font);
@@ -319,7 +394,7 @@ void HudRendererSP::drawSmartCruiseControlOnroadIcon(QPainter &p, const QRect &s
int box_width = 160;
int box_height = fm.height() + padding_v * 2;
QRectF bg_rect(x - (box_width / 2) + x_offset,
QRectF bg_rect(base_x - (box_width / 2) + x_offset,
y - (box_height / 2) + y_offset,
box_width, box_height);
@@ -684,15 +759,16 @@ void HudRendererSP::drawSpeedLimitPreActiveArrow(QPainter &p, QRect &sign_rect)
}
void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) {
// Draw outer box + border to contain set speed
const QSize default_size = {172, 204};
QSize set_speed_size = is_metric ? QSize(200, 204) : default_size;
QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
// Draw set speed box
p.setPen(QPen(QColor(255, 255, 255, 75), 6));
p.setBrush(QColor(0, 0, 0, 166));
p.drawRoundedRect(set_speed_rect, 32, 32);
// ICBM counter logic
if (!pcmCruiseSpeed && carControlEnabled) {
if (std::nearbyint(set_speed) != std::nearbyint(speedCluster)) {
icbm_active_counter = 3 * UI_FREQ;
} else if (icbm_active_counter > 0) {
icbm_active_counter--;
}
} else {
icbm_active_counter = 0;
}
// Colors based on status
QColor max_color = QColor(0xa6, 0xa6, 0xa6, 0xff);
@@ -713,29 +789,62 @@ void HudRendererSP::drawSetSpeedSP(QPainter &p, const QRect &surface_rect) {
}
}
// Draw "MAX" or carState.cruiseState.speedCluster (when ICBM is active) text
if (!pcmCruiseSpeed && carControlEnabled) {
if (std::nearbyint(set_speed) != std::nearbyint(speedCluster)) {
icbm_active_counter = 3 * UI_FREQ;
} else if (icbm_active_counter > 0) {
icbm_active_counter--;
}
} else {
icbm_active_counter = 0;
}
int max_str_size = (icbm_active_counter != 0) ? 60 : 40;
int max_str_y = (icbm_active_counter != 0) ? 15 : 27;
QString max_str = (icbm_active_counter != 0) ? QString::number(std::nearbyint(speedCluster)) : tr("MAX");
p.setFont(InterFont(max_str_size, QFont::DemiBold));
p.setPen(max_color);
p.drawText(set_speed_rect.adjusted(0, max_str_y, 0, 0), Qt::AlignTop | Qt::AlignHCenter, max_str);
// Draw set speed
QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(set_speed)) : "";
p.setFont(InterFont(90, QFont::Bold));
p.setPen(set_speed_color);
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
if (!navigationValid) {
// Original positions when navigation is not valid
const QSize default_size = {172, 204};
QSize set_speed_size = is_metric ? QSize(200, 204) : default_size;
QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
// Draw set speed box
p.setPen(QPen(QColor(255, 255, 255, 75), 6));
p.setBrush(QColor(0, 0, 0, 166));
p.drawRoundedRect(set_speed_rect, 32, 32);
// Draw "MAX" or carState.cruiseState.speedCluster (when ICBM is active) text
int max_str_size = (icbm_active_counter != 0) ? 60 : 40;
int max_str_y = (icbm_active_counter != 0) ? 15 : 27;
p.setFont(InterFont(max_str_size, QFont::DemiBold));
p.setPen(max_color);
p.drawText(set_speed_rect.adjusted(0, max_str_y, 0, 0), Qt::AlignTop | Qt::AlignHCenter, max_str);
// Draw set speed
p.setFont(InterFont(90, QFont::Bold));
p.setPen(set_speed_color);
p.drawText(set_speed_rect.adjusted(0, 77, 0, 0), Qt::AlignTop | Qt::AlignHCenter, setSpeedStr);
} else {
// Modified positions when navigation is valid
const int container_width = 200;
const int container_height = 320;
const int container_x = 40;
const int container_y = 45;
QRect speed_container(container_x, container_y, container_width, container_height);
// Draw outer rounded rectangle container
p.setPen(QPen(QColor(255, 255, 255, 75), 6));
p.setBrush(QColor(0, 0, 0, 166));
p.drawRoundedRect(speed_container, 24, 24);
int divider_y = container_y + 190;
p.setPen(QPen(QColor(255, 255, 255, 50), 2));
p.drawLine(container_x + 20, divider_y, container_x + container_width - 20, divider_y);
// max label
QRect max_label_rect(container_x, container_y + 200, container_width, 35);
p.setFont(InterFont(32, QFont::Normal));
p.setPen(max_color);
p.drawText(max_label_rect, Qt::AlignCenter, max_str);
// Set speed value
QRect set_speed_rect(container_x, container_y + 240, container_width, 70);
p.setFont(InterFont(68, QFont::Bold));
p.setPen(set_speed_color);
p.drawText(set_speed_rect, Qt::AlignCenter, setSpeedStr);
}
}
void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const QString &alert_alt_text) {
@@ -794,12 +903,40 @@ void HudRendererSP::drawE2eAlert(QPainter &p, const QRect &surface_rect, const Q
void HudRendererSP::drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect) {
QString speedStr = QString::number(std::nearbyint(speed));
QString unit = is_metric ? tr("km/h") : tr("mph");
p.setFont(InterFont(176, QFont::Bold));
HudRenderer::drawText(p, surface_rect.center().x(), 210, speedStr);
int speed_x = surface_rect.center().x();
int speed_y = 210;
int unit_y = 290;
QFont speed_font = InterFont(176, QFont::Bold);
QFont unit_font = InterFont(66);
p.setFont(InterFont(66));
HudRenderer::drawText(p, surface_rect.center().x(), 290, is_metric ? tr("km/h") : tr("mph"), 200);
if (navigationValid) {
speed_y = 75;
unit_y = 175;
speed_font = InterFont(100, QFont::Bold);
unit_font = InterFont(35, QFont::Normal);
}
// Draw speed
p.setFont(speed_font);
if (!navigationValid) {
HudRenderer::drawText(p, speed_x, speed_y, speedStr);
} else {
QRect current_speed_rect(40, speed_y, 200, 100);
p.setPen(Qt::white);
p.drawText(current_speed_rect, Qt::AlignCenter, speedStr);
}
// Draw unit
p.setFont(unit_font);
if (!navigationValid) {
HudRenderer::drawText(p, speed_x, unit_y, unit, 200);
} else {
QRect unit_rect(40, unit_y, 200, 40);
p.setPen(QColor(180, 180, 180, 255));
p.drawText(unit_rect, Qt::AlignCenter, unit);
}
}
void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) {
@@ -827,7 +964,7 @@ void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) {
const int circleRadius = 60;
const int arrowLength = 60;
const int x_gap = 160;
const int y_offset = 272;
const int y_offset = navigationValid ? 352 : 300;
const int centerX = surface_rect.center().x();
@@ -885,3 +1022,186 @@ void HudRendererSP::drawBlinker(QPainter &p, const QRect &surface_rect) {
p.restore();
}
QString HudRendererSP::getNavigationIconName(const QString &type, const QString &mod) {
static QMap<QString, QString> icon_map;
if (icon_map.isEmpty()) {
icon_map["turn|uturn"] = "direction_uturn.png";
icon_map["turn|sharp right"] = "direction_turn_sharp_right.png";
icon_map["turn|right"] = "direction_turn_right.png";
icon_map["turn|slight right"] = "direction_turn_slight_right.png";
icon_map["turn|straight"] = "direction_turn_straight.png";
icon_map["turn|slight left"] = "direction_turn_slight_left.png";
icon_map["turn|left"] = "direction_turn_left.png";
icon_map["turn|sharp left"] = "direction_turn_sharp_left.png";
icon_map["arrive|right"] = "direction_arrive_right.png";
icon_map["arrive|straight"] = "direction_arrive_straight.png";
icon_map["arrive|left"] = "direction_arrive_left.png";
icon_map["arrive|"] = "direction_arrive.png";
icon_map["merge|slight right"] = "direction_merge_slight_right.png";
icon_map["merge|right"] = "direction_merge_right.png";
icon_map["merge|straight"] = "direction_merge_straight.png";
icon_map["merge|slight left"] = "direction_merge_slight_left.png";
icon_map["merge|left"] = "direction_merge_left.png";
icon_map["on ramp|sharp right"] = "direction_on_ramp_sharp_right.png";
icon_map["on ramp|right"] = "direction_on_ramp_right.png";
icon_map["on ramp|slight right"] = "direction_on_ramp_slight_right.png";
icon_map["on ramp|straight"] = "direction_on_ramp_straight.png";
icon_map["on ramp|slight left"] = "direction_on_ramp_slight_left.png";
icon_map["on ramp|left"] = "direction_on_ramp_left.png";
icon_map["on ramp|sharp left"] = "direction_on_ramp_sharp_left.png";
icon_map["off ramp|slight right"] = "direction_off_ramp_slight_right.png";
icon_map["off ramp|right"] = "direction_off_ramp_right.png";
icon_map["off ramp|slight left"] = "direction_off_ramp_slight_left.png";
icon_map["off ramp|left"] = "direction_off_ramp_left.png";
icon_map["roundabout|sharp right"] = "direction_roundabout_sharp_right.png";
icon_map["roundabout|right"] = "direction_roundabout_right.png";
icon_map["roundabout|slight right"] = "direction_roundabout_slight_right.png";
icon_map["roundabout|straight"] = "direction_roundabout_straight.png";
icon_map["roundabout|slight left"] = "direction_roundabout_slight_left.png";
icon_map["roundabout|left"] = "direction_roundabout_left.png";
icon_map["roundabout|sharp left"] = "direction_roundabout_sharp_left.png";
icon_map["roundabout|"] = "direction_roundabout.png";
}
QString normalized_type = type;
if (normalized_type == "rotary") {
normalized_type = "roundabout";
} else if (normalized_type == "new name") {
normalized_type = "turn";
} else if (normalized_type == "continue") {
normalized_type = "turn";
}
QString icon_name;
QStringList keys = {normalized_type + "|" + mod, normalized_type + "|", "turn|" + mod};
for (const QString &key : keys) {
icon_name = icon_map.value(key);
if (!icon_name.isEmpty()) break;
}
if (icon_name.isEmpty()) {
icon_name = "direction_turn_straight.png";
}
return icon_name;
}
void HudRendererSP::drawNavigationHUD(QPainter &p, const QRect &surface_rect) {
if (!navigationValid) return;
p.save();
const int container_width = 1080;
const int container_height = 225;
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);
p.setBrush(QColor(0, 0, 0, 180));
p.drawRoundedRect(container_rect, border_radius, border_radius);
// Navigation icon
const int icon_size = 150;
const int icon_padding = 30;
const int icon_x = container_x + icon_padding;
const int icon_y = container_y;
QString icon_name = getNavigationIconName(navigationManeuverType, navigationModifier);
QPixmap nav_icon = loadPixmap("../../sunnypilot/selfdrive/assets/navigation/" + icon_name, {icon_size, icon_size});
if (!nav_icon.isNull()) {
p.drawPixmap(icon_x, icon_y, nav_icon);
}
// Distance
p.setFont(InterFont(48, QFont::Bold));
p.setPen(Qt::white);
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;
const int text_x = icon_x + icon_size + 53;
const int text_area_width = container_width - (text_x - container_x) - icon_padding - then_section_width;
// Street name
p.setFont(InterFont(75, QFont::Bold));
p.setPen(Qt::white);
QFontMetrics fm(p.font());
QString street_line1, street_line2;
QStringList words = navigationStreet.split(' ');
QString currentLine;
for (int i = 0; i < words.size(); ++i) {
QString testLine = currentLine.isEmpty() ? words[i] : currentLine + " " + words[i];
if (fm.horizontalAdvance(testLine) <= text_area_width) {
currentLine = testLine;
} else {
if (street_line1.isEmpty()) {
street_line1 = currentLine;
currentLine = words[i];
} else {
break;
}
}
}
if (street_line1.isEmpty()) {
street_line1 = currentLine;
} else if (!currentLine.isEmpty()) {
street_line2 = currentLine;
if (words.size() > words.indexOf(currentLine.split(' ').last()) + 1) {
street_line2 = fm.elidedText(street_line2, Qt::ElideRight, text_area_width);
}
}
if (street_line2.isEmpty()) {
QRect street_rect(text_x, container_y + (container_height - fm.height()) / 2, text_area_width, fm.height());
p.drawText(street_rect, Qt::AlignLeft | Qt::AlignVCenter, street_line1);
} else {
QRect street_rect1(text_x, container_y + 23, text_area_width, fm.height());
p.drawText(street_rect1, Qt::AlignLeft | Qt::AlignVCenter, street_line1);
QRect street_rect2(text_x, container_y + 23 + fm.height(), text_area_width, fm.height());
p.drawText(street_rect2, Qt::AlignLeft | Qt::AlignVCenter, street_line2);
}
// Next Maneuver
if (navigationHasNext) {
const int divider_x = container_x + container_width - then_section_width - 8;
p.setPen(QPen(QColor(255, 255, 255, 50), 2));
p.drawLine(divider_x, container_y + 23, divider_x, container_y + container_height - 23);
const int then_x = divider_x + 15;
const int then_icon_size = 105;
QRect then_label_rect(then_x, container_y + 30, then_section_width - 23, 38);
p.setFont(InterFont(53, QFont::Medium));
p.setPen(Qt::white);
p.drawText(then_label_rect, Qt::AlignCenter, tr("Then"));
// Next maneuver icon
const int then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2;
const int then_icon_y = container_y + 75;
QString next_icon_name = getNavigationIconName(navigationNextManeuverType, navigationNextModifier);
QPixmap next_nav_icon = loadPixmap("../../sunnypilot/selfdrive/assets/navigation/" + next_icon_name, {then_icon_size, then_icon_size});
if (!next_nav_icon.isNull()) {
p.drawPixmap(then_icon_x, then_icon_y, next_nav_icon);
}
}
p.restore();
}

View File

@@ -39,6 +39,8 @@ private:
void drawE2eAlert(QPainter &p, const QRect &surface_rect, const QString &alert_alt_text = "");
void drawCurrentSpeedSP(QPainter &p, const QRect &surface_rect);
void drawBlinker(QPainter &p, const QRect &surface_rect);
void drawNavigationHUD(QPainter &p, const QRect &surface_rect);
QString getNavigationIconName(const QString &type, const QString &mod);
bool lead_status;
float lead_d_rel;
@@ -120,4 +122,13 @@ private:
float speedCluster = 0;
int icbm_active_counter = 0;
bool pcmCruiseSpeed = true;
bool navigationValid;
QString navigationStreet;
QString navigationDistance;
QString navigationModifier;
QString navigationManeuverType;
QString navigationNextModifier;
QString navigationNextManeuverType;
bool navigationHasNext;
};

View File

@@ -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"
"carStateSP", "liveParameters", "liveMapDataSP", "carParamsSP", "navigationd"
});
// update timer

View File

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

View File

@@ -5,11 +5,12 @@ 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.
"""
class NAV_CV:
""" These distances are expected in meters format and convert to desired format """
SHORT_DISTANCE_METERS = 200.0
QUARTER_MILE = 402.336
POINT_ONE_MILE = 160.9344
METERS_TO_KILO = 1000 # divide m by this
METERS_TO_MILE = 1609.344 # divide m by this
METERS_TO_FEET = 3.280839895 # multiply m by this
METERS_TO_KILO = 1000 # divide n by this
METERS_TO_MILE = 1609.344 # divide n by this
METERS_TO_FEET = 3.280839895 # multiply n by this

View File

@@ -1,83 +0,0 @@
"""
Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other 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.
"""
from cereal import custom, messaging
from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL
from sunnypilot.navd.constants import NAV_CV
class EventBuilder:
def __init__(self):
self._counter: int = -1
self._enabled: bool = False
self._params = Params()
@staticmethod
def _build_banner_message(metric: bool, nav_msg):
m = nav_msg.allManeuvers[-1]
banner = m.instruction
if metric:
dist = f'{m.distance / NAV_CV.METERS_TO_KILO:.1f} km,'
if m.distance < NAV_CV.SHORT_DISTANCE_METERS:
dist = f'{int(m.distance)}m,'
else:
dist = f'{m.distance / NAV_CV.METERS_TO_MILE:.1f} mi,'
if m.distance < NAV_CV.QUARTER_MILE:
dist = f'{round((m.distance * NAV_CV.METERS_TO_FEET) / 50) * 50}ft,'
if m.type == 'arrive' or m.type == 'depart' or 'Your destination' in banner:
base_msg = banner
elif banner.startswith(('Continue', 'Drive', 'Head')):
base_msg = f'For {dist} {banner}'
elif 'Turn' in banner or 'Take' in banner or 'Make' in banner:
base_msg = f'In {dist} {banner}'
else:
base_msg = f'For {dist} Continue on {banner}'
return base_msg
@staticmethod
def _get_turning_message(upcoming_turn):
turn_messages = {
'left': 'Turning Left. Make sure to nudge the wheel!',
'right': 'Turning Right. Make sure to nudge the wheel!',
'slightLeft': 'Keeping Left',
'slightRight': 'Keeping Right',
'sharpLeft': 'Sharp Left Turn',
'sharpRight': 'Sharp Right Turn',
'straight': 'Continuing Straight',
'uturn': 'U-Turn Ahead',
}
return turn_messages.get(upcoming_turn, f"Upcoming {upcoming_turn.replace('_', ' ').title()}")
@staticmethod
def build_navigation_events(sm: messaging.SubMaster, metric=True) -> list:
nav_msg = sm['navigationd']
if not nav_msg.valid:
return []
banner_message = EventBuilder._build_banner_message(metric, nav_msg)
if nav_msg.upcomingTurn != 'none':
banner_message = EventBuilder._get_turning_message(nav_msg.upcomingTurn)
return [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': banner_message,
}]
def update(self, sm: messaging.SubMaster) -> list:
self._counter += 1
if self._counter % int(3.0 / DT_MDL) == 0:
self._enabled = self._params.get("NavEvents", return_default=True)
if self._enabled:
return self.build_navigation_events(sm)
else:
return []

View File

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

View File

@@ -22,28 +22,21 @@ class NavigationDesires:
def update_params(self):
self.param_counter += 1
if self.param_counter % 60 == 0: # every 3 seconds at 20hz
self.nav_allowed = self._params.get("NavAllowed", return_default=True)
self.nav_allowed = self._params.get("NavDesiresAllowed", return_default=True)
def update(self, CS: car.CarState, lateral_active: bool):
def update(self, CS: car.CarState, lateral_active: bool) -> log.Desire:
self.update_params()
self.sm.update(0)
nav_msg = self.sm['navigationd']
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
# Note to reviewers: This change to require steering pressed (nudge basically) is intentional to prevent unwanted turns
# for those who don't have blind spot detection.
def get_desire(self) -> log.Desire:
return self.desire

View File

@@ -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),
@@ -46,7 +46,7 @@ def make_nav_msg(valid=False, upcoming='none'):
def params_setter(allowed: bool):
params = Params()
params.put("NavAllowed", allowed)
params.put("NavDesiresAllowed", allowed)
@pytest.fixture
def mock_submaster(mocker):
@@ -75,8 +75,7 @@ def test_update(mock_submaster, mocker):
mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming='left'))
nav_desires = NavigationDesires()
nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True)
assert nav_desires.get_desire() == log.Desire.turnLeft
assert nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True) == log.Desire.turnLeft
params_setter(False)
nav_desires.param_counter = 59

View File

@@ -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)
@@ -43,7 +48,7 @@ class NavigationInstructions:
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance']))
all_maneuvers: list = []
max_maneuvers = 2
max_maneuvers = 3
for idx in range(current_step_idx, min(current_step_idx + max_maneuvers, len(route['steps']))):
step = route['steps'][idx]
if idx == current_step_idx:
@@ -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,22 +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 <= 100:
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
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

View File

@@ -27,12 +27,11 @@ class TestMapbox:
cls.mapbox.params.put('MapboxRoute', '740 E Ventura Blvd. Camarillo, CA')
cls.postvars = {"place_name": cls.mapbox.params.get('MapboxRoute')}
cls.postvars, cls.valid_addr = cls.mapbox.set_destination(cls.postvars, cls.current_lon, cls.current_lat)
assert cls.valid_addr
cls.route = cls.nav.get_current_route()
assert cls.route is not None
assert len(cls.route['steps']) > 0
cls.progress = cls.nav.get_route_progress(cls.current_lat, cls.current_lon)
def test_set_destination(self):
assert self.valid_addr
settings = self.mapbox.params.get('MapboxSettings')
assert settings is not None
dest_lat = settings['navData']['current']['latitude']
@@ -40,6 +39,7 @@ class TestMapbox:
assert dest_lat == self.postvars["latitude"] and dest_lon == self.postvars["longitude"]
def test_get_route(self):
assert self.route is not None
assert 'steps' in self.route
assert 'geometry' in self.route
assert 'maxspeed' in self.route
@@ -54,37 +54,45 @@ class TestMapbox:
assert 'modifier' in step
def test_upcoming_turn_detection(self):
progress = self.nav.get_route_progress(self.current_lat, self.current_lon)
upcoming = self.nav.get_upcoming_turn_from_progress(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.0008 # 80 ish meters before the turn
if progress and progress.get('next_turn'):
expected_turn = progress['next_turn']['modifier']
upcoming_close = self.nav.get_upcoming_turn_from_progress(progress, close_lat, turn_lon)
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, v_ego=0.0)
if expected_turn:
assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"
def test_route_progress_tracking(self):
progress = self.nav.get_route_progress(self.current_lat, self.current_lon)
assert progress is not None
assert 'distance_from_route' in progress
assert 'next_turn' in progress
assert 'current_maxspeed' in progress
assert 'all_maneuvers' in progress
assert 'distance_to_end_of_step' in progress
assert progress['distance_from_route'] >= 0
assert isinstance(progress['all_maneuvers'], list)
assert self.progress is not None
assert 'distance_from_route' in self.progress
assert 'next_turn' in self.progress
assert 'current_maxspeed' in self.progress
assert 'all_maneuvers' in self.progress
assert 'distance_to_end_of_step' in self.progress
assert self.progress['distance_from_route'] >= 0
assert isinstance(self.progress['all_maneuvers'], list)
speed_limit_metric = self.nav.get_current_speed_limit_from_progress(progress, True)
speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(progress, False)
def test_speed_limit_handling(self):
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(progress['current_maxspeed'][0])
expected_imperial = int(round(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, 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

View File

@@ -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
@@ -12,6 +13,7 @@ from openpilot.common.params import Params
from openpilot.common.realtime import Ratekeeper
from openpilot.common.swaglog import cloudlog
from openpilot.sunnypilot.navd.constants import NAV_CV
from openpilot.sunnypilot.navd.helpers import Coordinate, parse_banner_instructions
from openpilot.sunnypilot.navd.navigation_helpers.mapbox_integration import MapboxIntegration
from openpilot.sunnypilot.navd.navigation_helpers.nav_instructions import NavigationInstructions
@@ -23,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
@@ -31,6 +33,7 @@ class Navigationd:
self.destination: str | None = None
self.new_destination: str = ''
self.allow_navigation: bool = False
self.recompute_allowed: bool = False
self.allow_recompute: bool = False
self.reroute_counter: int = 0
@@ -39,29 +42,28 @@ 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:
self.is_metric = self.params.get('IsMetric', return_default=True)
if self.frame % 15 == 0:
self.allow_navigation = self.params.get('AllowNavigation', 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()
self.route = self.nav_instructions.get_current_route()
self.cancel_route_counter = 0
self.reroute_counter = 0
if self.cancel_route_counter == 30:
@@ -74,12 +76,15 @@ class Navigationd:
def _update_navigation(self) -> tuple[str, dict | None, dict]:
banner_instructions: str = ''
progress: dict | None = None
nav_data: dict = {}
if 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)
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'])
@@ -87,11 +92,21 @@ 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:
if progress['distance_from_route'] > 200:
self.cancel_route_counter += 1
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:
@@ -99,9 +114,13 @@ class Navigationd:
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 = {}
return banner_instructions, progress, nav_data
@@ -121,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()

View File

@@ -1,92 +0,0 @@
"""
Copyright (c) 2021-, James Vecellio, Haibin Wen, sunnypilot, and a number of other 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.
"""
from cereal import custom
from openpilot.common.params import Params
from openpilot.sunnypilot.navd.event_builder import EventBuilder
class MockSM(dict):
def __init__(self, nav_msg):
super().__init__()
self['navigationd'] = nav_msg
class TestEventBuilder:
def setup_method(self):
self.params = Params()
self.event_builder = EventBuilder()
def create_nav_msg(self, upcoming_turn='none', valid=True):
nav_msg = custom.Navigationd.new_message()
nav_msg.valid = valid
nav_msg.upcomingTurn = upcoming_turn
nav_msg.allManeuvers = [
custom.Navigationd.Maneuver.new_message(distance=192.84873284, type='turn', modifier='left', instruction='West Esplanade Drive'),
custom.Navigationd.Maneuver.new_message(distance=192.84809314, type='turn', modifier='right', instruction='West Esplanade Drive'),
]
return nav_msg
def test_validity(self):
nav_msg = self.create_nav_msg(valid=False)
events = EventBuilder.build_navigation_events(MockSM(nav_msg))
assert events == []
def test_enabled(self):
self.params.put("NavEvents", True)
nav_msg = self.create_nav_msg()
events = self.event_builder.update(MockSM(nav_msg))
expected = [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': 'For 192m, Continue on West Esplanade Drive'
}]
assert events == expected
self.params.put("NavEvents", False)
self.event_builder._counter = 59
events = self.event_builder.update(MockSM(nav_msg))
assert events == []
def test_build_navigation_events(self):
nav_msg = self.create_nav_msg()
events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
expected = [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': 'For 650ft, Continue on West Esplanade Drive',
}]
assert events == expected
def test_distance_condition_imperial(self):
nav_msg = self.create_nav_msg()
nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=160.0, type='continue', modifier='straight', instruction='1234 Apple Way')
events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
expected = [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': 'For 500ft, Continue on 1234 Apple Way',
}]
assert events == expected
def test_upcoming_turn_override(self):
nav_msg = self.create_nav_msg(upcoming_turn='left')
events = EventBuilder.build_navigation_events(MockSM(nav_msg))
expected = [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': 'Turning Left. Make sure to nudge the wheel!',
}]
assert events == expected
def test_straight(self):
nav_msg = self.create_nav_msg()
nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=80.0, type='continue', modifier='straight', instruction='1234 Apple Way')
events = EventBuilder.build_navigation_events(MockSM(nav_msg))
expected = [{
'name': custom.OnroadEventSP.EventName.navigationBanner,
'message': 'For 80m, Continue on 1234 Apple Way'
}]
assert events == expected

View File

@@ -9,7 +9,7 @@ import pytest
import cereal.messaging as messaging
from sunnypilot.navd.navigationd import Navigationd
from openpilot.sunnypilot.navd.navigationd import Navigationd
from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -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

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More