exact distance :)

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

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

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

@@ -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):
-      #   self.desire = log.Desire.keepLeft
-      # elif upcoming == 'slightRight' and (not CS.rightBlindspot or CS.vEgo < self._turn_speed_limit):
-      #   self.desire = log.Desire.keepRight
-      if (upcoming == 'left' and not CS.rightBlinker
-            and not CS.leftBlindspot and 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.leftBlinker and not CS.rightBlindspot and CS.steeringPressed and CS.steeringTorque < 0:
+        self.desire = log.Desire.keepRight
+      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 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_helpers/nav_instructions.py

@@ -4,22 +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.
 """
-import numpy as np
+from numpy import interp
 
-from openpilot.common.constants import CV
 from openpilot.common.params import Params
 
-from openpilot.sunnypilot.navd.helpers import Coordinate, string_to_direction
+from openpilot.sunnypilot.navd.helpers import Coordinate, bearing_between_two_points, string_to_direction, distance_along_geometry
 
 
 class NavigationInstructions:
   def __init__(self):
     self.coord = Coordinate(0, 0)
     self.params = Params()
+
     self._cached_route = None
     self._route_loaded = False
     self._no_route = False
 
+    self.closest_idx: float = 0
+    self.min_distance: float = 0
+
   def get_route_progress(self, current_lat, current_lon) -> dict | None:
     route = self.get_current_route()
     if not route or not route['geometry'] or not route['steps']:
@@ -29,8 +32,8 @@ class NavigationInstructions:
     self.coord.longitude = current_lon
 
     # Find the closest point on the route relative to self
-    closest_idx, min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
-    closest_cumulative = route['cumulative_distances'][closest_idx]
+    self.closest_idx, self.min_distance = min(((idx, self.coord.distance_to(coord)) for idx, coord in enumerate(route['geometry'])), key=lambda x: x[1])
+    closest_cumulative = distance_along_geometry(route['geometry'], self.coord)
 
     # Find the current step index, which is the HIGHEST idx where the step location cumulative less/equal closest cumulative
     current_step_idx = max((idx for idx, step in enumerate(route['steps']) if step['cumulative_distance'] <= closest_cumulative), default=-1)
@@ -55,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,
@@ -96,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'],
@@ -111,11 +115,25 @@ class NavigationInstructions:
     self._route_loaded = False
     self._no_route = False
 
+  def route_bearing_misalign(self, route, bearing, v_ego) -> bool:
+    route_bearing_misalign:bool = False
+
+    if v_ego < 5.0:
+      route_bearing_misalign = False
+    elif self.closest_idx > 0 and self.closest_idx < len(route['geometry']) -1:
+      route_bearing = route['bearings'][self.closest_idx -1]
+      current_bearing_normalized = (bearing + 360) % 360
+      bearing_difference = abs(current_bearing_normalized - route_bearing)
+
+      if min(bearing_difference, 360 - bearing_difference) > 110:
+        route_bearing_misalign = True  # flag for recompute/cancellation
+    return route_bearing_misalign
+
   def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon, v_ego: float) -> str:
     if progress and progress['next_turn']:
       speed_breakpoints: list = [0, 5, 10, 15, 20, 25, 30, 35, 40]
       distance_breakpoints: list = [20, 25, 30, 45, 60, 75, 90, 105, 120]
-      distance_interp = np.interp(v_ego, speed_breakpoints, distance_breakpoints)
+      distance_interp = interp(v_ego, speed_breakpoints, distance_breakpoints)
 
       self.coord.latitude = current_lat
       self.coord.longitude = current_lon
@@ -127,19 +145,9 @@ class NavigationInstructions:
     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

@@ -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,8 +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
-import numpy as np
+from math import degrees
+from numpy import interp
 
 import cereal.messaging as messaging
 from cereal import custom
@@ -25,7 +25,7 @@ class Navigationd:
     self.mapbox = MapboxIntegration()
     self.nav_instructions = NavigationInstructions()
 
-    self.sm = messaging.SubMaster(['carState', 'liveLocationKalman'])
+    self.sm = messaging.SubMaster(['carControlSP', 'liveLocationKalman'])
     self.pm = messaging.PubMaster(['navigationd'])
     self.rk = Ratekeeper(3) # 3 Hz
 
@@ -42,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()
@@ -71,21 +68,23 @@ 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
 
       self.valid = self.route is not None
 
-  def _update_navigation(self, v_ego) -> tuple[str, dict | None, dict]:
+  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):
+        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)
-        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)
+        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'])
@@ -95,27 +94,33 @@ class Navigationd:
         nav_data['distance_from_route'] = progress['distance_from_route']
         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(np.interp(v_ego, speed_breakpoints, distance_list))
+        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.reroute_counter = 0
+        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
 
@@ -135,7 +140,6 @@ class Navigationd:
       else []
     )
     msg.navigationd.allManeuvers = all_maneuvers
-
     return msg
 
   def run(self):
@@ -143,16 +147,15 @@ class Navigationd:
 
     while True:
       self.sm.update(0)
-      v_ego = self.sm['carState'].vEgo
       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()
-      banner_instructions, progress, nav_data = self._update_navigation(v_ego)
+      banner_instructions, progress, nav_data = self._update_navigation()
 
       msg = self._build_navigation_message(banner_instructions, progress, nav_data, valid=localizer_valid)
 

sunnypilot/navd/tests/test_navigationd.py

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

sunnypilot/selfdrive/controls/controlsd_ext.py

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