main entry point for navigation updates

common/params_keys.h

@@ -188,6 +188,7 @@ 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}},

sunnypilot/navd/constants.py

@@ -0,0 +1,16 @@
+"""
+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.
+"""
+
+
+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 n by this
+  METERS_TO_MILE = 1609.344  # divide n by this
+  METERS_TO_FEET = 3.280839895  # multiply n by this

sunnypilot/navd/navigationd.py

@@ -12,6 +12,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
@@ -31,6 +32,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
@@ -46,6 +48,7 @@ class Navigationd:
     if self.last_position is not None:
       self.frame += 1
       if self.frame % 9 == 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)
@@ -62,6 +65,7 @@ class Navigationd:
           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 +78,12 @@ 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.last_position is not None:
       if progress := self.nav_instructions.get_route_progress(self.last_position.latitude, self.last_position.longitude):
         nav_data['upcoming_turn'] = self.nav_instructions.get_upcoming_turn_from_progress(progress, self.last_position.latitude, self.last_position.longitude)
         nav_data['current_speed_limit'] = self.nav_instructions.get_current_speed_limit_from_progress(progress, self.is_metric)
+        arrived = self.nav_instructions.arrived_at_destination(progress)
 
         if progress['current_step']:
           parsed = parse_banner_instructions(progress['current_step']['bannerInstructions'], progress['distance_to_end_of_step'])
@@ -90,10 +94,11 @@ class Navigationd:
         large_distance = progress['distance_from_route'] > 100
 
         if large_distance:
-          if progress['distance_from_route'] > 200:
-            self.cancel_route_counter += 1
+          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
         else:
           self.cancel_route_counter = 0
           self.reroute_counter = 0
@@ -102,6 +107,11 @@ class Navigationd:
         if self.route:
           if progress['current_step_idx'] == len(self.route['steps']) - 1:
             self.allow_recompute = False
+    else:
+      banner_instructions = ''
+      progress = None
+      nav_data = {}
+      self.valid = False
 
     return banner_instructions, progress, nav_data
 

sunnypilot/navd/tests/test_navigationd.py

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