gap

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

cereal/custom.capnp

@@ -340,6 +340,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    navigationBanner @24;
   }
 }
 
@@ -454,7 +455,20 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
   }
 }
 
-struct CustomReserved10 @0xcb9fd56c7057593a {
+struct Navigationd @0xcb9fd56c7057593a {
+  upcomingTurn @0 :Text;
+  currentSpeedLimit @1 :UInt64;
+  bannerInstructions @2 :Text;
+  distanceFromRoute @3 :Float64;
+  allManeuvers @4 :List(Maneuver);
+  valid @5 :Bool;
+
+  struct Maneuver {
+    distance @0 :Float64;
+    type @1 :Text;
+    modifier @2 :Text;
+    instruction @3 :Text;
+  }
 }
 
 struct CustomReserved11 @0xc2243c65e0340384 {

cereal/log.capnp

@@ -2632,7 +2632,7 @@ struct Event {
     carStateSP @114 :Custom.CarStateSP;
     liveMapDataSP @115 :Custom.LiveMapDataSP;
     modelDataV2SP @116 :Custom.ModelDataV2SP;
-    customReserved10 @136 :Custom.CustomReserved10;
+    navigationd @136 :Custom.Navigationd;
     customReserved11 @137 :Custom.CustomReserved11;
     customReserved12 @138 :Custom.CustomReserved12;
     customReserved13 @139 :Custom.CustomReserved13;

cereal/services.py

@@ -89,6 +89,7 @@ _services: dict[str, tuple] = {
   "carStateSP": (True, 100., 10),
   "liveMapDataSP": (True, 1., 1),
   "modelDataV2SP": (True, 20.),
+  "navigationd": (True, 3.),
   "liveLocationKalman": (True, 20.),
 
   # debug

common/params_keys.h

@@ -188,9 +188,13 @@ 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", {CLEAR_ON_MANAGER_START, STRING}},
+    {"MapboxRoute", {PERSISTENT, STRING}},
+    {"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"NavDesiresAllowed", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"NavEvents", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // Neural Network Lateral Control
     {"NeuralNetworkLateralControl", {PERSISTENT | BACKUP, BOOL, "0"}},

selfdrive/controls/lib/desire_helper.py

@@ -3,6 +3,7 @@ from openpilot.common.constants import CV
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController
+from openpilot.sunnypilot.navd.navigation_desires.navigation_desires import NavigationDesires
 
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -51,6 +52,7 @@ class DesireHelper:
     self.alc = AutoLaneChangeController(self)
     self.lane_turn_controller = LaneTurnController(self)
     self.lane_turn_direction = TurnDirection.none
+    self.navigation_desires = NavigationDesires()
 
   @staticmethod
   def get_lane_change_direction(CS):
@@ -143,3 +145,7 @@ class DesireHelper:
         self.desire = log.Desire.none
 
     self.alc.update_state()
+
+    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

selfdrive/controls/plannerd.py

@@ -27,7 +27,7 @@ def main():
   longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
   pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
   sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
-                            'liveMapDataSP', 'carStateSP', gps_location_service],
+                            'liveMapDataSP', 'navigationd', 'carStateSP', gps_location_service],
                            poll='carState')
 
   while True:

selfdrive/selfdrived/selfdrived.py

@@ -88,7 +88,7 @@ class SelfdriveD(CruiseHelper):
     # TODO: de-couple selfdrived with card/conflate on carState without introducing controls mismatches
     self.car_state_sock = messaging.sub_sock('carState', timeout=20)
 
-    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
+    ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP'] + ['navigationd']
     if SIMULATION:
       ignore += ['driverCameraState', 'managerState']
     if REPLAY:
@@ -98,7 +98,7 @@ class SelfdriveD(CruiseHelper):
                                    'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug', 'userBookmark', 'audioFeedback',
-                                   'modelDataV2SP', 'longitudinalPlanSP'] + \
+                                   'modelDataV2SP', 'longitudinalPlanSP', 'navigationd'] + \
                                    self.camera_packets + self.sensor_packets + self.gps_packets,
                                   ignore_alive=ignore, ignore_avg_freq=ignore,
                                   ignore_valid=ignore, frequency=int(1/DT_CTRL))

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -71,6 +71,7 @@ class Plant:
     model = messaging.new_message('modelV2')
     car_state_sp = messaging.new_message('carStateSP')
     live_map_data_sp = messaging.new_message('liveMapDataSP')
+    navigationd = messaging.new_message('navigationd')
     gps_data = messaging.new_message('gpsLocation')
     a_lead = (v_lead - self.v_lead_prev)/self.ts
     self.v_lead_prev = v_lead
@@ -141,6 +142,7 @@ class Plant:
           'modelV2': model.modelV2,
           'carStateSP': car_state_sp.carStateSP,
           'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+          'navigationd': navigationd.navigationd,
           'gpsLocation': gps_data.gpsLocation}
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target

sunnypilot/navd/README.md

@@ -3,3 +3,4 @@
 Navigation daemon with Mapbox integration for semi-offline navigation. This module handles route planning, geocoding, and turn-by-turn instructions to support autonomous driving features.
 
 - `navigation_helpers/`: Mapbox API integration and navigation instructions processing.
+- `navigationd`: Navigation service which uses mapbox integration to generate a route and keep it up to date. This service runs at three hz, using keep time to ensure the while loop only updates three times a second rather than every time sm updates, which in this case would be twenty hz (LLK).

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/event_builder.py

@@ -0,0 +1,83 @@
+"""
+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.
+"""
+from cereal import custom, messaging
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+
+from sunnypilot.navd.constants import NAV_CV
+
+
+class EventBuilder:
+  def __init__(self):
+    self._counter: int = -1
+    self._enabled: bool = False
+    self._params = Params()
+
+  @staticmethod
+  def _build_banner_message(metric: bool, nav_msg):
+    m = nav_msg.allManeuvers[1] if len(nav_msg.allManeuvers) > 1 else nav_msg.allManeuvers[0]
+    banner = m.instruction
+
+    if metric:
+      dist = f'{m.distance / NAV_CV.METERS_TO_KILO:.1f} km,'
+      if m.distance < NAV_CV.SHORT_DISTANCE_METERS:
+        dist = f'{int(m.distance)}m,'
+    else:
+      dist = f'{m.distance / NAV_CV.METERS_TO_MILE:.1f} mi,'
+      if m.distance < NAV_CV.QUARTER_MILE:
+        dist = f'{round((m.distance * NAV_CV.METERS_TO_FEET) / 50) * 50}ft,'
+
+    if m.type == 'arrive' or m.type == 'depart' or 'Your destination' in banner:
+      base_msg = banner
+    elif banner.startswith(('Continue', 'Drive', 'Head')):
+      base_msg = f'For {dist} {banner}'
+    elif 'Turn' in banner or 'Take' in banner or 'Make' in banner:
+      base_msg = f'In {dist} {banner}'
+    else:
+      base_msg = f'For {dist} Continue on {banner}'
+
+    return base_msg
+
+  @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',
+      'slightLeft': 'Keeping Left',
+      'slightRight': 'Keeping Right',
+      'sharpLeft': 'Sharp Left Turn',
+      'sharpRight': 'Sharp Right Turn',
+      'straight': 'Continuing Straight',
+      'uturn': 'U-Turn Ahead',
+    }
+    return turn_messages.get(upcoming_turn, f"Upcoming {upcoming_turn.replace('_', ' ').title()}")
+
+  @staticmethod
+  def build_navigation_events(sm: messaging.SubMaster, metric=True) -> list:
+    nav_msg = sm['navigationd']
+    if not nav_msg.valid:
+      return []
+
+    banner_message = EventBuilder._build_banner_message(metric, nav_msg)
+
+    if nav_msg.upcomingTurn != 'none':
+      banner_message = EventBuilder._get_turning_message(nav_msg.upcomingTurn)
+
+    return [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': banner_message,
+    }]
+
+  def update(self, sm: messaging.SubMaster) -> list:
+    self._counter += 1
+    if self._counter % int(3.0 / DT_MDL) == 0:
+      self._enabled = self._params.get("NavEvents", return_default=True)
+
+    if self._enabled:
+      return self.build_navigation_events(sm)
+    else:
+      return []

sunnypilot/navd/navigation_desires/navigation_desires.py

@@ -0,0 +1,44 @@
+"""
+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.
+"""
+import cereal.messaging as messaging
+from cereal import car, log
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+
+
+class NavigationDesires:
+  def __init__(self):
+    self.sm = messaging.SubMaster(['navigationd'])
+    self.desire = log.Desire.none
+    self._turn_speed_limit = 20 * CV.MPH_TO_MS
+    self._params = Params()
+    self.param_counter = -1
+    self.nav_allowed: bool = False
+
+  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("NavDesiresAllowed", return_default=True)
+
+  def update(self, CS: car.CarState, lateral_active: bool) -> log.Desire:
+    self.update_params()
+    self.sm.update(0)
+    nav_msg = self.sm['navigationd']
+    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
+      elif (upcoming == 'left' and CS.steeringPressed and CS.steeringTorque > 0 and not CS.rightBlinker
+            and not CS.leftBlindspot and CS.vEgo < self._turn_speed_limit):
+        self.desire = log.Desire.turnLeft
+      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
+    return self.desire

sunnypilot/navd/navigation_desires/tests/test_navigation_desires.py

@@ -0,0 +1,96 @@
+"""
+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.
+"""
+import pytest
+import types
+
+from cereal import log
+from openpilot.common.params import Params
+
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.sunnypilot.navd.navigation_desires.navigation_desires import NavigationDesires
+
+
+def make_car(vEgo=0, leftBlinker=False, rightBlinker=False, leftBlindspot=False, rightBlindspot=False, steeringPressed=False, steeringTorque=0):
+  return types.SimpleNamespace(
+    vEgo=vEgo, leftBlinker=leftBlinker, rightBlinker=rightBlinker,
+    leftBlindspot=leftBlindspot, rightBlindspot=rightBlindspot,
+    steeringPressed=steeringPressed, steeringTorque=steeringTorque
+  )
+
+NAVIGATION_PARAMS: list[tuple] = [
+  ('slightLeft', make_car(), log.Desire.keepLeft),
+  ('slightRight', make_car(), log.Desire.keepRight),
+  ('slightLeft', make_car(vEgo=9, leftBlindspot=True), log.Desire.none),
+  ('slightRight', make_car(vEgo=9, rightBlindspot=True), log.Desire.none),
+  ('left', make_car(vEgo=5, leftBlinker=True, rightBlinker=False, leftBlindspot=False, steeringPressed=True, steeringTorque=1), log.Desire.turnLeft),
+  ('left', make_car(vEgo=5, leftBlinker=False, rightBlinker=True), log.Desire.none),
+  ('right', make_car(vEgo=6, rightBlinker=True, leftBlindspot=False, steeringPressed=True, steeringTorque=-1), log.Desire.turnRight),
+  ('right', make_car(vEgo=6, rightBlinker=True, rightBlindspot=True), log.Desire.none),
+  ('left', make_car(vEgo=9, leftBlinker=True), log.Desire.none),
+]
+
+INTEGRATION_PARAMS: list[tuple] = [(carstate, upcoming, log.Desire.none, expected) for upcoming, carstate, expected in NAVIGATION_PARAMS] + [
+  (make_car(vEgo=9, leftBlinker=True, steeringPressed=True, steeringTorque=1), 'slightLeft', log.Desire.turnLeft, log.Desire.keepLeft),
+  (make_car(vEgo=9, rightBlinker=True, steeringPressed=True, steeringTorque=-1), 'slightRight', log.Desire.turnRight, log.Desire.keepRight),
+  (make_car(vEgo=9, leftBlinker=True), 'slightLeft', log.Desire.laneChangeLeft, log.Desire.laneChangeLeft),
+  (make_car(vEgo=9, rightBlinker=True), 'slightRight', log.Desire.laneChangeRight, log.Desire.laneChangeRight),
+  (make_car(vEgo=9), 'none', log.Desire.none, log.Desire.none),
+]
+
+def make_nav_msg(valid=False, upcoming='none'):
+  return types.SimpleNamespace(valid=valid, upcomingTurn=upcoming)
+
+def params_setter(allowed: bool):
+  params = Params()
+  params.put("NavDesiresAllowed", allowed)
+
+@pytest.fixture
+def mock_submaster(mocker):
+  mock_sm = mocker.patch('cereal.messaging.SubMaster')
+  mock_sm_instance = mocker.Mock()
+  mock_sm.return_value = mock_sm_instance
+  mock_sm_instance.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=False))
+  params_setter(True)
+  return mock_sm_instance
+
+@pytest.mark.parametrize("upcoming, carstate, expected", NAVIGATION_PARAMS)
+def test_navigation_desires_update(mock_submaster, mocker, upcoming, carstate, expected):
+  nav_desires = NavigationDesires()
+  nav_desires.sm.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming=upcoming))
+  nav_desires.update(carstate, True)
+  assert nav_desires.desire == expected
+
+@pytest.mark.parametrize("msg_valid,lateral_active", [(False, True), (True, False)])
+def test_invalid_or_inactive(mock_submaster, mocker, msg_valid, lateral_active):
+  nav_desires = NavigationDesires()
+  nav_desires.sm.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=msg_valid, upcoming='slightLeft'))
+  nav_desires.update(make_car(), lateral_active)
+  assert nav_desires.desire == log.Desire.none
+
+def test_update(mock_submaster, mocker):
+  mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming='left'))
+  nav_desires = NavigationDesires()
+
+  assert nav_desires.update(make_car(leftBlinker=True, steeringPressed=True, steeringTorque=1), True) == log.Desire.turnLeft
+
+  params_setter(False)
+  nav_desires.param_counter = 59
+  nav_desires.update(make_car(leftBlinker=True), True)
+  assert nav_desires.desire == log.Desire.none
+
+@pytest.mark.parametrize("carstate, upcoming, current_desire, expected", INTEGRATION_PARAMS)
+def test_desire_helper(mock_submaster, mocker, carstate, upcoming, current_desire, expected):
+  mock_submaster.__getitem__ = mocker.Mock(return_value=make_nav_msg(valid=True, upcoming=upcoming))
+  dh = DesireHelper()
+  dh.desire = current_desire
+
+  if current_desire in (log.Desire.laneChangeLeft, log.Desire.laneChangeRight):
+    dh.lane_change_state = log.LaneChangeState.laneChangeStarting
+    dh.lane_change_direction = log.LaneChangeDirection.left if current_desire == log.Desire.laneChangeLeft else log.LaneChangeDirection.right
+
+  dh.update(carstate, True, 1.0)
+  assert dh.desire == expected

sunnypilot/navd/navigationd.py

@@ -0,0 +1,160 @@
+"""
+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.
+"""
+import math
+
+import cereal.messaging as messaging
+from cereal import custom
+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
+
+
+class Navigationd:
+  def __init__(self):
+    self.params = Params()
+    self.mapbox = MapboxIntegration()
+    self.nav_instructions = NavigationInstructions()
+
+    self.sm = messaging.SubMaster(['liveLocationKalman'])
+    self.pm = messaging.PubMaster(['navigationd'])
+    self.rk = Ratekeeper(3) # 3 Hz
+
+    self.route = None
+    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
+    self.cancel_route_counter: int = 0
+
+    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:
+        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
+      )
+
+      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()
+          self.route = self.nav_instructions.get_current_route()
+          self.cancel_route_counter = 0
+          self.reroute_counter = 0
+
+      if self.cancel_route_counter == 30:
+        self.cancel_route_counter = 0
+        self.destination = None
+        self.nav_instructions.clear_route_cache()
+        self.route = None
+
+      self.valid = self.route is not None
+
+  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 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'])
+          if parsed:
+            banner_instructions = parsed['maneuverPrimaryText']
+
+        nav_data['distance_from_route'] = progress['distance_from_route']
+        large_distance = progress['distance_from_route'] > 100
+
+        if large_distance:
+          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
+
+        # Don't recompute in last segment to prevent reroute loops
+        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
+
+  def _build_navigation_message(self, banner_instructions: str, progress: dict | None, nav_data: dict, valid: bool):
+    msg = messaging.new_message('navigationd')
+    msg.valid = valid
+    msg.navigationd.upcomingTurn = nav_data.get('upcoming_turn', 'none')
+    msg.navigationd.currentSpeedLimit = nav_data.get('current_speed_limit', 0)
+    msg.navigationd.bannerInstructions = banner_instructions
+    msg.navigationd.distanceFromRoute = nav_data.get('distance_from_route', 0.0)
+    msg.navigationd.valid = self.valid
+
+    all_maneuvers = (
+      [custom.Navigationd.Maneuver.new_message(distance=m['distance'], type=m['type'], modifier=m['modifier'],
+                                               instruction=m['instruction']) for m in progress['all_maneuvers']]
+      if progress
+      else []
+    )
+    msg.navigationd.allManeuvers = all_maneuvers
+
+    return msg
+
+  def run(self):
+    cloudlog.warning('navigationd init')
+
+    while True:
+      self.sm.update()
+      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_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
+
+      self._update_params()
+      banner_instructions, progress, nav_data = self._update_navigation()
+
+      msg = self._build_navigation_message(banner_instructions, progress, nav_data, valid=localizer_valid)
+
+      self.pm.send('navigationd', msg)
+      self.rk.keep_time()
+
+
+def main():
+  nav = Navigationd()
+  nav.run()

sunnypilot/navd/tests/test_event_builder.py

@@ -0,0 +1,92 @@
+"""
+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.
+"""
+from cereal import custom
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
+
+
+class MockSM(dict):
+  def __init__(self, nav_msg):
+    super().__init__()
+    self['navigationd'] = nav_msg
+
+
+class TestEventBuilder:
+  def setup_method(self):
+    self.params = Params()
+    self.event_builder = EventBuilder()
+
+  def create_nav_msg(self, upcoming_turn='none', valid=True):
+    nav_msg = custom.Navigationd.new_message()
+    nav_msg.valid = valid
+    nav_msg.upcomingTurn = upcoming_turn
+    nav_msg.allManeuvers = [
+      custom.Navigationd.Maneuver.new_message(distance=192.84873284, type='turn', modifier='left', instruction='West Esplanade Drive'),
+      custom.Navigationd.Maneuver.new_message(distance=192.84809314, type='turn', modifier='right', instruction='West Esplanade Drive'),
+    ]
+    return nav_msg
+
+  def test_validity(self):
+    nav_msg = self.create_nav_msg(valid=False)
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    assert events == []
+
+  def test_enabled(self):
+    self.params.put("NavEvents", True)
+    nav_msg = self.create_nav_msg()
+    events = self.event_builder.update(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 192m, Continue on West Esplanade Drive'
+    }]
+    assert events == expected
+
+    self.params.put("NavEvents", False)
+    self.event_builder._counter = 59
+    events = self.event_builder.update(MockSM(nav_msg))
+    assert events == []
+
+
+  def test_build_navigation_events(self):
+    nav_msg = self.create_nav_msg()
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 650ft, Continue on West Esplanade Drive',
+    }]
+    assert events == expected
+
+  def test_distance_condition_imperial(self):
+    nav_msg = self.create_nav_msg()
+    nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=160.0, type='continue', modifier='straight', instruction='1234 Apple Way')
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg), False)
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 500ft, Continue on 1234 Apple Way',
+    }]
+    assert events == expected
+
+  def test_upcoming_turn_override(self):
+    nav_msg = self.create_nav_msg(upcoming_turn='left')
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'Turning Left, Make sure to nudge the wheel',
+    }]
+    assert events == expected
+
+  def test_straight(self):
+    nav_msg = self.create_nav_msg()
+    nav_msg.allManeuvers[1] = custom.Navigationd.Maneuver.new_message(distance=80.0, type='continue', modifier='straight', instruction='1234 Apple Way')
+
+    events = EventBuilder.build_navigation_events(MockSM(nav_msg))
+    expected = [{
+      'name': custom.OnroadEventSP.EventName.navigationBanner,
+      'message': 'For 80m, Continue on 1234 Apple Way'
+    }]
+    assert events == expected

sunnypilot/navd/tests/test_navigationd.py

@@ -0,0 +1,76 @@
+"""
+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.
+"""
+import platform
+import pytest
+
+import cereal.messaging as messaging
+
+from openpilot.sunnypilot.navd.navigationd import Navigationd
+from openpilot.sunnypilot.navd.helpers import Coordinate
+
+
+class TestNavigationd:
+  is_darwin = platform.system() == "Darwin"
+
+  @pytest.fixture(autouse=True)
+  def setup_method(self, mocker):
+    if self.is_darwin:
+      mocker.patch('cereal.messaging.SubMaster')
+      mocker.patch('cereal.messaging.PubMaster')
+
+  def test_update_params(self):
+    nav = Navigationd()
+    nav.last_position = None
+    nav._update_params()
+    assert nav.frame == -1
+    nav.last_position = Coordinate(latitude=37.0, longitude=128.0)
+    nav._update_params()
+    assert nav.frame == 0  # frame only updates when last position is set
+
+  def test_update_navigation_no_position(self):
+    nav = Navigationd()
+    nav.last_position = None
+    banner, progress, nav_data = nav._update_navigation()
+    assert banner == ''
+    assert progress is None
+    assert nav_data == {}
+
+  def test_update_navigation(self):
+    nav = Navigationd()
+    nav.last_position = Coordinate(latitude=37.0, longitude=128.0)
+    nav.route = {'580 Winchester dr, oxnard, CA': True}
+    banner, progress, nav_data = nav._update_navigation()
+    assert isinstance(banner, str)
+    assert not progress  # no route was actually set
+    assert isinstance(nav_data, dict)
+
+  def test_build_navigation_message(self):
+    if self.is_darwin:
+      nav = Navigationd()
+      msg = nav._build_navigation_message('', None, {}, True)
+      assert msg.navigationd.bannerInstructions == ''
+      assert msg.navigationd.valid is False
+    else:
+      sm = messaging.SubMaster(['navigationd'])
+      nav = Navigationd()
+      msg = nav._build_navigation_message('', None, {}, True)
+
+      nav.pm.send('navigationd', msg)
+      sm.update()
+      received_msg = sm['navigationd']
+
+      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/lib/longitudinal_planner.py

@@ -16,6 +16,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
@@ -32,6 +33,7 @@ class LongitudinalPlannerSP:
     self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
     self.source = LongitudinalPlanSource.cruise
     self.e2e_alerts_helper = E2EAlertsHelper()
+    self.event_builder = EventBuilder()
 
     self.output_v_target = 0.
     self.output_a_target = 0.
@@ -77,10 +79,16 @@ class LongitudinalPlannerSP:
     self.output_v_target, self.output_a_target = targets[self.source]
     return self.output_v_target, self.output_a_target
 
+  def update_navigation_events(self, sm: messaging.SubMaster) -> None:
+    nav_events = self.event_builder.update(sm)
+    for event in nav_events:
+      self.events_sp.add(event['name'])
+
   def update(self, sm: messaging.SubMaster) -> None:
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
+    self.update_navigation_events(sm)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')

sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py

@@ -7,11 +7,17 @@ See the LICENSE.md file in the root directory for more details.
 
 from parameterized import parameterized
 
+import cereal.messaging
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \
   AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY
 
+class MockSubMaster:
+  def __init__(self, services):
+    pass
+
+
 AUTO_LANE_CHANGE_TIMER_COMBOS = [
   (AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]),
   (AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]),
@@ -23,6 +29,7 @@ AUTO_LANE_CHANGE_TIMER_COMBOS = [
 
 class TestAutoLaneChangeController:
   def setup_method(self):
+    cereal.messaging.SubMaster = MockSubMaster
     self.DH = DesireHelper()
     self.alc = AutoLaneChangeController(self.DH)
 

sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -1,14 +1,29 @@
 import pytest
+
+import cereal.messaging
 from cereal import log, custom
 from openpilot.common.params import Params
 
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
-from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
 
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
 
+class MockSubMaster:
+  def __init__(self, services): pass
+
+  def update(self, timeout): pass
+
+  def __getitem__(self, key):
+    return type('nav_msg', (), {'valid': False})()
+
+
+@pytest.fixture(autouse=True)
+def mock_submaster():
+  cereal.messaging.SubMaster = MockSubMaster
+
+
 @pytest.mark.parametrize("left_blinker,right_blinker,v_ego,blindspot_left,blindspot_right,expected", [
   (True, False, 5, False, False, TurnDirection.turnLeft),
   (False, True, 6, False, False, TurnDirection.turnRight),
@@ -107,7 +122,6 @@ def set_lane_turn_params():
 ])
 def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, expected_desire, set_lane_turn_params):
   dh = DesireHelper()
-  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
     dh.update(carstate, lateral_active, lane_change_prob)
-  assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+  assert dh.desire == expected_desire

sunnypilot/selfdrive/selfdrived/events.py

@@ -4,6 +4,7 @@ from openpilot.common.constants import CV
 from openpilot.sunnypilot.selfdrive.selfdrived.events_base import EventsBase, Priority, ET, Alert, \
   NoEntryAlert, ImmediateDisableAlert, EngagementAlert, NormalPermanentAlert, AlertCallbackType, wrong_car_mode_alert
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit import PCM_LONG_REQUIRED_MAX_SET_SPEED, CONFIRM_SPEED_THRESHOLD
+from openpilot.sunnypilot.navd.event_builder import EventBuilder
 
 
 AlertSize = log.SelfdriveState.AlertSize
@@ -55,6 +56,14 @@ def speed_limit_pre_active_alert(CP: car.CarParams, CS: car.CarState, sm: messag
     Priority.LOW, VisualAlert.none, AudibleAlertSP.promptSingleLow, .1)
 
 
+def navigation_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  events = EventBuilder.build_navigation_events(sm, metric)
+  if not events:
+    return Alert("", "", AlertStatus.normal, AlertSize.none, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0.)
+
+  return Alert(events[0]['message'], "", AlertStatus.normal, AlertSize.small, Priority.LOW, VisualAlert.none, AudibleAlert.none, 2.)
+
+
 class EventsSP(EventsBase):
   def __init__(self):
     super().__init__()
@@ -226,4 +235,8 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.navigationBanner: {
+    ET.WARNING: navigation_alert,
+  },
 }

system/manager/process_config.py

@@ -180,6 +180,9 @@ procs += [
   NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
   PythonProcess("mapd_manager", "sunnypilot.mapd.mapd_manager", always_run),
 
+  # navigationd
+  PythonProcess("navigationd", "sunnypilot.navd.navigationd", only_onroad),
+
   # locationd
   NativeProcess("locationd_llk", "sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
 ]