exact distance :)

common/params_keys.h

@@ -189,6 +189,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
 
     // Navigation params
     {"AllowNavigation", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"MapboxFavorites", {PERSISTENT | BACKUP, STRING}},
     {"MapboxToken", {PERSISTENT | BACKUP, STRING}},
     {"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
     {"MapboxRoute", {PERSISTENT, STRING}},

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

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

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

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

sunnypilot/navd/helpers.py

@@ -73,12 +73,12 @@ class Coordinate:
 
 
 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
+  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):

sunnypilot/navd/navigation_helpers/nav_instructions.py

@@ -8,7 +8,7 @@ from numpy import interp
 
 from openpilot.common.params import Params
 
-from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, string_to_direction
+from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, string_to_direction, distance_along_geometry
 
 
 class NavigationInstructions:
@@ -33,7 +33,7 @@ class NavigationInstructions:
 
     # Find the closest point on the route relative to self
     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]
+    closest_cumulative = distance_along_geometry(route['geometry'], self.coord)
 
     # Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
     current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
@@ -99,6 +99,7 @@ class NavigationInstructions:
         'instruction': step['instruction'],
       })
     self._cached_route = {
+      'bearings': [bearing_between_two_points(geometry[i], geometry[i+2]) for i in range(len(geometry)-2)],
       'steps': steps,
       'total_distance': route['totalDistance'],
       'total_duration': route['totalDuration'],
@@ -120,13 +121,11 @@ class NavigationInstructions:
     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)
+      route_bearing = route['bearings'][self.closest_idx -1]
       current_bearing_normalized = (bearing + 360) % 360
       bearing_difference = abs(current_bearing_normalized - route_bearing)
 
-      if min(bearing_difference, 360 - bearing_difference) > 91:
+      if min(bearing_difference, 360 - bearing_difference) > 110:
         route_bearing_misalign = True  # flag for recompute/cancellation
     return route_bearing_misalign
 

sunnypilot/navd/navigationd.py

@@ -68,7 +68,7 @@ class Navigationd:
 
       if self.cancel_route_counter == 30:
         self.cancel_route_counter = 0
-        self.destination = None
+        self.params.put_nonblocking("MapboxRoute", "")
         self.nav_instructions.clear_route_cache()
         self.route = None