From cfb8f3ae242e0f1ed97193b576be21e9c41a698b Mon Sep 17 00:00:00 2001 From: discountchubbs Date: Wed, 5 Nov 2025 18:49:32 -0800 Subject: [PATCH] main entry point for navigation updates --- common/params_keys.h | 1 + sunnypilot/navd/constants.py | 16 ++++++++++++++++ sunnypilot/navd/navigationd.py | 18 ++++++++++++++---- sunnypilot/navd/tests/test_navigationd.py | 2 +- 4 files changed, 32 insertions(+), 5 deletions(-) create mode 100644 sunnypilot/navd/constants.py diff --git a/common/params_keys.h b/common/params_keys.h index ff7bab7ec1..dc70052562 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -188,6 +188,7 @@ inline static std::unordered_map 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}}, diff --git a/sunnypilot/navd/constants.py b/sunnypilot/navd/constants.py new file mode 100644 index 0000000000..716aa7df1a --- /dev/null +++ b/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 diff --git a/sunnypilot/navd/navigationd.py b/sunnypilot/navd/navigationd.py index 57c10bc040..ec2d43c4e7 100755 --- a/sunnypilot/navd/navigationd.py +++ b/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 diff --git a/sunnypilot/navd/tests/test_navigationd.py b/sunnypilot/navd/tests/test_navigationd.py index c9d9f5d75b..641d39491a 100644 --- a/sunnypilot/navd/tests/test_navigationd.py +++ b/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