jitterbug

selfdrive/ui/onroad/starpilot/aethergauge.py

@@ -221,10 +221,6 @@ def _is_stop_light() -> bool:
 
 def _stop_light_data() -> AetherGaugeData:
   dist_m = _get_val("starpilotPlan", "forcingStopLength", 0.0)
-  if dist_m == 0.0 and _sm_valid("modelV2"):
-    model = ui_state.sm["modelV2"]
-    if len(model.position.x) > 0:
-      dist_m = model.position.x[min(32, len(model.position.x) - 1)]
   display_dist, unit = _to_display_distance(dist_m)
   return AetherGaugeData(
     text=str(display_dist), unit=unit, color=COLOR_FORCE_STOP,
@@ -282,14 +278,17 @@ def _is_lead() -> bool:
           and _sm_valid("radarState")
           and ui_state.sm["radarState"].leadOne.status)
 
+_lead_is_stopped = False
+
 def _lead_data() -> AetherGaugeData:
+  global _lead_is_stopped
   lead = ui_state.sm["radarState"].leadOne
-  is_stopped = lead.vLead < LEAD_STOPPED_SPEED_THRESHOLD
+  _lead_is_stopped = lead.vLead < (1.2 if _lead_is_stopped else 0.8)
   return AetherGaugeData(
-    text="STOPPED" if is_stopped else "SLOW",
-    color=COLOR_LEAD_STOPPED if is_stopped else COLOR_LEAD_SLOWER,
+    text="STOPPED" if _lead_is_stopped else "SLOW",
+    color=COLOR_LEAD_STOPPED if _lead_is_stopped else COLOR_LEAD_SLOWER,
     indicator_type=IndicatorType.LEAD, indicator_value=lead.dRel,
-    indicator_extra="stopped" if is_stopped else "slower",
+    indicator_extra="stopped" if _lead_is_stopped else "slower",
   )
 
 
@@ -422,6 +421,8 @@ class AetherGauge:
     self._cooldown = 0.5
     self._road_h_filter = FirstOrderFilter(ROAD_HEIGHT, 0.06, 1 / gui_app.target_fps)
     self._current_road_h = ROAD_HEIGHT
+    self._dist_filter = FirstOrderFilter(0.0, 0.12, 1 / gui_app.target_fps)
+    self._last_indicator_type = IndicatorType.NONE
 
   def has_active_source(self) -> bool:
     """Lightweight visibility check — no side effects, no data construction."""
@@ -443,10 +444,13 @@ class AetherGauge:
 
     # Treat None as a priority 999 state: switch immediately if higher/equal priority,
     # or wait for cooldown to downgrade/hide.
-    if best_priority <= self._active_priority or (now - self._last_active_time > self._cooldown):
+    if new_data is not None:
       self._cached_data = new_data
       self._active_priority = best_priority
-      self._last_active_time = now if new_data is not None else 0.0
+      self._last_active_time = now
+    elif now - self._last_active_time > self._cooldown:
+      self._cached_data = None
+      self._active_priority = 999
 
     return self._cached_data
 
@@ -467,6 +471,31 @@ class AetherGauge:
       icx = cx
       icy = bottom - ROAD_HALF_SIZE
 
+    if data.indicator_type != self._last_indicator_type:
+      self._dist_filter.x = data.indicator_value
+      self._last_indicator_type = data.indicator_type
+
+    if data.indicator_type in (IndicatorType.FORCE_STOP, IndicatorType.STOP_LIGHT):
+      v_ego = _get_val("carState", "vEgo", 0.0)
+      raw_dist = data.indicator_value
+
+      # Moving forward at ANY speed (even a 0.1 mph creep): distance can ONLY count down
+      if v_ego > 0.05 and self._dist_filter.x > 0.0 and (raw_dist - self._dist_filter.x) < 20.0:
+        target_dist = min(self._dist_filter.x, raw_dist)
+      else:
+        target_dist = raw_dist
+
+      smoothed_dist = self._dist_filter.update(target_dist)
+
+      # Clean zero-lock at standstill or within final 1 foot (< 0.3m)
+      if _get_val("carState", "standstill", False) or smoothed_dist < 0.3:
+        smoothed_dist = 0.0
+        self._dist_filter.x = 0.0
+
+      display_dist, unit = _to_display_distance(smoothed_dist)
+      data.text = str(display_dist)
+      data.unit = unit
+
     if data.indicator_type in (IndicatorType.ROAD_CURVE, IndicatorType.FORCE_STOP, IndicatorType.LEAD, IndicatorType.STOP_LIGHT):
       self._render_unified_road(rect, icx, icy, data, font_bold, font_medium, alpha)
 

selfdrive/ui/onroad/starpilot/stopping_point.py

@@ -22,11 +22,10 @@ def render_stopping_point(renderer, font):
   if not plan or not plan.redLight:
     return
 
-  model = ui_state.sm["modelV2"] if ui_state.sm.valid.get("modelV2", False) else None
-  if not model or not len(model.position.x):
-    return
-
-  stopping_distance = model.position.x[min(32, len(model.position.x) - 1)]
+  # Get calibrated stopping distance from controls planner (aligned with Aethergauge)
+  stopping_distance = getattr(plan, "forcingStopLength", 0.0)
+  if ui_state.sm.valid.get("carState", False) and ui_state.sm["carState"].standstill:
+    stopping_distance = 0.0
 
   # Get the end of the projected path on the screen
   projected = renderer._path.projected_points

selfdrive/ui/onroad/starpilot/widgets/aethergauge.py

@@ -16,7 +16,7 @@ class AetherGaugeWidget(LayoutWidget):
 
   @property
   def is_visible(self) -> bool:
-    return self._aethergauge.has_active_source()
+    return self._aethergauge.has_active_source() or self._alpha_filter.x > 0.01
 
   def get_size(self) -> tuple[float, float]:
     # Match the left control width; height covers the road visual and text cradle.

selfdrive/ui/tests/test_aethergauge.py

@@ -231,3 +231,79 @@ def test_csc_source_precedes_cem_source(monkeypatch):
   assert data is not None
   assert data.indicator_type is IndicatorType.ROAD_CURVE
   assert data.indicator_value == pytest.approx(0.01)
+
+
+
+
+def test_priority_downgrade_updates_live_immediately(monkeypatch):
+  # Start at Priority 0 (Force Stop)
+  _set_plan(forcingStop=True, redLight=False, forcingStopLength=20.0)
+  mock_ui_state.sm.valid["carState"] = True
+  mock_ui_state.sm["carState"] = types.SimpleNamespace(gearShifter=1)
+  time_val = [1.0]
+  monkeypatch.setattr(aethergauge.rl, "get_time", lambda: time_val[0])
+
+  gauge = AetherGauge()
+  data0 = gauge.get_active_data()
+  assert data0.indicator_type is IndicatorType.FORCE_STOP
+  assert data0.indicator_value == 20.0
+
+  # Downgrade to Priority 1 (Stop Light)
+  time_val[0] = 1.05  # only 50ms later (well before 0.5s cooldown)
+  _set_plan(forcingStop=False, redLight=True, forcingStopLength=15.0)
+  data1 = gauge.get_active_data()
+  assert data1 is not None
+  assert data1.indicator_type is IndicatorType.STOP_LIGHT
+  assert data1.indicator_value == 15.0
+
+
+def test_lead_data_schmitt_trigger_hysteresis():
+  mock_ui_state.sm.valid["radarState"] = True
+
+  # 1. Start fast -> SLOW
+  mock_ui_state.sm["radarState"] = types.SimpleNamespace(
+    leadOne=types.SimpleNamespace(status=True, vLead=3.0, dRel=20.0)
+  )
+  data = _lead_data()
+  assert data.text == "SLOW"
+
+  # 2. Slow down to 0.9 m/s (below 1.0, but above 0.8 entry) -> should STAY SLOW
+  mock_ui_state.sm["radarState"].leadOne.vLead = 0.9
+  data = _lead_data()
+  assert data.text == "SLOW"
+
+  # 3. Slow down to 0.7 m/s (below 0.8 entry) -> transitions to STOPPED
+  mock_ui_state.sm["radarState"].leadOne.vLead = 0.7
+  data = _lead_data()
+  assert data.text == "STOPPED"
+
+  # 4. Accelerate to 1.1 m/s (above 1.0, but below 1.2 exit) -> should STAY STOPPED
+  mock_ui_state.sm["radarState"].leadOne.vLead = 1.1
+  data = _lead_data()
+  assert data.text == "STOPPED"
+
+  # 5. Accelerate to 1.3 m/s (above 1.2 exit) -> transitions to SLOW
+  mock_ui_state.sm["radarState"].leadOne.vLead = 1.3
+  data = _lead_data()
+  assert data.text == "SLOW"
+
+
+def test_monotonic_ratchet_clamp_prevents_upward_bounce(monkeypatch):
+  mock_ui_state.sm.valid["carState"] = True
+  mock_ui_state.sm["carState"] = types.SimpleNamespace(vEgo=5.0, standstill=False)
+  mock_ui_state.is_metric = True
+
+  rendered_data = []
+  gauge = AetherGauge()
+  monkeypatch.setattr(gauge, "_render_unified_road", lambda rect, cx, cy, data, fb, fm, alpha: rendered_data.append(data))
+
+  # Initial frame at 30m
+  _set_plan(redLight=True, forcingStopLength=30.0)
+  gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
+  assert int(rendered_data[-1].text) == 30
+
+  # Step closer: raw jumps UPWARD to 35m due to optical camera noise
+  _set_plan(redLight=True, forcingStopLength=35.0)
+  gauge.render(None, None, None, current_speed=10.0, cx=100.0, bottom=200.0)
+  # Ratchet clamp must prevent the display number from increasing above 30m!
+  assert int(rendered_data[-1].text) <= 30