mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-13 02:42:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

use resolver

Browse Source
This commit is contained in:
Jason Wen
2025-09-24 22:54:32 -04:00
parent 236540611e
commit d9b97ffbaf
3 changed files with 37 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+3 -2
View File
@@ -61,8 +61,9 @@ class LongitudinalPlannerSP:
self.resolver.update(v_ego, sm)
# Speed Limit Assist
self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster,
self.resolver.speed_limit, self.resolver.speed_limit_offset, self.resolver.distance, self.events_sp)
has_speed_limit = self.resolver.speed_limit_valid or self.resolver.speed_limit_last_valid
self.sla.update(long_enabled, long_override, v_ego, a_ego, v_cruise_cluster, self.resolver.speed_limit,
self.resolver.speed_limit_final_last, has_speed_limit, self.resolver.distance, self.events_sp)
targets = {
LongitudinalPlanSource.cruise: (v_cruise, a_ego),
sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py
+17 -34
View File
@@ -33,13 +33,6 @@ LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and curren
class SpeedLimitAssist:
_speed_limit: float
_speed_limit_offset: float
_distance: float
v_ego: float
a_ego: float
v_offset: float
last_valid_speed_limit_final: float
output_v_target: float = V_CRUISE_UNSET
output_a_target: float = 0.
@@ -64,10 +57,10 @@ class SpeedLimitAssist:
self.v_cruise_cluster_prev = 0.
self.v_cruise_cluster_conv = 0
self.prev_v_cruise_cluster_conv = 0
self._has_speed_limit = False
self._speed_limit = 0.
self._speed_limit_offset = 0.
self._speed_limit_final_last = 0.
self.speed_limit_prev = 0.
self.last_valid_speed_limit_final = 0.
self.speed_limit_final_conv = 0
self.prev_speed_limit_final_conv = 0
self._distance = 0.
@@ -86,10 +79,6 @@ class SpeedLimitAssist:
SpeedLimitAssistState.active: self.get_active_state_target_acceleration,
}
@property
def speed_limit_final(self) -> float:
return self._speed_limit + self._speed_limit_offset
@property
def speed_limit_changed(self) -> bool:
return bool(self._speed_limit != self.speed_limit_prev)
@@ -99,15 +88,8 @@ class SpeedLimitAssist:
return bool(self.v_cruise_cluster != self.v_cruise_cluster_prev)
def get_v_target_from_control(self) -> float:
if self.is_enabled:
# If we have a current valid speed limit, use it
if self._speed_limit > 0:
self.last_valid_speed_limit_final = self.speed_limit_final
return self.speed_limit_final
# If no current speed limit but we have a last valid one, use that
if self.last_valid_speed_limit_final > 0:
return self.last_valid_speed_limit_final
if self.is_enabled and (self._speed_limit_final_last > 0):
return self._speed_limit_final_last
# Fallback
return V_CRUISE_UNSET
@@ -130,9 +112,9 @@ class SpeedLimitAssist:
self.v_cruise_cluster = v_cruise_cluster
# Update current velocity offset (error)
self.v_offset = self.speed_limit_final - self.v_ego
self.v_offset = self._speed_limit_final_last - self.v_ego
self.speed_limit_final_conv = round(self.speed_limit_final * speed_conv)
self.speed_limit_final_conv = round(self._speed_limit_final_last * speed_conv)
self.v_cruise_cluster_conv = round(self.v_cruise_cluster * speed_conv)
pcm_long_required_max_set_speed_conv = round(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.is_metric] * speed_conv)
self.target_set_speed_conv = pcm_long_required_max_set_speed_conv if self.pcm_op_long else self.speed_limit_final_conv
@@ -142,7 +124,7 @@ class SpeedLimitAssist:
def get_adapting_state_target_acceleration(self) -> float:
if self._distance > 0:
return (self.speed_limit_final ** 2 - self.v_ego ** 2) / (2. * self._distance)
return (self._speed_limit_final_last ** 2 - self.v_ego ** 2) / (2. * self._distance)
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
@@ -150,7 +132,7 @@ class SpeedLimitAssist:
return self.v_offset / float(ModelConstants.T_IDXS[CONTROL_N])
def _update_confirmed_state(self):
if self._speed_limit > 0:
if self._has_speed_limit:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
@@ -188,7 +170,7 @@ class SpeedLimitAssist:
if self.state == SpeedLimitAssistState.active:
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
# ADAPTING
@@ -200,7 +182,7 @@ class SpeedLimitAssist:
# PENDING
elif self.state == SpeedLimitAssistState.pending:
if self._speed_limit > 0:
if self._has_speed_limit:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
@@ -260,7 +242,7 @@ class SpeedLimitAssist:
if self.state == SpeedLimitAssistState.active:
if self.v_cruise_cluster_changed:
self.state = SpeedLimitAssistState.inactive
elif self._speed_limit > 0 and self.v_offset < LIMIT_SPEED_OFFSET_TH:
elif self._has_speed_limit and self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
# ADAPTING
@@ -272,7 +254,7 @@ class SpeedLimitAssist:
# PENDING
elif self.state == SpeedLimitAssistState.pending:
if self._speed_limit > 0:
if self._has_speed_limit:
if self.v_offset < LIMIT_SPEED_OFFSET_TH:
self.state = SpeedLimitAssistState.adapting
else:
@@ -325,20 +307,21 @@ class SpeedLimitAssist:
events_sp.add(EventNameSP.speedLimitActive)
# only notify if we acquire a valid speed limit
elif self.speed_limit_changed and self._speed_limit > 0:
elif self.speed_limit_changed and self._has_speed_limit:
if self.speed_limit_prev <= 0:
events_sp.add(EventNameSP.speedLimitActive)
elif self.speed_limit_prev > 0:
events_sp.add(EventNameSP.speedLimitChanged)
def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float,
speed_limit: float, speed_limit_offset: float, distance: float, events_sp: EventsSP) -> None:
def update(self, long_enabled: bool, long_override: bool, v_ego: float, a_ego: float, v_cruise_cluster: float, speed_limit: float,
speed_limit_final_last: float, has_speed_limit: bool, distance: float, events_sp: EventsSP) -> None:
self.long_enabled = long_enabled
self.v_ego = v_ego
self.a_ego = a_ego
self._has_speed_limit = has_speed_limit
self._speed_limit = speed_limit
self._speed_limit_offset = speed_limit_offset
self._speed_limit_final_last = speed_limit_final_last
self._distance = distance
self.update_params()
sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py
+17 -19
View File
@@ -84,58 +84,58 @@ class TestSpeedLimitAssist:
def test_disabled(self):
self.params.put("SpeedLimitMode", int(Mode.off))
for _ in range(int(10. / DT_MDL)):
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.disabled
def test_transition_disabled_to_preactive(self):
for _ in range(int(3. / DT_MDL)):
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.preActive
assert self.sla.is_enabled and not self.sla.is_active
def test_preactive_to_active_with_max_speed_confirmation(self):
self.sla.state = SpeedLimitAssistState.preActive
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.active
assert self.sla.is_enabled and self.sla.is_active, f"enabled: {self.sla.is_enabled}, active: {self.sla.is_active}"
assert self.sla.output_v_target == SPEED_LIMITS['city']
def test_preactive_timeout_to_inactive(self):
self.sla.state = SpeedLimitAssistState.preActive
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
for _ in range(int(PRE_ACTIVE_GUARD_PERIOD / DT_MDL)):
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.inactive
def test_preactive_to_pending_no_speed_limit(self):
self.sla.state = SpeedLimitAssistState.preActive
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, False, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.pending
assert self.sla.is_enabled and not self.sla.is_active
def test_pending_to_active_when_speed_limit_available(self):
self.sla.state = SpeedLimitAssistState.pending
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.active
def test_pending_to_adapting_when_below_speed_limit(self):
self.sla.state = SpeedLimitAssistState.pending
self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'] + 5, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.adapting
assert self.sla.is_enabled and self.sla.is_active
def test_active_to_adapting_transition(self):
self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric])
self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'] + 2, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.adapting
def test_adapting_to_active_transition(self):
self.sla.state = SpeedLimitAssistState.adapting
self.sla.v_cruise_cluster_prev = PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric]
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.active
def test_manual_cruise_change_detection(self):
@@ -144,7 +144,7 @@ class TestSpeedLimitAssist:
self.sla.v_cruise_cluster_prev = expected_cruise
different_cruise = SPEED_LIMITS['highway'] + 5
self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, different_cruise, SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.inactive
def test_rapid_speed_limit_changes(self):
@@ -152,26 +152,24 @@ class TestSpeedLimitAssist:
speed_limits = [SPEED_LIMITS['city'], SPEED_LIMITS['highway'], SPEED_LIMITS['residential']]
for _, speed_limit in enumerate(speed_limits):
self.sla.update(True, False, speed_limit, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], speed_limit, 0, 0, self.events_sp)
self.sla.update(True, False, speed_limit, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], speed_limit, speed_limit, True, 0, self.events_sp)
assert self.sla.state in ACTIVE_STATES
def test_invalid_speed_limits_handling(self):
self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric])
self.sla.last_valid_speed_limit_final = SPEED_LIMITS['city']
invalid_limits = [-10, 0, 200 * CV.MPH_TO_MS]
for invalid_limit in invalid_limits:
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], invalid_limit, 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], invalid_limit, SPEED_LIMITS['city'], True, 0, self.events_sp)
assert isinstance(self.sla.output_v_target, (int, float))
assert self.sla.output_v_target == V_CRUISE_UNSET or self.sla.output_v_target > 0
def test_stale_data_handling(self):
self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric])
old_speed_limit = SPEED_LIMITS['city']
self.sla.last_valid_speed_limit_final = old_speed_limit
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], 0, old_speed_limit, False, 0, self.events_sp)
assert self.sla.state in ACTIVE_STATES
assert self.sla.output_v_target == old_speed_limit
@@ -183,7 +181,7 @@ class TestSpeedLimitAssist:
current_speed = SPEED_LIMITS['highway']
target_speed = SPEED_LIMITS['city']
self.sla.update(True, False, current_speed, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], target_speed, 0, distance, self.events_sp)
self.sla.update(True, False, current_speed, 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], target_speed, target_speed, True, distance, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.adapting
assert self.sla.output_v_target == target_speed # TODO-SP: assert expected accel, need to enable self.acceleration_solutions
@@ -191,7 +189,7 @@ class TestSpeedLimitAssist:
self.initialize_active_state(PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric])
self.sla.update(False, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'],
0, 0, self.events_sp)
SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state == SpeedLimitAssistState.disabled
assert self.sla.output_v_target == V_CRUISE_UNSET
@@ -210,7 +208,7 @@ class TestSpeedLimitAssist:
initial_state = state
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric],SPEED_LIMITS['city'], 0, 0, self.events_sp)
self.sla.update(True, False, SPEED_LIMITS['city'], 0, PCM_LONG_REQUIRED_MAX_SET_SPEED[self.sla.is_metric], SPEED_LIMITS['city'], SPEED_LIMITS['city'], True, 0, self.events_sp)
assert self.sla.state in ALL_STATES # Sanity check