Refactor slope and interpolation logic into a utility module

Moved `compute_symmetric_slopes` and `hermite_interpolate` functions from the dynamic personality controller to a shared utility module. Updated imports and references accordingly to improve code reusability and separation of concerns.

sunnypilot/common/utils.py

@@ -0,0 +1,52 @@
+import numpy as np
+
+def compute_symmetric_slopes(x, y):
+  """
+  Compute symmetric slopes for use in Hermite interpolation.
+
+  Args:
+      x: Array of x coordinates
+      y: Array of y coordinates
+
+  Returns:
+      Array of computed slopes at each point
+  """
+  n = len(x)
+  m = np.zeros(n)
+  for i in range(n):
+    if i == 0:
+      m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i])
+    elif i == n-1:
+      m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
+    else:
+      m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
+  return m
+
+def hermite_interpolate(x, xp, yp, slopes):
+  """
+  Perform Hermite interpolation at point x given data points and slopes.
+
+  Args:
+      x: Point at which to interpolate
+      xp: Array of x coordinates of data points
+      yp: Array of y coordinates of data points
+      slopes: Array of slopes at data points
+
+  Returns:
+      Interpolated value at x
+  """
+  x = np.clip(x, xp[0], xp[-1])
+  idx = np.searchsorted(xp, x) - 1
+  idx = np.clip(idx, 0, len(slopes) - 2)
+
+  x0, x1 = xp[idx], xp[idx+1]
+  y0, y1 = yp[idx], yp[idx+1]
+  m0, m1 = slopes[idx], slopes[idx+1]
+
+  t = (x - x0) / (x1 - x0)
+  h00 = 2*t**3 - 3*t**2 + 1
+  h10 = t**3 - 2*t**2 + t
+  h01 = -2*t**3 + 3*t**2
+  h11 = t**3 - t**2
+
+  return (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1)

sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_personality_controller.py

@@ -1,5 +1,5 @@
 from cereal import log
-import numpy as np
+from openpilot.sunnypilot.common.utils import compute_symmetric_slopes, hermite_interpolate
 
 class DynamicPersonalityController:
   """
@@ -10,35 +10,6 @@ class DynamicPersonalityController:
   def __init__(self):
     pass
 
-  def compute_symmetric_slopes(self, x, y):
-    n = len(x)
-    m = np.zeros(n)
-    for i in range(n):
-      if i == 0:
-        m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i])
-      elif i == n-1:
-        m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
-      else:
-        m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
-    return m
-
-  def hermite_interpolate(self, x, xp, yp, slopes):
-    x = np.clip(x, xp[0], xp[-1])
-    idx = np.searchsorted(xp, x) - 1
-    idx = np.clip(idx, 0, len(slopes) - 2)
-
-    x0, x1 = xp[idx], xp[idx+1]
-    y0, y1 = yp[idx], yp[idx+1]
-    m0, m1 = slopes[idx], slopes[idx+1]
-
-    t = (x - x0) / (x1 - x0)
-    h00 = 2*t**3 - 3*t**2 + 1
-    h10 = t**3 - 2*t**2 + t
-    h01 = -2*t**3 + 3*t**2
-    h11 = t**3 - t**2
-
-    return (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1)
-
   def get_dynamic_follow_distance(self, v_ego, personality=log.LongitudinalPersonality.standard):
     """
     Calculate the dynamic follow distance based on current speed and personality.
@@ -62,7 +33,7 @@ class DynamicPersonalityController:
     else:
       raise NotImplementedError("Dynamic personality not supported")
 
-    slopes = self.compute_symmetric_slopes(x_vel, y_dist)
-    result = float(self.hermite_interpolate(v_ego, x_vel, y_dist, slopes))
+    slopes = compute_symmetric_slopes(x_vel, y_dist)
+    result = float(hermite_interpolate(v_ego, x_vel, y_dist, slopes))
 
     return result

sunnypilot/selfdrive/controls/lib/dynamic_personality/tests/pytest_dpc.py

@@ -3,6 +3,7 @@ import numpy as np
 from cereal import log
 
 from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_personality_controller import DynamicPersonalityController
+from openpilot.sunnypilot.common.utils import compute_symmetric_slopes, hermite_interpolate
 
 class TestDynamicPersonalityController:
 
@@ -25,7 +26,7 @@ class TestDynamicPersonalityController:
     expected_slopes[2] = ((y[3] - y[2]) / (x[3] - x[2]) + (y[2] - y[1]) / (x[2] - x[1])) / 2  # (3.0 + 2.0) / 2 = 2.5
     expected_slopes[3] = (y[3] - y[2]) / (x[3] - x[2])  # 3.0
 
-    computed_slopes = self.controller.compute_symmetric_slopes(x, y)
+    computed_slopes = compute_symmetric_slopes(x, y)
 
     np.testing.assert_allclose(computed_slopes, expected_slopes, rtol=1e-5)
 
@@ -36,13 +37,13 @@ class TestDynamicPersonalityController:
     slopes = np.array([1.0, 0.0, -1.0])
 
     # Test at data points
-    assert self.controller.hermite_interpolate(0.0, xp, yp, slopes) == pytest.approx(5.0)
-    assert self.controller.hermite_interpolate(10.0, xp, yp, slopes) == pytest.approx(15.0)
-    assert self.controller.hermite_interpolate(20.0, xp, yp, slopes) == pytest.approx(10.0)
+    assert hermite_interpolate(0.0, xp, yp, slopes) == pytest.approx(5.0)
+    assert hermite_interpolate(10.0, xp, yp, slopes) == pytest.approx(15.0)
+    assert hermite_interpolate(20.0, xp, yp, slopes) == pytest.approx(10.0)
 
     # Test interpolation
-    assert self.controller.hermite_interpolate(5.0, xp, yp, slopes) == pytest.approx(11.25)
-    assert self.controller.hermite_interpolate(15.0, xp, yp, slopes) == pytest.approx(13.75)
+    assert hermite_interpolate(5.0, xp, yp, slopes) == pytest.approx(11.25)
+    assert hermite_interpolate(15.0, xp, yp, slopes) == pytest.approx(13.75)
 
   def test_hermite_interpolate_clipping(self):
     """Test that hermite interpolation properly clips values outside the range"""
@@ -51,10 +52,10 @@ class TestDynamicPersonalityController:
     slopes = np.array([1.0, 0.0, -1.0])
 
     # Test clipping below minimum
-    assert self.controller.hermite_interpolate(-5.0, xp, yp, slopes) == pytest.approx(5.0)
+    assert hermite_interpolate(-5.0, xp, yp, slopes) == pytest.approx(5.0)
 
     # Test clipping above maximum
-    assert self.controller.hermite_interpolate(25.0, xp, yp, slopes) == pytest.approx(10.0)
+    assert hermite_interpolate(25.0, xp, yp, slopes) == pytest.approx(10.0)
 
   def test_get_dynamic_follow_distance_relaxed(self):
     """Test follow distance calculation for relaxed personality"""