gap

#	common/params_keys.h
#	sunnypilot/navd/constants.py

common/params_keys.h

@@ -188,11 +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"}},
+    {"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"NavEvents", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // Neural Network Lateral Control

selfdrive/controls/lib/desire_helper.py

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

sunnypilot/navd/constants.py

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

sunnypilot/navd/event_builder.py

@@ -19,7 +19,7 @@ class EventBuilder:
 
   @staticmethod
   def _build_banner_message(metric: bool, nav_msg):
-    m = nav_msg.allManeuvers[-1]
+    m = nav_msg.allManeuvers[1] if len(nav_msg.allManeuvers) > 1 else nav_msg.allManeuvers[0]
     banner = m.instruction
 
     if metric:
@@ -45,8 +45,8 @@ class EventBuilder:
   @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!',
+      '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',

sunnypilot/navd/navigation_desires/navigation_desires.py

@@ -22,9 +22,9 @@ 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']
@@ -41,9 +41,4 @@ class NavigationDesires:
       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):
         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

sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py

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

sunnypilot/navd/navigation_helpers/nav_instructions.py

@@ -43,7 +43,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:
@@ -128,3 +128,11 @@ class NavigationInstructions:
       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
+    return False

sunnypilot/navd/navigation_helpers/tests/test_mapbox.py

@@ -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,8 +54,7 @@ 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)
     assert isinstance(upcoming, str)
     assert upcoming == 'none'
 
@@ -63,28 +62,33 @@ class TestMapbox:
       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)
+      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)
         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.nav.get_current_speed_limit_from_progress(self.progress, True)
+    speed_limit_imperial = self.nav.get_current_speed_limit_from_progress(self.progress, False)
     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))
+    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)
+    assert isinstance(is_arrived, bool)
+    assert not is_arrived

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_event_builder.py

@@ -76,7 +76,7 @@ class TestEventBuilder:
     events = EventBuilder.build_navigation_events(MockSM(nav_msg))
     expected = [{
       'name': custom.OnroadEventSP.EventName.navigationBanner,
-      'message': 'Turning Left. Make sure to nudge the wheel!',
+      'message': 'Turning Left, Make sure to nudge the wheel',
     }]
     assert events == expected
 

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