From ef956fb54e731a34bace06d6fcbd1673f325941c Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Tue, 18 Aug 2026 03:28:48 -0400 Subject: [PATCH] jitterbug --- selfdrive/ui/onroad/starpilot/aethergauge.py | 49 +++++++++--- .../ui/onroad/starpilot/stopping_point.py | 9 +-- .../onroad/starpilot/widgets/aethergauge.py | 2 +- selfdrive/ui/tests/test_aethergauge.py | 76 +++++++++++++++++++ 4 files changed, 120 insertions(+), 16 deletions(-) diff --git a/selfdrive/ui/onroad/starpilot/aethergauge.py b/selfdrive/ui/onroad/starpilot/aethergauge.py index 5c4f7a912..98507293f 100644 --- a/selfdrive/ui/onroad/starpilot/aethergauge.py +++ b/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) diff --git a/selfdrive/ui/onroad/starpilot/stopping_point.py b/selfdrive/ui/onroad/starpilot/stopping_point.py index 4eb067bc6..f611b8a91 100644 --- a/selfdrive/ui/onroad/starpilot/stopping_point.py +++ b/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 diff --git a/selfdrive/ui/onroad/starpilot/widgets/aethergauge.py b/selfdrive/ui/onroad/starpilot/widgets/aethergauge.py index 496727564..90f338986 100644 --- a/selfdrive/ui/onroad/starpilot/widgets/aethergauge.py +++ b/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. diff --git a/selfdrive/ui/tests/test_aethergauge.py b/selfdrive/ui/tests/test_aethergauge.py index cef3ad1a9..5b0389b0a 100644 --- a/selfdrive/ui/tests/test_aethergauge.py +++ b/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