diff --git a/selfdrive/ui/mici/onroad/speed_limit_utils.py b/selfdrive/ui/mici/onroad/speed_limit_utils.py index 77786564c..ffc4de090 100644 --- a/selfdrive/ui/mici/onroad/speed_limit_utils.py +++ b/selfdrive/ui/mici/onroad/speed_limit_utils.py @@ -3,10 +3,22 @@ from __future__ import annotations from collections.abc import Mapping +SOURCE_PRIORITY_MODES = {"Highest", "Lowest"} +NO_SOURCE = {"", "None"} + + def _normalize_limit(limit: float) -> float: return limit if limit >= 1.0 else 0.0 +def _source_allowed(source: str, primary_priority: str, secondary_priority: str) -> bool: + if source in NO_SOURCE: + return False + if primary_priority in SOURCE_PRIORITY_MODES: + return True + return source in (primary_priority, secondary_priority, "Mapbox") + + def resolve_display_speed_limit_ms( slc_speed_limit: float, speed_limit_source: str, @@ -15,13 +27,13 @@ def resolve_display_speed_limit_ms( secondary_priority: str, ) -> float: slc_speed_limit = _normalize_limit(slc_speed_limit) - if slc_speed_limit > 0.0: + if slc_speed_limit > 0.0 and _source_allowed(speed_limit_source, primary_priority, secondary_priority): return slc_speed_limit normalized_limits = {source: _normalize_limit(limit) for source, limit in source_limits.items()} active_source_limit = normalized_limits.get(speed_limit_source, 0.0) - if active_source_limit > 0.0: + if active_source_limit > 0.0 and _source_allowed(speed_limit_source, primary_priority, secondary_priority): return active_source_limit available_limits = {source: limit for source, limit in normalized_limits.items() if limit > 0.0} @@ -38,4 +50,4 @@ def resolve_display_speed_limit_ms( if limit > 0.0: return limit - return next(iter(available_limits.values())) + return 0.0 diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 5c26333e6..a5d9bbc72 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -27,7 +27,6 @@ HISTORY_SECONDS = 2.0 CONSISTENT_DETECTIONS = 2 CHANGE_CONSISTENT_DETECTIONS = 3 MODEL_DETECTION_SHORT_CIRCUIT_CONFIDENCE = 0.65 -PUBLISHED_HOLD_SECONDS = 12.0 PUBLISHED_CHANGE_COOLDOWN_SECONDS = 1.4 PUBLISHED_REVERT_CONFIDENCE = 0.97 AUTO_BOOKMARK_CONFIRM_DELAY_SECONDS = 0.9 @@ -668,9 +667,6 @@ class SpeedLimitVisionDaemon: self.last_map_transition_miss_speed_limit_mph = current_speed_limit_mph self._record_map_transition_miss(current_speed_limit_mph, previous_speed_limit_mph) - def _published_detection_stale(self, now): - return self.published_speed_limit_mph > 0 and now - self.last_detection_at > PUBLISHED_HOLD_SECONDS - def _load_model(self): self.net = None self.classifier_net = None @@ -1753,12 +1749,6 @@ class SpeedLimitVisionDaemon: if not self._connect_camera(): status = "Waiting for camera stream" if self.published_speed_limit_mph > 0: - now = time.monotonic() - if self._published_detection_stale(now): - self._write_debug_event("stale_clear", reason="stream_unavailable") - self._publish_status("Waiting for camera stream", clear_speed=True) - ratekeeper.keep_time() - continue status = f"{status}, holding {self.published_speed_limit_mph} mph" self._publish_status(status, clear_speed=False) ratekeeper.keep_time() @@ -1768,11 +1758,7 @@ class SpeedLimitVisionDaemon: inference_interval = FOLLOWUP_INFERENCE_INTERVAL if now < self.followup_until else INFERENCE_INTERVAL if now - self.last_inference_at < inference_interval: if self.published_speed_limit_mph > 0: - if self._published_detection_stale(now): - self._write_debug_event("stale_clear", reason="hold_timeout") - self._publish_status(f"Scanning {self.stream_name}", clear_speed=True) - else: - self._publish_detection(self.published_speed_limit_mph, self.published_confidence, "Holding") + self._publish_detection(self.published_speed_limit_mph, self.published_confidence, "Holding") else: self._publish_status(f"Scanning {self.stream_name}", clear_speed=False) ratekeeper.keep_time() @@ -1781,9 +1767,8 @@ class SpeedLimitVisionDaemon: buffer = self.client.recv() if self.client is not None else None self.last_inference_at = now if buffer is None or not buffer.data.any(): - if self._published_detection_stale(now): - self._write_debug_event("stale_clear", reason="empty_frame") - self._publish_status(f"Waiting for {self.stream_name}", clear_speed=True) + if self.published_speed_limit_mph > 0: + self._publish_status(f"Waiting for {self.stream_name}, holding {self.published_speed_limit_mph} mph", clear_speed=False) else: self._publish_status(f"Waiting for {self.stream_name}", clear_speed=False) ratekeeper.keep_time() @@ -1797,11 +1782,7 @@ class SpeedLimitVisionDaemon: if detection is not None: self._update_detection(detection) elif self.published_speed_limit_mph > 0: - if self._published_detection_stale(now): - self._write_debug_event("stale_clear", reason="no_detection") - self._publish_status(f"Scanning {self.stream_name}", clear_speed=True) - else: - self._publish_detection(self.published_speed_limit_mph, self.published_confidence, "Holding") + self._publish_detection(self.published_speed_limit_mph, self.published_confidence, "Holding") else: self._publish_status(f"Scanning {self.stream_name}", clear_speed=False)