diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 07a517423b..319f3e8224 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -1,4 +1,4 @@ -from datetime import datetime, timezone +from datetime import UTC, datetime, timezone from types import SimpleNamespace import pytest @@ -68,10 +68,10 @@ def make_toggles(**overrides): return SimpleNamespace(**defaults) -def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False, long_active=True, v_cruise_kph=255.0): +def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False, long_active=True, standstill=False, v_cruise_kph=255.0): return { "carControl": SimpleNamespace(longActive=long_active), - "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, vCruise=v_cruise_kph), + "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, standstill=standstill, vCruise=v_cruise_kph), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), "mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0, roadName=""), "selfdriveState": SimpleNamespace(enabled=enabled), @@ -318,6 +318,25 @@ def test_unset_cruise_applies_vehicle_speed_large_delta_guard(): controller.shutdown() +def test_standstill_ignores_vehicle_speed_jitter_for_vision_limit_guard(): + controller = make_controller( + speed_limit_priority1="Vision", + vision_speed_limit_detection=True, + ) + try: + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(35)) + controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(35)) + controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) + sm = make_sm(gas_pressed=False, standstill=True) + + for v_ego in (-0.0067, 0.0005): + controller.update_limits(0.0, datetime.now(UTC), False, mph(35), v_ego, sm) + assert controller.target == pytest.approx(mph(35)) + assert controller.source == "Vision" + finally: + controller.shutdown() + + def test_display_only_applies_large_delta_guard(): controller = make_controller( speed_limit_priority1="Vision", diff --git a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py index 34a3c404f4..4b7174dad5 100644 --- a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py +++ b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py @@ -63,7 +63,6 @@ VISION_SPEED_LIMIT_CHANGE_THRESHOLD = 0.1 # m/s # is a procedural API consumed once per frame by StarPilotOnroadView. _pulse = { - "active": False, # is the active source currently "Vision"? "last": 0.0, # last resolved vision limit (m/s) seen while active "start": -VISION_SPEED_LIMIT_PULSE_SECONDS, # get_time() stamp of the last change } @@ -71,7 +70,6 @@ _pulse = { def _reset_pulse() -> None: """Clear pulse state when SLC goes hidden or stale.""" - _pulse["active"] = False _pulse["last"] = 0.0 _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS @@ -79,15 +77,17 @@ def _reset_pulse() -> None: def _tick_pulse(source: str, resolved_ms: float) -> None: """Update pulse state once per frame from the resolved speed limit. - The pulse fires when the active source is "Vision" and either the source - just became active or the resolved value changed by at least - VISION_SPEED_LIMIT_CHANGE_THRESHOLD (m/s). + The pulse fires when the active source is "Vision" and its resolved value + changed by at least VISION_SPEED_LIMIT_CHANGE_THRESHOLD (m/s). """ vision_active = source == "Vision" and resolved_ms > 0.0 - if vision_active and (not _pulse["active"] or abs(resolved_ms - _pulse["last"]) >= VISION_SPEED_LIMIT_CHANGE_THRESHOLD): + if not vision_active: + _pulse["start"] = -VISION_SPEED_LIMIT_PULSE_SECONDS + return + + if abs(resolved_ms - _pulse["last"]) >= VISION_SPEED_LIMIT_CHANGE_THRESHOLD: _pulse["start"] = rl.get_time() - _pulse["active"] = vision_active - _pulse["last"] = resolved_ms if vision_active else 0.0 + _pulse["last"] = resolved_ms def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color: diff --git a/selfdrive/ui/tests/test_slc_sources_bubble.py b/selfdrive/ui/tests/test_slc_sources_bubble.py index f4c441a73e..fc129760bf 100644 --- a/selfdrive/ui/tests/test_slc_sources_bubble.py +++ b/selfdrive/ui/tests/test_slc_sources_bubble.py @@ -113,3 +113,31 @@ def test_source_label_color_override_and_engagement_states(): assert (color_ui_override.r, color_ui_override.g, color_ui_override.b, color_ui_override.a) == ( COLORS.OVERRIDE.r, COLORS.OVERRIDE.g, COLORS.OVERRIDE.b, 255 ) + + +def test_vision_pulse_ignores_same_limit_source_flapping(monkeypatch): + from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit as slc + + def rgba(color): + return color.r, color.g, color.b, color.a + + now = [0.0] + monkeypatch.setattr(slc.rl, "get_time", lambda: now[0]) + base = slc.rl.Color(255, 255, 255, 255) + + slc._reset_pulse() + slc._tick_pulse("Vision", 15.6464) + now[0] = 0.5 + assert rgba(slc._speed_limit_pulse_color(base, 255)) == (188, 132, 255, 255) + + slc._tick_pulse("Map Data", 15.6464) + assert rgba(slc._speed_limit_pulse_color(base, 255)) == (255, 255, 255, 255) + + now[0] = 0.6 + slc._tick_pulse("Vision", 15.6464) + assert rgba(slc._speed_limit_pulse_color(base, 255)) == (255, 255, 255, 255) + + now[0] = 0.7 + slc._tick_pulse("Vision", 13.4112) + now[0] = 1.2 + assert rgba(slc._speed_limit_pulse_color(base, 255)) == (188, 132, 255, 255) diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index aa647904bc..312fb3bff8 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -361,8 +361,9 @@ class SpeedLimitController: raw_set_speed_kph = float(sm["carState"].vCruise) selected_set_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0 reference_speed = selected_set_speed if selected_set_speed > 0 else max(float(v_ego), 0) + # vEgo jitters around zero at standstill; do not let that switch the active source. if ( - usable_vision_limit > 0 and reference_speed > 0 and + usable_vision_limit > 0 and reference_speed > 0 and not sm["carState"].standstill and abs(usable_vision_limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA ): support_count = self.starpilot_planner.params_memory.get_int("VisionSpeedLimitSupportCount")