some more

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

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
 
 
 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 = 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)
@@ -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,
@@ -111,11 +114,27 @@ 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:
+      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 = 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 +146,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()
@@ -77,15 +74,17 @@ class Navigationd:
 
       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