From d9b97ffbaffd41307e7ca18a4e8fbeed66f8031f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 24 Sep 2025 22:54:32 -0400 Subject: [PATCH] use resolver --- .../controls/lib/longitudinal_planner.py | 5 +- .../lib/speed_limit/speed_limit_assist.py | 51 +++++++------------ .../tests/test_speed_limit_assist.py | 36 +++++++------ 3 files changed, 37 insertions(+), 55 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f963fe0eda..2b40fd523b 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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), diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py index e331e74ce5..18b01d8c51 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/speed_limit_assist.py @@ -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() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py index 770f8e6eaa..5dd62be878 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit/tests/test_speed_limit_assist.py @@ -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