diff --git a/sunnypilot/navd/navigation_helpers/nav_instructions.py b/sunnypilot/navd/navigation_helpers/nav_instructions.py index 9d4ea000e9..8c232db958 100644 --- a/sunnypilot/navd/navigation_helpers/nav_instructions.py +++ b/sunnypilot/navd/navigation_helpers/nav_instructions.py @@ -4,6 +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 numpy as np + from openpilot.common.constants import CV from openpilot.common.params import Params @@ -109,12 +111,17 @@ class NavigationInstructions: self._route_loaded = False self._no_route = False - def get_upcoming_turn_from_progress(self, progress, current_lat, current_lon) -> str: + 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) + self.coord.latitude = current_lat self.coord.longitude = current_lon distance = self.coord.distance_to(progress['next_turn']['location']) - if distance <= 30.0: + + if distance <= distance_interp: modifier = progress['next_turn']['modifier'] return str(modifier) return 'none' diff --git a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py index 1e477604ad..fe47cfa4e1 100644 --- a/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py +++ b/sunnypilot/navd/navigation_helpers/tests/test_mapbox.py @@ -54,17 +54,17 @@ class TestMapbox: assert 'modifier' in step def test_upcoming_turn_detection(self): - upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon) + upcoming = self.nav.get_upcoming_turn_from_progress(self.progress, self.current_lat, self.current_lon, v_ego=40.0) assert isinstance(upcoming, str) assert upcoming == 'none' if self.route['steps']: turn_lat = self.route['steps'][1]['location'].latitude turn_lon = self.route['steps'][1]['location'].longitude - close_lat = turn_lat - 0.00025 # slightly before the turn + close_lat = turn_lat - 0.000175 # slightly before the turn 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) + upcoming_close = self.nav.get_upcoming_turn_from_progress(self.progress, close_lat, turn_lon, v_ego=0.0) if expected_turn: assert upcoming_close == expected_turn == 'right', "Should be a right turn upcoming"