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.

common/params_keys.h

@@ -128,6 +128,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"CarParamsSPCache", CLEAR_ON_MANAGER_START},
     {"CarParamsSPPersistent", PERSISTENT},
     {"CarPlatformBundle", PERSISTENT},
+    {"DynamicPersonality", PERSISTENT | BACKUP},
     {"EnableGithubRunner", PERSISTENT | BACKUP},
     {"MaxTimeOffroad", PERSISTENT | BACKUP},
     {"ModelRunnerTypeCache", CLEAR_ON_ONROAD_TRANSITION},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,6 +10,9 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
+from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_personality_controller import DynamicPersonalityController
+
+
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -228,6 +231,7 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
+    self.dynamic_personality_controller = DynamicPersonalityController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -327,9 +331,9 @@ class LongitudinalMpc:
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
     return lead_xv
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-    t_follow = get_T_FOLLOW(personality)
+  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, dynamic_personality=False):
     v_ego = self.x0[1]
+    t_follow = self.dynamic_personality_controller.get_dynamic_follow_distance(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality)
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
     lead_xv_0 = self.process_lead(radarstate.leadOne)

selfdrive/controls/lib/longitudinal_planner.py

@@ -1,7 +1,6 @@
 #!/usr/bin/env python3
 import math
 import numpy as np
-
 import cereal.messaging as messaging
 from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.common.conversions import Conversions as CV
@@ -149,7 +148,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality, dynamic_personality = self.dynamic_personality)
 
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/ui/qt/offroad/developer_panel.cc

@@ -60,6 +60,9 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) {
   enableGithubRunner = new ParamControl("EnableGithubRunner", tr("Enable GitHub runner service"), tr("Enables or disables the github runner service."), "");
   addItem(enableGithubRunner);
 
+    dynamicpersonality = new ParamControl("DynamicPersonality", tr("Enable Dynamic Personality"), tr("Adjust follow distance dynamically "), "");
+  addItem(dynamicpersonality);
+
   // error log button
   errorLogBtn = new ButtonControl(tr("Error Log"), tr("VIEW"), tr("View the error log for sunnypilot crashes."));
   connect(errorLogBtn, &ButtonControl::clicked, [=]() {
@@ -84,7 +87,7 @@ void DeveloperPanel::updateToggles(bool _offroad) {
      * - visible, and
      * - during onroad & offroad states
      */
-    if (btn != experimentalLongitudinalToggle) {
+    if (btn != experimentalLongitudinalToggle && btn != dynamicpersonality) {
       btn->setEnabled(_offroad);
     }
   }
@@ -122,6 +125,7 @@ void DeveloperPanel::updateToggles(bool _offroad) {
 
   // Handle specific controls visibility for release branches
   enableGithubRunner->setVisible(!is_release);
+  dynamicpersonality->setVisible(!is_release);
   errorLogBtn->setVisible(!is_release);
   joystickToggle->setVisible(!is_release);
 

selfdrive/ui/qt/offroad/developer_panel.h

@@ -21,6 +21,7 @@ private:
   ParamControl* experimentalLongitudinalToggle;
   ParamControl* hyundaiRadarTracksToggle;
   ParamControl* enableGithubRunner;
+  ParamControl* dynamicpersonality;
   bool is_release;
   bool offroad = false;
 

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

@@ -0,0 +1,39 @@
+from cereal import log
+from openpilot.sunnypilot.common.utils import compute_symmetric_slopes, hermite_interpolate
+
+class DynamicPersonalityController:
+  """
+  Controller for managing dynamic personality-based following distances
+  that adapt based on vehicle speed and selected driving personality.
+  """
+
+  def __init__(self):
+    pass
+
+  def get_dynamic_follow_distance(self, v_ego, personality=log.LongitudinalPersonality.standard):
+    """
+    Calculate the dynamic follow distance based on current speed and personality.
+
+    Args:
+        v_ego: Current vehicle speed
+        personality: Selected longitudinal personality mode
+
+    Returns:
+        float: The calculated follow distance factor
+    """
+    if personality == log.LongitudinalPersonality.relaxed:
+      x_vel =  [0., 2.5,  5.,   19.7,   22.2, 40.]
+      y_dist = [1.25, 1.25, 1.3, 1.3, 1.75, 1.75]
+    elif personality == log.LongitudinalPersonality.standard:
+      x_vel =  [0., 2.5,  5.,   19.7,   22.2, 40.]
+      y_dist = [1.20, 1.20, 1.275, 1.275, 1.50, 1.50]
+    elif personality == log.LongitudinalPersonality.aggressive:
+      x_vel =  [0., 2.5,  6.,   19.7,   22.2, 40.]
+      y_dist = [0.92, 0.9, 1.25, 1.25, 1.30, 1.30]
+    else:
+      raise NotImplementedError("Dynamic personality not supported")
+
+    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

@@ -0,0 +1,116 @@
+import pytest
+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:
+
+  def setup_method(self):
+    """Set up the controller before each test"""
+    self.controller = DynamicPersonalityController()
+
+  def test_compute_symmetric_slopes(self):
+    """Test the symmetric slope computation method"""
+    x = np.array([1.0, 2.0, 3.0, 4.0])
+    y = np.array([2.0, 3.0, 5.0, 8.0])
+
+    # Manual calculation of expected slopes based on the algorithm:
+    # First slope: direct derivative from first segment
+    # Middle slopes: average of adjacent segments
+    # Last slope: direct derivative from last segment
+    expected_slopes = np.zeros(4)
+    expected_slopes[0] = (y[1] - y[0]) / (x[1] - x[0])  # 1.0
+    expected_slopes[1] = ((y[2] - y[1]) / (x[2] - x[1]) + (y[1] - y[0]) / (x[1] - x[0])) / 2  # (2.0 + 1.0) / 2 = 1.5
+    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 = compute_symmetric_slopes(x, y)
+
+    np.testing.assert_allclose(computed_slopes, expected_slopes, rtol=1e-5)
+
+  def test_hermite_interpolate(self):
+    """Test hermite interpolation with known values"""
+    xp = np.array([0.0, 10.0, 20.0])
+    yp = np.array([5.0, 15.0, 10.0])
+    slopes = np.array([1.0, 0.0, -1.0])
+
+    # Test at data points
+    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 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"""
+    xp = np.array([0.0, 10.0, 20.0])
+    yp = np.array([5.0, 15.0, 10.0])
+    slopes = np.array([1.0, 0.0, -1.0])
+
+    # Test clipping below minimum
+    assert hermite_interpolate(-5.0, xp, yp, slopes) == pytest.approx(5.0)
+
+    # Test clipping above maximum
+    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"""
+    # Test at specific data points
+    assert self.controller.get_dynamic_follow_distance(0.0, log.LongitudinalPersonality.relaxed) == pytest.approx(1.25)
+    assert self.controller.get_dynamic_follow_distance(5.0, log.LongitudinalPersonality.relaxed) == pytest.approx(1.3)
+    assert self.controller.get_dynamic_follow_distance(40.0, log.LongitudinalPersonality.relaxed) == pytest.approx(1.75)
+
+    # Test interpolation
+    assert self.controller.get_dynamic_follow_distance(20.0, log.LongitudinalPersonality.relaxed) > 1.3
+    assert self.controller.get_dynamic_follow_distance(20.0, log.LongitudinalPersonality.relaxed) < 1.75
+
+  def test_get_dynamic_follow_distance_standard(self):
+    """Test follow distance calculation for standard personality"""
+    # Test at specific data points
+    assert self.controller.get_dynamic_follow_distance(0.0, log.LongitudinalPersonality.standard) == pytest.approx(1.20)
+    assert self.controller.get_dynamic_follow_distance(5.0, log.LongitudinalPersonality.standard) == pytest.approx(1.275)
+    assert self.controller.get_dynamic_follow_distance(40.0, log.LongitudinalPersonality.standard) == pytest.approx(1.50)
+
+    # Test interpolation
+    mid_value = self.controller.get_dynamic_follow_distance(21.0, log.LongitudinalPersonality.standard)
+    assert mid_value > 1.275
+    assert mid_value < 1.50
+
+  def test_get_dynamic_follow_distance_aggressive(self):
+    """Test follow distance calculation for aggressive personality"""
+    # Test at specific data points
+    assert self.controller.get_dynamic_follow_distance(0.0, log.LongitudinalPersonality.aggressive) == pytest.approx(0.92)
+    assert self.controller.get_dynamic_follow_distance(6.0, log.LongitudinalPersonality.aggressive) == pytest.approx(1.25)
+    assert self.controller.get_dynamic_follow_distance(40.0, log.LongitudinalPersonality.aggressive) == pytest.approx(1.30)
+
+    # Test intermediate value
+    assert self.controller.get_dynamic_follow_distance(4.0, log.LongitudinalPersonality.aggressive) > 0.9
+    assert self.controller.get_dynamic_follow_distance(4.0, log.LongitudinalPersonality.aggressive) < 1.25
+
+  def test_get_dynamic_follow_distance_invalid(self):
+    """Test that an invalid personality raises NotImplementedError"""
+    with pytest.raises(NotImplementedError, match="Dynamic personality not supported"):
+      self.controller.get_dynamic_follow_distance(10.0, 999)  # Using an invalid personality value
+
+  def test_get_dynamic_follow_distance_comparison(self):
+    """Test that relaxed > standard > aggressive for follow distances"""
+    speed = 20.0
+    relaxed = self.controller.get_dynamic_follow_distance(speed, log.LongitudinalPersonality.relaxed)
+    standard = self.controller.get_dynamic_follow_distance(speed, log.LongitudinalPersonality.standard)
+    aggressive = self.controller.get_dynamic_follow_distance(speed, log.LongitudinalPersonality.aggressive)
+
+    assert relaxed > standard
+    assert standard > aggressive
+
+  def test_speed_impact_on_follow_distance(self):
+    """Test that follow distance increases with speed"""
+    personality = log.LongitudinalPersonality.standard
+
+    low_speed = self.controller.get_dynamic_follow_distance(5.0, personality)
+    high_speed = self.controller.get_dynamic_follow_distance(30.0, personality)
+
+    assert high_speed > low_speed

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.models.helpers import get_active_model_runner
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.common.params import Params
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 
@@ -18,6 +19,17 @@ class LongitudinalPlannerSP:
     self.dec = DynamicExperimentalController(CP, mpc)
     self.is_stock = get_active_model_runner() == custom.ModelManagerSP.Runner.stock
 
+    self.params = Params()
+    self.param_read_counter = 0
+    self.dynamic_personality = False
+    self.read_param()
+
+  def read_param(self):
+    try:
+      self.dynamic_personality = self.params.get_bool("DynamicPersonality")
+    except AttributeError:
+      pass
+
   def get_mpc_mode(self) -> str | None:
     if not self.dec.active():
       return None
@@ -25,6 +37,9 @@ class LongitudinalPlannerSP:
     return self.dec.mode()
 
   def update(self, sm: messaging.SubMaster) -> None:
+    self.param_read_counter += 1
+    if self.param_read_counter % 50 == 0:
+      self.read_param()
     self.dec.update(sm)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:

system/manager/manager.py

@@ -45,6 +45,7 @@ def manager_init() -> None:
     ("AutoLaneChangeTimer", "0"),
     ("AutoLaneChangeBsmDelay", "0"),
     ("DynamicExperimentalControl", "0"),
+    ("DynamicPersonality", "0"),
     ("HyundaiLongitudinalTuning", "0"),
     ("Mads", "1"),
     ("MadsMainCruiseAllowed", "1"),