mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-20 14:32:17 +08:00
diff is red so == good?
This commit is contained in:
+3
-9
@@ -458,15 +458,9 @@ struct Navigationd @0xcb9fd56c7057593a {
|
||||
upcomingTurn @0 :Text;
|
||||
currentSpeedLimit @1 :UInt64;
|
||||
bannerInstructions @2 :Text;
|
||||
distanceToNextTurn @3 :Float64;
|
||||
routeProgressPercent @4 :Float64;
|
||||
distanceFromRoute @5 :Float64;
|
||||
routePositionCumulative @6 :Float64;
|
||||
distanceToEndOfStep @7 :Float64;
|
||||
totalDistanceRemaining @8 :Float64;
|
||||
totalTimeRemaining @9 :Float64;
|
||||
allManeuvers @10 :List(Maneuver);
|
||||
valid @11 :Bool;
|
||||
distanceFromRoute @3 :Float64;
|
||||
allManeuvers @4 :List(Maneuver);
|
||||
valid @5 :Bool;
|
||||
|
||||
struct Maneuver {
|
||||
distance @0 :Float64;
|
||||
|
||||
@@ -190,7 +190,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
// Navigation params
|
||||
{"MapboxToken", {PERSISTENT | BACKUP, STRING}},
|
||||
{"MapboxSettings", {CLEAR_ON_MANAGER_START, JSON}},
|
||||
{"MapboxRoute", {CLEAR_ON_MANAGER_START, STRING}},
|
||||
{"MapboxRoute", {PERSISTENT, STRING}},
|
||||
{"MapboxRecompute", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// Neural Network Lateral Control
|
||||
|
||||
@@ -31,18 +31,11 @@ class NavigationInstructions:
|
||||
# The next turn is the next step relative to our cumulative index
|
||||
next_turn_idx = current_step_idx + 1
|
||||
next_turn = route['steps'][next_turn_idx] if 0 <= next_turn_idx < len(route['steps']) else None
|
||||
next_turn_distance = max(0, next_turn['cumulative_distance'] - closest_cumulative) if next_turn else None
|
||||
|
||||
current_maxspeed = current_step['maxspeed']
|
||||
|
||||
distance_to_end_of_step = max(0, current_step['distance'] - (closest_cumulative - current_step['cumulative_distance']))
|
||||
|
||||
# Calculate total remaining distance and time
|
||||
total_distance_remaining: float = max(0, route['total_distance'] - closest_cumulative)
|
||||
progress_in_step = (closest_cumulative - current_step['cumulative_distance']) / current_step['distance']
|
||||
time_left_in_step = (1 - progress_in_step) * current_step['duration']
|
||||
total_time_remaining: float = time_left_in_step + sum(step['duration'] for step in route['steps'][current_step_idx + 1 :])
|
||||
|
||||
all_maneuvers: list = []
|
||||
max_maneuvers = 2
|
||||
for idx in range(current_step_idx, min(current_step_idx + max_maneuvers, len(route['steps']))):
|
||||
@@ -55,17 +48,12 @@ class NavigationInstructions:
|
||||
|
||||
return {
|
||||
'distance_from_route': min_distance,
|
||||
'route_position_cumulative': closest_cumulative,
|
||||
'current_step': current_step,
|
||||
'next_turn': next_turn,
|
||||
'distance_to_next_turn': next_turn_distance,
|
||||
'route_progress_percent': (closest_cumulative / max(1, route['total_distance'])) * 100,
|
||||
'current_maxspeed': current_maxspeed,
|
||||
'distance_to_end_of_step': distance_to_end_of_step,
|
||||
'total_distance_remaining': total_distance_remaining,
|
||||
'total_time_remaining': total_time_remaining,
|
||||
'all_maneuvers': all_maneuvers,
|
||||
'current_step_idx': current_step_idx,
|
||||
'distance_to_end_of_step': distance_to_end_of_step,
|
||||
}
|
||||
|
||||
def get_current_route(self):
|
||||
|
||||
@@ -68,15 +68,10 @@ class TestMapbox:
|
||||
assert progress is not None
|
||||
assert 'distance_from_route' in progress
|
||||
assert 'next_turn' in progress
|
||||
assert 'route_progress_percent' in progress
|
||||
assert 'current_maxspeed' in progress
|
||||
assert 'total_distance_remaining' in progress
|
||||
assert 'total_time_remaining' in progress
|
||||
assert 'all_maneuvers' in progress
|
||||
assert 'distance_to_end_of_step' in progress
|
||||
assert progress['distance_from_route'] >= 0
|
||||
assert 0 <= progress['route_progress_percent'] <= 100
|
||||
assert progress['total_distance_remaining'] >= 0
|
||||
assert progress['total_time_remaining'] >= 0
|
||||
assert isinstance(progress['all_maneuvers'], list)
|
||||
|
||||
speed_limit_metric = self.nav.get_current_speed_limit_from_progress(progress, True)
|
||||
|
||||
@@ -73,11 +73,7 @@ class Navigationd:
|
||||
if parsed:
|
||||
banner_instructions = parsed['maneuverPrimaryText']
|
||||
|
||||
nav_data['distance_to_next_turn'] = progress['distance_to_next_turn']
|
||||
nav_data['distance_to_end_of_step'] = progress['distance_to_end_of_step']
|
||||
nav_data['route_progress_percent'] = progress['route_progress_percent']
|
||||
nav_data['distance_from_route'] = progress['distance_from_route']
|
||||
nav_data['route_position_cumulative'] = progress['route_position_cumulative']
|
||||
|
||||
# Don't recompute in last segment to prevent reroute loops
|
||||
if self.route:
|
||||
@@ -85,22 +81,17 @@ class Navigationd:
|
||||
self.allow_recompute = False
|
||||
|
||||
if self.recompute_allowed:
|
||||
self.reroute_counter += 1 if nav_data['distance_from_route'] > 25 else 0
|
||||
self.reroute_counter += 1 if nav_data['distance_from_route'] > 100 else 0
|
||||
|
||||
return banner_instructions, progress, nav_data
|
||||
|
||||
def _build_navigation_message(self, 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.distanceToNextTurn = nav_data.get('distance_to_next_turn', 0.0)
|
||||
msg.navigationd.distanceToEndOfStep = nav_data.get('distance_to_end_of_step', 0.0)
|
||||
msg.navigationd.routeProgressPercent = nav_data.get('route_progress_percent', 0.0)
|
||||
msg.navigationd.distanceFromRoute = nav_data.get('distance_from_route', 0.0)
|
||||
msg.navigationd.routePositionCumulative = nav_data.get('route_position_cumulative', 0.0)
|
||||
msg.navigationd.totalDistanceRemaining = progress['total_distance_remaining'] if progress else 0.0
|
||||
msg.navigationd.totalTimeRemaining = progress['total_time_remaining'] if progress else 0.0
|
||||
msg.navigationd.valid = self.valid
|
||||
|
||||
all_maneuvers = (
|
||||
@@ -127,7 +118,7 @@ class Navigationd:
|
||||
self._update_params()
|
||||
banner_instructions, progress, nav_data = self._update_navigation()
|
||||
|
||||
msg = self._build_navigation_message(banner_instructions, progress, nav_data)
|
||||
msg = self._build_navigation_message(banner_instructions, progress, nav_data, valid=localizer_valid)
|
||||
|
||||
self.pm.send('navigationd', msg)
|
||||
self.rk.keep_time()
|
||||
|
||||
@@ -47,8 +47,6 @@ class TestNavigationd:
|
||||
nav = Navigationd()
|
||||
msg = nav._build_navigation_message('', None, {})
|
||||
assert msg.navigationd.bannerInstructions == ''
|
||||
assert msg.navigationd.totalDistanceRemaining == 0.0
|
||||
assert msg.navigationd.totalTimeRemaining == 0.0
|
||||
assert msg.navigationd.valid is False
|
||||
else:
|
||||
sm = messaging.SubMaster(['navigationd'])
|
||||
@@ -60,6 +58,4 @@ class TestNavigationd:
|
||||
received_msg = sm['navigationd']
|
||||
|
||||
assert received_msg.bannerInstructions == msg.navigationd.bannerInstructions
|
||||
assert received_msg.totalDistanceRemaining == msg.navigationd.totalDistanceRemaining
|
||||
assert received_msg.totalTimeRemaining == msg.navigationd.totalTimeRemaining
|
||||
assert received_msg.valid == msg.navigationd.valid
|
||||
|
||||
Reference in New Issue
Block a user