diff --git a/selfdrive/clearpilot/speed_logic.py b/selfdrive/clearpilot/speed_logic.py index df7a3a8..b5a1d4a 100644 --- a/selfdrive/clearpilot/speed_logic.py +++ b/selfdrive/clearpilot/speed_logic.py @@ -24,6 +24,21 @@ class SpeedState: self.prev_warning = "" self.prev_warning_speed_limit = 0 + # Cache last-written param values — each put() is mkstemp+fsync+flock+rename. + # Sentinel None so the first call always writes. + self._w_has_speed = None + self._w_speed_display = None + self._w_speed_limit_display = None + self._w_speed_unit = None + self._w_is_metric = None + self._w_cruise_warning = None + self._w_cruise_warning_speed = None + + def _put_if_changed(self, key, value, attr): + if getattr(self, attr) != value: + self.params_memory.put(key, value) + setattr(self, attr, value) + def update(self, speed_ms: float, has_speed: bool, speed_limit_ms: float, is_metric: bool, cruise_speed_ms: float = 0.0, cruise_active: bool = False, cruise_standstill: bool = False): """ @@ -48,12 +63,12 @@ class SpeedState: self.prev_speed_limit = speed_limit_int - # Write display-ready values to params_memory - self.params_memory.put("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0") - self.params_memory.put("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "") - self.params_memory.put("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0") - self.params_memory.put("ClearpilotSpeedUnit", unit) - self.params_memory.put("ClearpilotIsMetric", "1" if is_metric else "0") + # Write display-ready values to params_memory (gated on change) + self._put_if_changed("ClearpilotHasSpeed", "1" if has_speed and speed_int > 0 else "0", "_w_has_speed") + self._put_if_changed("ClearpilotSpeedDisplay", str(speed_int) if has_speed and speed_int > 0 else "", "_w_speed_display") + self._put_if_changed("ClearpilotSpeedLimitDisplay", str(speed_limit_int) if speed_limit_int > 0 else "0", "_w_speed_limit_display") + self._put_if_changed("ClearpilotSpeedUnit", unit, "_w_speed_unit") + self._put_if_changed("ClearpilotIsMetric", "1" if is_metric else "0", "_w_is_metric") # Cruise warning logic warning = "" @@ -61,7 +76,16 @@ class SpeedState: cruise_engaged = cruise_active and not cruise_standstill if speed_limit_int >= 20 and cruise_engaged and cruise_int > 0: - over_threshold = 10 if speed_limit_int >= 50 else 7 + # Tiers (warning fires at >= limit + threshold): + # limit >= 50: +9 over ok, warn at +10 (e.g. 60 → warn at 70) + # limit 26-49: +6 over ok, warn at +7 (e.g. 35 → warn at 42) + # limit <= 25: +8 over ok, warn at +9 (e.g. 25 → warn at 34, so 33 is ok) + if speed_limit_int >= 50: + over_threshold = 10 + elif speed_limit_int <= 25: + over_threshold = 9 + else: + over_threshold = 7 if cruise_int >= speed_limit_int + over_threshold: warning = "over" warning_speed = str(cruise_int) @@ -69,8 +93,8 @@ class SpeedState: warning = "under" warning_speed = str(cruise_int) - self.params_memory.put("ClearpilotCruiseWarning", warning) - self.params_memory.put("ClearpilotCruiseWarningSpeed", warning_speed) + self._put_if_changed("ClearpilotCruiseWarning", warning, "_w_cruise_warning") + self._put_if_changed("ClearpilotCruiseWarningSpeed", warning_speed, "_w_cruise_warning_speed") # Ding logic: play when warning sign appears or speed limit changes while visible should_ding = False