CurvatureD Optimization (#12)

* performance optimization

* performance optimization

* optimization

* quality

* safety and params optimization

* optimization

selfdrive/controls/lib/curvatured.py

@@ -2,9 +2,28 @@ import numpy as np
 
 from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION
 
+# Cache granularity for get_correction(). Inputs are rounded before comparison so that
+# sensor noise does not invalidate the cache between consecutive 100Hz calls.
+#
+# Rationale per input:
+#   CACHE_V_EGO_DECIMALS = 1   (0.1 m/s granularity)
+#     - carState.vEgo sensor noise is typically ~0.001 m/s, but during normal
+#       driving v_ego rarely changes faster than 0.5 m/s per frame.
+#     - 0.1 m/s is loose enough to absorb realistic sensor jitter but still
+#       sensitive to actual acceleration/deceleration events.
+#   CACHE_CURVATURE_DECIMALS = 7   (1e-7 granularity)
+#     - controlsState.modelDesiredCurvature model output has ~1e-6 noise.
+#     - 1e-7 is below the model's noise floor so it rounds to the same value
+#       across consecutive frames; coarser would miss genuine steering changes.
+# Both values are well below steering precision (1 deg of steering ≈ 1e-3 curvature).
+CACHE_V_EGO_DECIMALS = 1        # 0.1 m/s
+CACHE_CURVATURE_DECIMALS = 7    # 1e-7
+
 
 class CurvatureDController(CurvatureDLookup):
   def __init__(self) -> None:
+    # Cache total_size once; the bucket shape is fixed for the process lifetime.
+    self._expected_size: int = self.total_size()
     self.reset()
 
   def reset(self) -> None:
@@ -12,38 +31,63 @@ class CurvatureDController(CurvatureDLookup):
     self.live_valid = False
     self.fit_corrections = np.zeros(self.bucket_shape(), dtype=np.float32)
     self.fit_valid = np.zeros(self.bucket_shape(), dtype=bool)
+    self._cached_v_ego_q: float | None = None
+    self._cached_curvature_q: float | None = None
+    self._cached_projected: float = 0.0
 
   def update_live_params(self, msg) -> None:
-    expected_size = self.total_size()
-    if msg.version != VERSION or len(msg.corrections) != expected_size:
+    if msg.version != VERSION or len(msg.corrections) != self._expected_size:
       self.reset()
       return
 
-    self.use_params = bool(msg.useParams)
-    self.live_valid = bool(msg.liveValid)
-    self.fit_corrections = self.unflatten_bucket(msg.corrections, dtype=np.float32)
-
-    if len(msg.fitValid) == expected_size:
-      self.fit_valid = self.unflatten_bucket(msg.fitValid, dtype=bool)
+    # Build all new state from the message first. Then invalidate the cache
+    # BEFORE swapping fields, so any concurrent reader sees either the old
+    # state with a fresh cache miss (forcing a recompute) or the new state
+    # (with cache already empty). The opposite order could briefly leave a
+    # reader with the new state but a stale cache hit.
+    new_use_params = bool(msg.useParams)
+    new_live_valid = bool(msg.liveValid)
+    new_fit_corrections = self.unflatten_bucket(msg.corrections, dtype=np.float32)
+    if len(msg.fitValid) == self._expected_size:
+      new_fit_valid = self.unflatten_bucket(msg.fitValid, dtype=bool)
     else:
-      self.fit_valid = np.abs(self.fit_corrections) > 0.0
+      new_fit_valid = np.abs(new_fit_corrections) > 0.0
 
-    if not self.live_valid:
+    if not new_live_valid:
       self.reset()
+      return
+
+    # Invalidate-first, then coherent field swap. The tuple assignment makes
+    # the cache reset a single bytecode operation, so no reader can observe
+    # a half-zeroed cache.
+    self._invalidate_correction_cache()
+    self.use_params = new_use_params
+    self.live_valid = new_live_valid
+    self.fit_corrections = new_fit_corrections
+    self.fit_valid = new_fit_valid
+
+  def _invalidate_correction_cache(self) -> None:
+    # Tuple assignment is a single bytecode op in CPython, so this is
+    # effectively atomic with respect to readers (no half-zeroed state).
+    self._cached_v_ego_q, self._cached_curvature_q, self._cached_projected = None, None, 0.0
 
   def get_correction(self, desired_curvature: float, v_ego: float) -> float:
     if not self.use_params or not self.live_valid:
       return 0.0
 
     abs_curvature = abs(float(desired_curvature))
-    if abs_curvature < self.CURVATURE_MIN or abs_curvature > self.CURVATURE_MAX:
-      return 0.0
-    if abs_curvature * (float(v_ego) ** 2) > self.MAX_LAT_ACCEL_APPLY:
+    if self._exceeds_safety_bounds(abs_curvature, v_ego):
       return 0.0
 
-    projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs_curvature)
+    v_ego_q = round(v_ego, CACHE_V_EGO_DECIMALS)
+    curvature_q = round(abs_curvature, CACHE_CURVATURE_DECIMALS)
+    if v_ego_q == self._cached_v_ego_q and curvature_q == self._cached_curvature_q:
+      projected = self._cached_projected
+    else:
+      projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs_curvature)
+      self._cached_v_ego_q = v_ego_q
+      self._cached_curvature_q = curvature_q
+      self._cached_projected = projected
+
     direction = 1.0 if desired_curvature >= 0.0 else -1.0
     return float(direction * projected)
-
-  def apply(self, desired_curvature: float, v_ego: float) -> float:
-    return float(desired_curvature + self.get_correction(desired_curvature, v_ego))

selfdrive/controls/tests/test_curvatured.py

@@ -1,6 +1,6 @@
 import cereal.messaging as messaging
 
-from openpilot.selfdrive.controls.lib.curvatured import CurvatureDController
+from openpilot.selfdrive.controls.lib.curvatured import CACHE_CURVATURE_DECIMALS, CACHE_V_EGO_DECIMALS, CurvatureDController
 from openpilot.selfdrive.locationd.curvatured import CurvatureDLookup, VERSION
 
 
@@ -72,7 +72,10 @@ class TestCurvatureDController:
     assert neg < 0.0
     assert abs(pos + neg) < 1e-12
 
-  def test_invalid_message_disables_corrections(self):
+  def test_live_valid_false_disables_corrections(self):
+    """When the message's liveValid flag is False, get_correction must return 0.0
+    regardless of any cached state, since the upstream signal is invalid.
+    """
     controller = CurvatureDController()
     msg = messaging.new_message('liveCurvatureParameters')
     msg.liveCurvatureParameters.liveValid = False
@@ -85,7 +88,55 @@ class TestCurvatureDController:
 
     controller.update_live_params(msg.liveCurvatureParameters)
 
-    assert controller.apply(32e-6, 20.0) == 32e-6
+    # Without a learnable curve and no live_valid, get_correction returns 0.0.
+    assert controller.get_correction(32e-6, 20.0) == 0.0
+
+  def test_version_mismatch_resets_controller(self):
+    """A message with the wrong version number must trigger a full reset, not
+    a partial state update. This is the actual 'invalid message' path.
+    """
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION + 99  # intentionally wrong
+    msg.liveCurvatureParameters.useParams = True
+    msg.liveCurvatureParameters.corrections = [0.0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.fitValid = [False] * CurvatureDLookup.total_size()
+
+    # Pretend we had a valid state before the bad message
+    controller.use_params = True
+    controller.live_valid = True
+
+    controller.update_live_params(msg.liveCurvatureParameters)
+
+    # Bad version -> full reset -> both flags back to False
+    assert not controller.use_params
+    assert not controller.live_valid
+    assert controller.get_correction(32e-6, 20.0) == 0.0
+
+  def test_size_mismatch_resets_controller(self):
+    """A message with the wrong corrections array size must also trigger a full reset."""
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION
+    msg.liveCurvatureParameters.useParams = True
+    # Wrong size (1 element instead of total_size())
+    msg.liveCurvatureParameters.corrections = [0.0]
+    msg.liveCurvatureParameters.counts = [0]
+    msg.liveCurvatureParameters.biases = [0.0]
+    msg.liveCurvatureParameters.fitValid = [False]
+
+    controller.use_params = True
+    controller.live_valid = True
+
+    controller.update_live_params(msg.liveCurvatureParameters)
+
+    assert not controller.use_params
+    assert not controller.live_valid
+    assert controller.get_correction(32e-6, 20.0) == 0.0
 
   def test_correction_fades_outside_supported_curvature_range(self):
     controller = CurvatureDController()
@@ -155,3 +206,136 @@ class TestCurvatureDController:
     assert at_last_edge > 0.0
     assert 0.0 < in_fade < at_last_edge
     assert at_max == 0.0
+
+  def test_get_correction_caches_within_quantization_window(self):
+    """Identical (v_ego, abs_curvature) within quantization granularity must
+    hit the cache. The interp_curve_value source must not be called twice.
+    """
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION
+    msg.liveCurvatureParameters.useParams = True
+    msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
+
+    curvature_idx = CurvatureDLookup.curvature_index(32e-6)
+    assert curvature_idx is not None
+    self._set_curve(msg, 3, {curvature_idx: 8e-6})
+    controller.update_live_params(msg.liveCurvatureParameters)
+
+    v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
+
+    # Wrap the source to count calls
+    call_count = {"n": 0}
+    original = CurvatureDLookup.interp_curve_value
+    def counting(*args, **kwargs):
+      call_count["n"] += 1
+      return original(*args, **kwargs)
+    CurvatureDLookup.interp_curve_value = counting
+    try:
+      # First call: cache miss, calls interp_curve_value once
+      first = controller.get_correction(32e-6, v_ego)
+      assert call_count["n"] == 1
+      # Subsequent identical calls: cache hit, no further invocations
+      for _ in range(5):
+        cached = controller.get_correction(32e-6, v_ego)
+        assert cached == first
+      assert call_count["n"] == 1
+
+      # v_ego noise below quantization must still hit the cache
+      v_ego_step = 10 ** -CACHE_V_EGO_DECIMALS
+      noised = controller.get_correction(32e-6, v_ego + v_ego_step * 0.5)
+      assert noised == first
+      assert call_count["n"] == 1
+
+      # Curvature noise below quantization must still hit the cache
+      curvature_step = 10 ** -CACHE_CURVATURE_DECIMALS
+      noised = controller.get_correction(32e-6 + curvature_step * 0.5, v_ego)
+      assert noised == first
+      assert call_count["n"] == 1
+    finally:
+      CurvatureDLookup.interp_curve_value = original
+
+  def test_get_correction_cache_invalidates_on_live_params_update(self):
+    """Cache must be invalidated when fit_corrections / fit_valid change,
+    otherwise stale corrections would be served after a params update.
+    """
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION
+    msg.liveCurvatureParameters.useParams = True
+    msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
+
+    curvature_idx = CurvatureDLookup.curvature_index(32e-6)
+    assert curvature_idx is not None
+    self._set_curve(msg, 3, {curvature_idx: 4e-6})
+    controller.update_live_params(msg.liveCurvatureParameters)
+    v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
+
+    first = controller.get_correction(32e-6, v_ego)
+
+    # Update the underlying curve to a different value
+    self._set_curve(msg, 3, {curvature_idx: 16e-6})
+    controller.update_live_params(msg.liveCurvatureParameters)
+
+    second = controller.get_correction(32e-6, v_ego)
+    assert second > first
+    # Specifically: must not be the cached value
+    assert second != first
+
+  def test_get_correction_cache_invalidates_on_reset(self):
+    """reset() must clear the cache to avoid stale hits after disengage/engage."""
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION
+    msg.liveCurvatureParameters.useParams = True
+    msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
+
+    curvature_idx = CurvatureDLookup.curvature_index(32e-6)
+    assert curvature_idx is not None
+    self._set_curve(msg, 3, {curvature_idx: 8e-6})
+    controller.update_live_params(msg.liveCurvatureParameters)
+    v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
+
+    # Warm the cache
+    controller.get_correction(32e-6, v_ego)
+    assert controller._cached_v_ego_q is not None
+
+    controller.reset()
+    # Cache state must be reset
+    assert controller._cached_v_ego_q is None
+    assert controller._cached_curvature_q is None
+    assert controller._cached_projected == 0.0
+
+  def test_get_correction_bypasses_cache_when_params_disabled(self):
+    """When use_params is False, the early-return at the top of get_correction
+    must not interfere with cache invariants (the cache may legitimately be stale).
+    """
+    controller = CurvatureDController()
+    msg = messaging.new_message('liveCurvatureParameters')
+    msg.liveCurvatureParameters.liveValid = True
+    msg.liveCurvatureParameters.version = VERSION
+    msg.liveCurvatureParameters.useParams = True
+    msg.liveCurvatureParameters.counts = [0] * CurvatureDLookup.total_size()
+    msg.liveCurvatureParameters.biases = [0.0] * CurvatureDLookup.total_size()
+
+    curvature_idx = CurvatureDLookup.curvature_index(32e-6)
+    assert curvature_idx is not None
+    self._set_curve(msg, 3, {curvature_idx: 8e-6})
+    controller.update_live_params(msg.liveCurvatureParameters)
+    v_ego = float(CurvatureDLookup.SPEED_ANCHORS[3])
+
+    # Warm the cache
+    controller.get_correction(32e-6, v_ego)
+    assert controller._cached_projected != 0.0
+
+    # Disable params -> early return 0.0
+    controller.use_params = False
+    assert controller.get_correction(32e-6, v_ego) == 0.0
+    # The cache still holds the old value but is not consulted on this path
+    assert controller._cached_projected != 0.0  # cache not touched

selfdrive/locationd/curvatured.py

@@ -1,4 +1,3 @@
-import math
 import time
 from collections import deque
 
@@ -69,6 +68,10 @@ class CurvatureDLookup:
   LAST_BUCKET_WIDTH = float(CURVATURE_BUCKET_EDGES[-1] - CURVATURE_BUCKET_EDGES[-2])
   CURVATURE_MAX = CURVATURE_BUCKET_MAX + LAST_BUCKET_WIDTH
 
+  # Precomputed log of bucket centers for sample interpolation. Avoids recomputing
+  # np.log on every interp_curve_value / _interp_curve_impl call.
+  _LOG_CENTERS = np.log(CURVATURE_BUCKET_CENTERS.astype(np.float64))
+
   MIN_SPEED = float(SPEED_ANCHORS[0] * 0.5)  # learning/apply speed floor
   MAX_LAT_ACCEL_APPLY = 1.0  # apply accel gate
   RELATIVE_CAP_FULL_RATIO = 0.50  # inner relative cap
@@ -265,8 +268,13 @@ class CurvatureDLookup:
     return int(round(100.0 * fully_calibrated_speeds / float(len(cls.SPEED_ANCHORS))))
 
   @classmethod
-  def smoothstep(cls, x: float) -> float:
-    y = float(np.clip(x, 0.0, 1.0))
+  def smoothstep(cls, x) -> np.ndarray:
+    """Smoothstep, vectorized. Accepts scalar or array input.
+
+    Note: the original scalar-only form is preserved implicitly via np.clip
+    on the input - it just no longer raises on arrays.
+    """
+    y = np.clip(x, 0.0, 1.0)
     return y * y * (3.0 - 2.0 * y)
 
   @classmethod
@@ -375,10 +383,52 @@ class CurvatureDLookup:
 
   @classmethod
   def interp_curve_value(cls, fit_corrections: np.ndarray, fit_valid: np.ndarray,
-                         v_ego: float, abs_curvature: float) -> float:
-    if abs_curvature < cls.CURVATURE_MIN or abs_curvature > cls.CURVATURE_MAX:
+                         v_ego: float, abs_curvature: float | np.ndarray) -> float | np.ndarray:
+    """Interpolate curvature correction(s) from the learned fit curves.
+
+    Single Public-API that accepts both scalar and array input, dispatching
+    internally to the vectorized implementation:
+
+    - Scalar input (float)  -> returns float (100Hz controlsd hot path)
+    - Array input (np.ndarray) -> returns np.ndarray (UI rendering, batched)
+
+    Caching of common constants (_LOG_CENTERS) and a single speed_interp() call
+    per batch keep both paths fast.
+    """
+    abs_curvatures = np.asarray(abs_curvature, dtype=np.float64)
+    was_scalar = abs_curvatures.ndim == 0
+    if was_scalar:
+      abs_curvatures = abs_curvatures.reshape(1)
+    if was_scalar and cls._exceeds_safety_bounds(abs_curvatures[0], float(v_ego)):
       return 0.0
 
+    result = cls._interp_curve_impl(fit_corrections, fit_valid, v_ego, abs_curvatures)
+    return float(result[0]) if was_scalar else result
+
+  @classmethod
+  def _exceeds_safety_bounds(cls, abs_curvature: float, v_ego: float) -> bool:
+    """Single source of truth for the physical safety bounds.
+
+    Returns True if the requested curvature should not be applied because
+    it would exceed the per-vehicle lateral acceleration limit or is
+    outside the learned bucket range.
+    """
+    if abs_curvature < cls.CURVATURE_MIN or abs_curvature > cls.CURVATURE_MAX:
+      return True
+    if abs_curvature * (float(v_ego) ** 2) > cls.MAX_LAT_ACCEL_APPLY:
+      return True
+    return False
+
+  @classmethod
+  def _interp_curve_impl(cls, fit_corrections: np.ndarray, fit_valid: np.ndarray,
+                         v_ego: float, abs_curvatures: np.ndarray) -> np.ndarray:
+    """Vectorized core. Always receives a 1-D abs_curvatures array of len >= 1."""
+    n = len(abs_curvatures)
+    out = np.zeros(n, dtype=np.float32)
+
+    log_curvatures = np.log(np.maximum(abs_curvatures, cls.CURVATURE_BUCKET_MIN))
+
+    # Compute speed interpolation indices once (avoid per-sample calls)
     low_speed, high_speed, speed_alpha = cls.speed_interp(v_ego)
     low_curve = fit_corrections[low_speed]
     high_curve = fit_corrections[high_speed]
@@ -386,55 +436,63 @@ class CurvatureDLookup:
     high_valid = fit_valid[high_speed]
 
     if not low_valid.any() and not high_valid.any():
-      return 0.0
+      return out
 
-    log_centers = np.log(cls.CURVATURE_BUCKET_CENTERS.astype(np.float64))
-    log_curvature = math.log(max(abs_curvature, cls.CURVATURE_BUCKET_MIN))
+    log_centers = cls._LOG_CENTERS
+    curvature_edges = cls.CURVATURE_BUCKET_EDGES
+    curvature_max = cls.CURVATURE_MAX
+    curvature_min = cls.CURVATURE_MIN
+    n_centers = len(cls.CURVATURE_BUCKET_CENTERS)
 
-    def curve_value(curve: np.ndarray, valid_mask: np.ndarray) -> float:
+    def interp_speed_row(curve: np.ndarray, valid_mask: np.ndarray) -> np.ndarray:
+      row_out = np.zeros(n, dtype=np.float32)
       runs = cls.valid_runs(valid_mask)
       if len(runs) == 0:
-        return 0.0
+        return row_out
 
       for start, end in runs:
         run_idx = np.arange(start, end + 1)
         run_log_x = log_centers[run_idx]
         run_curve = curve[run_idx]
-        first_edge = cls.CURVATURE_BUCKET_EDGES[start]
-        last_edge = cls.CURVATURE_BUCKET_EDGES[end + 1]
+        first_edge = float(curvature_edges[start])
+        last_edge = float(curvature_edges[end + 1])
 
-        if first_edge <= abs_curvature <= last_edge:
-          return float(np.interp(log_curvature, run_log_x, run_curve))
+        # Main interpolation: curvature within run range
+        in_range = (abs_curvatures >= first_edge) & (abs_curvatures <= last_edge)
+        if in_range.any():
+          row_out[in_range] = np.interp(log_curvatures[in_range], run_log_x, run_curve).astype(np.float32)
 
+        # Fade in (between previous bucket and first_edge)
         if start > 0:
-          fade_in_start = cls.CURVATURE_BUCKET_EDGES[start - 1]
-          if fade_in_start <= abs_curvature < first_edge:
-            fade_span = first_edge - fade_in_start
-            fade = cls.smoothstep((abs_curvature - fade_in_start) / max(fade_span, 1e-9))
-            return float(run_curve[0] * np.clip(fade, 0.0, 1.0))
-        elif cls.CURVATURE_MIN <= abs_curvature < first_edge:
-          fade_span = first_edge - cls.CURVATURE_MIN
-          fade = cls.smoothstep((abs_curvature - cls.CURVATURE_MIN) / max(fade_span, 1e-9))
-          return float(run_curve[0] * np.clip(fade, 0.0, 1.0))
+          fade_in_start = float(curvature_edges[start - 1])
+          fade_in_mask = (abs_curvatures >= fade_in_start) & (abs_curvatures < first_edge)
+        else:
+          fade_in_start = curvature_min
+          fade_in_mask = (abs_curvatures >= curvature_min) & (abs_curvatures < first_edge)
+        if fade_in_mask.any():
+          fade_span = max(first_edge - fade_in_start, 1e-9)
+          fade_vals = cls.smoothstep((abs_curvatures[fade_in_mask] - fade_in_start) / fade_span)
+          row_out[fade_in_mask] = (run_curve[0] * fade_vals).astype(np.float32)
 
-        if end < len(cls.CURVATURE_BUCKET_CENTERS) - 1:
-          fade_out_end = cls.CURVATURE_BUCKET_EDGES[end + 2]
-          if last_edge < abs_curvature <= fade_out_end:
-            fade_span = fade_out_end - last_edge
-            fade = 1.0 - cls.smoothstep((abs_curvature - last_edge) / max(fade_span, 1e-9))
-            return float(run_curve[-1] * np.clip(fade, 0.0, 1.0))
-        elif last_edge < abs_curvature <= cls.CURVATURE_MAX:
-          fade_span = cls.CURVATURE_MAX - last_edge
-          fade = 1.0 - cls.smoothstep((abs_curvature - last_edge) / max(fade_span, 1e-9))
-          return float(run_curve[-1] * np.clip(fade, 0.0, 1.0))
+        # Fade out (between last_edge and next bucket)
+        if end < n_centers - 1:
+          fade_out_end = float(curvature_edges[end + 2])
+          fade_out_mask = (abs_curvatures > last_edge) & (abs_curvatures <= fade_out_end)
+        else:
+          fade_out_end = curvature_max
+          fade_out_mask = (abs_curvatures > last_edge) & (abs_curvatures <= fade_out_end)
+        if fade_out_mask.any():
+          fade_span = max(fade_out_end - last_edge, 1e-9)
+          fade_vals = 1.0 - cls.smoothstep((abs_curvatures[fade_out_mask] - last_edge) / fade_span)
+          row_out[fade_out_mask] = (run_curve[-1] * fade_vals).astype(np.float32)
 
-      return 0.0
+      return row_out
 
-    low_val = curve_value(low_curve, low_valid)
-    high_val = curve_value(high_curve, high_valid)
+    low_vals = interp_speed_row(low_curve, low_valid)
     if low_speed == high_speed:
-      return low_val
-    return float((1.0 - speed_alpha) * low_val + speed_alpha * high_val)
+      return low_vals
+    high_vals = interp_speed_row(high_curve, high_valid)
+    return ((1.0 - speed_alpha) * low_vals + speed_alpha * high_vals).astype(np.float32)
 
 
 class CurvatureEstimator(CurvatureDLookup):
@@ -756,9 +814,15 @@ class CurvatureEstimator(CurvatureDLookup):
     self.current_bias = float(self.bias[speed_idx, curvature_idx])
     self.current_bucket_points = self.bucket_points_for_index(self.counts, idx)
 
+    if not self.use_params:
+      self.current_correction = 0.0
+      return
     if not self.fit_valid[speed_idx].any():
       self.current_correction = 0.0
       return
+    if self._exceeds_safety_bounds(abs(float(desired_curvature)), float(v_ego)):
+      self.current_correction = 0.0
+      return
 
     direction = 1.0 if desired_curvature >= 0.0 else -1.0
     projected = self.interp_curve_value(self.fit_corrections, self.fit_valid, v_ego, abs(desired_curvature))
@@ -913,7 +977,8 @@ def main():
                                           include_debug=curvature_estimator.publish_debug_data,
                                           include_preview=curvature_estimator.publish_preview_data))
 
-    # Cache params every 60 seconds
+    # Persistence is non-blocking by default (Params.put with block=False).
+    # We do this every 60s and accept the rare latency if the disk stalls.
     if sm.frame % 240 == 0:
       live_valid = sm.all_checks() and curvature_estimator.use_params
       params.put("LiveCurvatureParameters", curvature_estimator.get_msg(valid=sm.all_checks(),

selfdrive/locationd/test/test_curvatured.py

@@ -341,3 +341,123 @@ class TestCurvatureEstimator:
 
     assert estimator.steering_pressed[-1]
     assert estimator.last_override_t == 12.0
+
+  def test_interp_curve_value_matches_interp_curve_samples(self):
+    """Verifies the unified interp_curve_value API returns the same result for
+    scalar and array input, and that both branches share the same code path.
+    This protects both UI and controlsd (100Hz) code paths from drift.
+    """
+    speed_idx = 3
+    v_ego = float(CurvatureDLookup.SPEED_ANCHORS[speed_idx])
+    fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
+    fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool)
+
+    # Two valid runs with a gap
+    fit_valid[speed_idx, 3] = True
+    fit_valid[speed_idx, 6] = True
+    fit_corrections[speed_idx, 3] = 1.0e-6
+    fit_corrections[speed_idx, 6] = 8.0e-6
+
+    # Test points covering: out of range, in valid run, in gap (returns 0), fade regions
+    test_curvatures = [
+      0.0,
+      1.0e-7,
+      float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[2]),  # before first valid run (fade-in)
+      float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[3]),  # in first run
+      float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[4]),  # in gap (should be 0)
+      float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[6]),  # in second run
+      float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[7]),  # after second run (fade-out)
+      float(CurvatureDLookup.CURVATURE_MAX + 1.0),  # out of range
+    ]
+
+    for c in test_curvatures:
+      scalar_result = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, c)
+      array_result = CurvatureDLookup.interp_curve_value(
+        fit_corrections, fit_valid, v_ego, np.asarray([c], dtype=np.float64)
+      )
+      # Scalar path returns float
+      assert isinstance(scalar_result, float)
+      # Array path returns np.ndarray
+      assert isinstance(array_result, np.ndarray)
+      assert np.isclose(scalar_result, array_result[0]), f"mismatch at c={c}: scalar={scalar_result}, array={array_result[0]}"
+
+  def test_interp_curve_value_handles_speed_interp_transition(self):
+    """When v_ego falls between two SPEED_ANCHORS, interp_curve_value must blend
+    the two speed buckets with weight (1-alpha, alpha). This exercises the speed
+    blending path that the vectorized impl shares with the scalar path.
+    """
+    fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
+    fit_valid = np.zeros(CurvatureDLookup.bucket_shape(), dtype=bool)
+
+    # Use a value per speed_idx that is easy to verify after blending.
+    per_speed_value = 1.0e-5 * np.arange(1, len(CurvatureDLookup.SPEED_ANCHORS) + 1, dtype=np.float32)
+    for s in range(len(CurvatureDLookup.SPEED_ANCHORS)):
+      fit_valid[s, 5] = True
+      fit_corrections[s, 5] = per_speed_value[s]
+
+    c = float(CurvatureDLookup.CURVATURE_BUCKET_CENTERS[5])
+
+    # At v_ego exactly at a SPEED_ANCHOR, alpha=0 -> only the low bucket contributes.
+    v_ego_at_anchor = float(CurvatureDLookup.SPEED_ANCHORS[3])
+    expected_at_anchor = float(per_speed_value[3])
+    val = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego_at_anchor, c)
+    assert np.isclose(val, expected_at_anchor), f"at anchor: {val} != {expected_at_anchor}"
+
+    # Midpoint between two anchors: alpha=0.5 -> exact 50/50 blend.
+    v_ego_mid = 0.5 * (float(CurvatureDLookup.SPEED_ANCHORS[2]) + float(CurvatureDLookup.SPEED_ANCHORS[3]))
+    expected_mid = 0.5 * (float(per_speed_value[2]) + float(per_speed_value[3]))
+    val = CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego_mid, c)
+    assert np.isclose(val, expected_mid, rtol=1e-6), f"at midpoint: {val} != {expected_mid}"
+
+    # General position: verify the explicit (1-alpha) * low + alpha * high formula.
+    low, high, alpha = CurvatureDLookup.speed_interp(v_ego_mid)
+    expected = (1.0 - alpha) * float(per_speed_value[low]) + alpha * float(per_speed_value[high])
+    assert np.isclose(val, expected, rtol=1e-6), f"blend formula: {val} != {expected}"
+
+  def test_exceeds_safety_bounds(self):
+    """Centralized safety check used by both controller.get_correction and
+    CurvatureEstimator._update_current_lookup.
+    """
+    # In-range curvature is safe
+    assert not CurvatureDLookup._exceeds_safety_bounds(1.0e-5, 20.0)
+
+    # Below CURVATURE_MIN -> exceed
+    assert CurvatureDLookup._exceeds_safety_bounds(-1.0, 20.0)
+
+    # Above CURVATURE_MAX -> exceed
+    assert CurvatureDLookup._exceeds_safety_bounds(CurvatureDLookup.CURVATURE_MAX + 1.0, 20.0)
+
+    # abs_curvature * v_ego^2 > MAX_LAT_ACCEL_APPLY (1.0 m/s^2) -> exceed
+    # v=20 m/s, c=3e-3 -> 3e-3 * 400 = 1.2 > 1.0
+    assert CurvatureDLookup._exceeds_safety_bounds(3.0e-3, 20.0)
+
+    # At the boundary, exactly at the limit: 2.5e-3 * 400 = 1.0, must NOT exceed (strict >)
+    assert not CurvatureDLookup._exceeds_safety_bounds(2.5e-3, 20.0)
+
+  def test_update_current_lookup_respects_safety_bounds(self):
+    """Ensure the published current_correction is 0.0 when the requested
+    curvature would exceed the lateral acceleration limit, not just when the
+    curvature is out of bucket range.
+    """
+    estimator = get_estimator()
+    estimator.use_params = True
+    estimator.update_use_params(force=True)
+
+    # Populate fit_corrections and fit_valid so we can isolate the safety check
+    estimator.fit_corrections = np.zeros(CurvatureDLookup.bucket_shape(), dtype=np.float32)
+    estimator.fit_valid = np.ones(CurvatureDLookup.bucket_shape(), dtype=bool)
+
+    # Inputs within the bucket range but exceeding the lateral-accel cap:
+    # 3e-3 * 20^2 = 1.2 m/s^2, above 1.0 m/s^2 limit
+    estimator._update_current_lookup(3.0e-3, 20.0)
+    assert estimator.current_correction == 0.0
+
+    # Inputs at exactly the limit: 2.5e-3 * 20^2 = 1.0, must not exceed
+    estimator._update_current_lookup(2.5e-3, 20.0)
+    # Not necessarily zero here (correction is small at the limit), but must be finite
+    assert np.isfinite(estimator.current_correction)
+
+    # use_params = False forces 0.0 even if all other conditions would allow a value
+    estimator.use_params = False
+    estimator._update_current_lookup(1.0e-4, 20.0)
+    assert estimator.current_correction == 0.0

selfdrive/ui/layouts/settings/ictoggles.py

@@ -194,12 +194,13 @@ class ICTogglesLayout(Widget):
     self._update_toggles()
 
   def _update_toggles(self):
+    # Use ui_state's params cache (refreshed in own thread at 5Hz) to avoid extra IPC roundtrips
     ui_state.update_params()
 
     # TODO: make a param control list item so we don't need to manage internal state as much here
     # refresh toggles from params to mirror external changes
     for param in self._toggle_defs:
-      self._toggles[param].action_item.set_state(self._params.get_bool(param))
+      self._toggles[param].action_item.set_state(ui_state.params.get_bool(param))
 
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
@@ -217,6 +218,6 @@ class ICTogglesLayout(Widget):
     self._scroller.render(rect)
 
   def _toggle_callback(self, state: bool, param: str):
-    self._params.put_bool(param, state, block=True)
+    self._params.put_bool(param, state)
     if self._toggle_defs[param][3]:
-      self._params.put_bool("OnroadCycleRequested", True, block=True)
+      self._params.put_bool("OnroadCycleRequested", True)

selfdrive/ui/mici/onroad/dynamic_steering_learner_graph.py

@@ -87,14 +87,17 @@ class DynamicSteeringLearnerGraphMici(Widget):
                          fit_corrections: np.ndarray, fit_valid: np.ndarray,
                          v_ego: float) -> tuple[np.ndarray, np.ndarray, float, float]:
     if lcp_frame != self._cached_lcp_frame:
-      self._cached_preview_curve = np.array([
-        CurvatureDLookup.interp_curve_value(preview_corrections, preview_valid, v_ego, abs(float(k)))
-        for k in self._plot_x
-      ], dtype=np.float32)
-      self._cached_fit_curve = np.array([
-        CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, abs(float(k)))
-        for k in self._plot_x
-      ], dtype=np.float32)
+      abs_curvatures = np.abs(self._plot_x).astype(np.float64)
+      # Recomputes only when liveCurvatureParameters changes (4Hz); cached across UI frames.
+      self._cached_fit_curve = CurvatureDLookup.interp_curve_value(
+        fit_corrections, fit_valid, v_ego, abs_curvatures
+      )
+      # Preview is only populated when ShowDynamicSteeringLearnerGraph is on.
+      has_preview = preview_corrections.shape == fit_corrections.shape and np.any(preview_corrections)
+      if has_preview:
+        self._cached_preview_curve = CurvatureDLookup.interp_curve_value(
+          preview_corrections, preview_valid, v_ego, abs_curvatures
+        )
       self._cached_min_y, self._cached_max_y = self._compute_y_bounds(self._cached_preview_curve, self._cached_fit_curve)
       self._cached_lcp_frame = lcp_frame
 

selfdrive/ui/onroad/dynamic_steering_learner_graph.py

@@ -96,14 +96,17 @@ class DynamicSteeringLearnerGraph(Widget):
                          fit_corrections: np.ndarray, fit_valid: np.ndarray,
                          v_ego: float) -> tuple[np.ndarray, np.ndarray, float, float]:
     if lcp_frame != self._cached_lcp_frame:
-      self._cached_preview_curve = np.array([
-        CurvatureDLookup.interp_curve_value(preview_corrections, preview_valid, v_ego, abs(float(k)))
-        for k in self._plot_x
-      ], dtype=np.float32)
-      self._cached_fit_curve = np.array([
-        CurvatureDLookup.interp_curve_value(fit_corrections, fit_valid, v_ego, abs(float(k)))
-        for k in self._plot_x
-      ], dtype=np.float32)
+      abs_curvatures = np.abs(self._plot_x).astype(np.float64)
+      # Recomputes only when liveCurvatureParameters changes (4Hz); cached across UI frames.
+      self._cached_fit_curve = CurvatureDLookup.interp_curve_value(
+        fit_corrections, fit_valid, v_ego, abs_curvatures
+      )
+      # Preview is only populated when ShowDynamicSteeringLearnerGraph is on.
+      has_preview = preview_corrections.shape == fit_corrections.shape and np.any(preview_corrections)
+      if has_preview:
+        self._cached_preview_curve = CurvatureDLookup.interp_curve_value(
+          preview_corrections, preview_valid, v_ego, abs_curvatures
+        )
       self._cached_min_y, self._cached_max_y = self._compute_y_bounds(self._cached_preview_curve, self._cached_fit_curve)
       self._cached_lcp_frame = lcp_frame
 
@@ -117,23 +120,18 @@ class DynamicSteeringLearnerGraph(Widget):
     if sm.recv_frame["carState"] < ui_state.started_frame or sm.recv_frame["controlsState"] < ui_state.started_frame:
       return
 
-    battery_line_height = int(BATTERY_CONFIG.line_height * BATTERY_CONFIG.scale_factor)
-    battery_panel_height = battery_line_height * 4
-    battery_panel_margin = BATTERY_CONFIG.panel_margin
-    graph_bottom = rect.y + rect.height - CONFIG.bottom_gap_to_battery - battery_panel_height - battery_panel_margin
-    graph_top = rect.y + CONFIG.speed_display_bottom_y + CONFIG.top_gap_to_speed
-    graph_height = max(CONFIG.height, int(graph_bottom - graph_top))
-    graph_width = int(graph_height * CONFIG.aspect_ratio)
+    # When curvatured is not running for this car, render a placeholder instead of
+    # doing any interpolation work.
+    lcp = sm["liveCurvatureParameters"]
+    if not bool(getattr(lcp, "useParams", False)):
+      graph_rect = self._build_graph_rect(rect)
+      rl.draw_rectangle_rounded(graph_rect, 0.08, 8, self._panel_bg)
+      self._draw_title_only(graph_rect)
+      return
 
-    graph_rect = rl.Rectangle(
-      rect.x + rect.width - graph_width - battery_panel_margin,
-      graph_bottom - graph_height,
-      graph_width,
-      graph_height,
-    )
+    graph_rect = self._build_graph_rect(rect)
     rl.draw_rectangle_rounded(graph_rect, 0.08, 8, self._panel_bg)
 
-    lcp = sm["liveCurvatureParameters"]
     lcp_frame = sm.recv_frame["liveCurvatureParameters"]
     controls_state = sm["controlsState"]
     car_state = sm["carState"]
@@ -170,6 +168,30 @@ class DynamicSteeringLearnerGraph(Widget):
     self._draw_overlay_info(graph_rect, lcp, float(car_state.vEgo), float(controls_state.modelDesiredCurvature),
                             fit_corrections, fit_valid, min_y, max_y, transport_valid, payload_valid)
 
+  def _build_graph_rect(self, rect: rl.Rectangle) -> rl.Rectangle:
+    """Compute the on-screen rectangle for the dynamic steering learner panel."""
+    battery_line_height = int(BATTERY_CONFIG.line_height * BATTERY_CONFIG.scale_factor)
+    battery_panel_height = battery_line_height * 4
+    battery_panel_margin = BATTERY_CONFIG.panel_margin
+    graph_bottom = rect.y + rect.height - CONFIG.bottom_gap_to_battery - battery_panel_height - battery_panel_margin
+    graph_top = rect.y + CONFIG.speed_display_bottom_y + CONFIG.top_gap_to_speed
+    graph_height = max(CONFIG.height, int(graph_bottom - graph_top))
+    graph_width = int(graph_height * CONFIG.aspect_ratio)
+    return rl.Rectangle(
+      rect.x + rect.width - graph_width - battery_panel_margin,
+      graph_bottom - graph_height,
+      graph_width,
+      graph_height,
+    )
+
+  def _draw_title_only(self, graph_rect: rl.Rectangle) -> None:
+    """Draw the panel title when no live curve data is available."""
+    title_size = min(42, max(34, int(graph_rect.height * 0.095)))
+    text_x = float(graph_rect.x + CONFIG.padding)
+    title_y = float(graph_rect.y + 12)
+    rl.draw_text_ex(self._font_bold, "Dynamic Steering Learner", rl.Vector2(text_x, title_y),
+                    title_size, 0, self._text_color)
+
   def _draw_plot(self, plot_rect: rl.Rectangle,
                  preview_curve: np.ndarray, corrections: np.ndarray,
                  min_y: float, max_y: float,