Purple RainX

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",

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:

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)

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")