rip vibecontroller in to two halfs

cereal/custom.capnp

@@ -192,6 +192,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelPersonality @8 :AccelerationPersonality;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -203,7 +204,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
-
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+  }
   struct SmartCruiseControl {
     vision @0 :Vision;
     map @1 :Map;
@@ -446,6 +451,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
+  leftLaneChangeEdgeBlock @1 :Bool;
+  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

common/params_keys.h

@@ -133,6 +133,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"Version", {PERSISTENT, STRING}},
 
     // --- sunnypilot params --- //
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast<int>(cereal::LongitudinalPlanSP::AccelerationPersonality::NORMAL))}},
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "0"}},
@@ -151,6 +153,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"CustomAccShortPressIncrement", {PERSISTENT | BACKUP, INT, "1"}},
     {"DeviceBootMode", {PERSISTENT | BACKUP, INT, "0"}},
     {"DevUIInfo", {PERSISTENT | BACKUP, INT, "0"}},
+    {"DynamicFollow", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
     {"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
     {"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,6 +10,8 @@ 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.accel_personality.accel_controller import AccelPersonalityController
+from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -228,6 +230,8 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
+    self.accel_controller = AccelPersonalityController()
+    self.dynamic_follow = FollowDistanceController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -328,10 +332,27 @@ class LongitudinalMpc:
     return lead_xv
 
   def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
-    t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
+
+    if self.dynamic_follow.is_enabled():
+      t_follow = self.dynamic_follow.get_follow_distance_multiplier(v_ego)
+      #print(f"DEBUG: dynamic_follow enabled, t_follow={t_follow:.3f}, v_ego={v_ego:.2f}, v_cruise={v_cruise:.2f}")
+    else:
+      t_follow = get_T_FOLLOW(personality)
+      #print(f"DEBUG: dynamic_follow disabled, using personality t_follow={t_follow:.3f}, personality={personality}")
+
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
+    # Get acceleration limits
+    if self.accel_controller.is_enabled():
+      min_accel = self.accel_controller.get_min_accel(v_ego)
+      #print(f"DEBUG: accel_enabled=True, min_accel={min_accel:.3f}")
+    else:
+      min_accel = CRUISE_MIN_ACCEL
+      #print(f"DEBUG: accel_enabled=False, using stock min_accel={min_accel}")
+
+    a_cruise_min = min_accel
+
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
@@ -350,7 +371,7 @@ class LongitudinalMpc:
 
       # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
       # when the leads are no factor.
-      v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05)
+      v_lower = v_ego + (T_IDXS * a_cruise_min * 1.05)
       # TODO does this make sense when max_a is negative?
       v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05)
       v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

selfdrive/controls/lib/longitudinal_planner.py

@@ -124,7 +124,13 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
     if mode == 'acc':
-      accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+      if self.accel_controller.is_enabled():
+        max_accel = self.accel_controller.get_max_accel(v_ego)
+        #print(f"Vibe personality active - max accel: {max_accel:.3f}")
+        accel_clip = [ACCEL_MIN, max_accel]
+      else:
+        accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
+
       steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
       accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
     else:

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -0,0 +1,112 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from cereal import custom
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+# Acceleration Profiles
+MAX_ACCEL_PROFILES = {
+  AccelPersonality.eco:    [2.0, 1.99, 1.88, 1.10, 0.500, 0.292, 0.15, 0.10],
+  AccelPersonality.normal: [1.0, 2.00, 1.94, 1.22, 0.635, 0.33, 0.22, 0.16],
+  AccelPersonality.sport:  [.5, 2.00, 2.00, 1.85, 0.800, 0.54, 0.32, 0.22],
+}
+MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.]
+# Braking Profiles
+MIN_ACCEL_PROFILES = {
+  AccelPersonality.eco:    [-0.14, -0.0006, -0.010, -0.30, -1.20],
+  AccelPersonality.normal: [-0.1, -0.0007, -0.012, -0.35, -1.20],
+  AccelPersonality.sport:  [-0.6, -0.0008, -0.014, -0.40, -1.20],
+}
+MIN_ACCEL_BREAKPOINTS = [0., 3., 11., 14., 50.]
+
+
+class AccelPersonalityController:
+
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.accel_personality = AccelPersonality.normal
+    self.param_keys = {
+      'personality': 'AccelPersonality',
+      'enabled': 'AccelPersonalityEnabled'
+    }
+    self._load_personality_from_params()
+
+  def _load_personality_from_params(self):
+    try:
+      saved = self.params.get(self.param_keys['personality'])
+      if saved is not None:
+        personality_value = int(saved)
+        if personality_value in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
+          self.accel_personality = personality_value
+        else:
+          cloudlog.warning(f"Invalid personality value {personality_value}, using normal")
+          self.accel_personality = AccelPersonality.normal
+    except (ValueError, TypeError) as e:
+      cloudlog.warning(f"Failed to load personality from params: {e}")
+      self.accel_personality = AccelPersonality.normal
+
+  def _update_from_params(self):
+    if self.frame % int(1. / DT_MDL) != 0:
+      return
+    self._load_personality_from_params()
+
+  def get_accel_personality(self) -> int:
+    self._update_from_params()
+    return int(self.accel_personality)
+
+  def set_accel_personality(self, personality: int):
+    if personality not in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
+      cloudlog.error(f"Invalid personality {personality}, ignoring")
+      return
+
+    self.accel_personality = personality
+    self.params.put(self.param_keys['personality'], str(personality))
+    cloudlog.info(f"Accel personality set to {personality}")
+
+  def cycle_accel_personality(self) -> int:
+    personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
+    current_idx = personalities.index(self.accel_personality)
+    next_personality = personalities[(current_idx + 1) % len(personalities)]
+    self.set_accel_personality(next_personality)
+    return int(next_personality)
+
+  def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
+    max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
+    min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality])
+    return float(min_a), float(max_a)
+
+  def get_min_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[0]
+
+  def get_max_accel(self, v_ego: float) -> float:
+    return self.get_accel_limits(v_ego)[1]
+
+  def is_enabled(self) -> bool:
+    return self.params.get_bool(self.param_keys['enabled'])
+
+  def set_enabled(self, enabled: bool):
+    self.params.put_bool(self.param_keys['enabled'], enabled)
+    cloudlog.info(f"Accel personality controller {'enabled' if enabled else 'disabled'}")
+
+  def toggle_enabled(self) -> bool:
+    current = self.is_enabled()
+    self.set_enabled(not current)
+    return not current
+
+  def reset(self):
+    self.accel_personality = AccelPersonality.normal
+    self.frame = 0
+
+  def update(self):
+    self.frame += 1
+    self._update_from_params()

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

@@ -0,0 +1,48 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from cereal import log
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+LongPersonality = log.LongitudinalPersonality
+
+# Follow distance profiles mapped to LongPersonality
+FOLLOW_PROFILES = {
+  LongPersonality.relaxed:    [1.55, 1.65, 1.65, 1.80],
+  LongPersonality.standard:   [1.45, 1.45, 1.45, 1.55],
+  LongPersonality.aggressive: [1.20, 1.25, 1.28, 1.35],
+}
+FOLLOW_BREAKPOINTS = [0., 6., 18., 36.]
+
+
+class FollowDistanceController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.personality = LongPersonality.standard
+
+  def _update_from_params(self):
+    if self.frame % int(1. / DT_MDL) != 0:
+      return
+    self.personality = int(self.params.get('LongitudinalPersonality'))
+
+  def is_enabled(self) -> bool:
+    return self.params.get_bool('DynamicFollow')
+
+  def toggle(self) -> bool:
+    enabled = self.is_enabled()
+    self.params.put_bool('DynamicFollow', not enabled)
+    return not enabled
+
+  def get_follow_distance_multiplier(self, v_ego: float) -> float:
+    self._update_from_params()
+    return float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality]))
+
+  def update(self):
+    self.frame += 1

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -17,6 +17,7 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolve
 from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP
 from openpilot.sunnypilot.models.helpers import get_active_bundle
 
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
@@ -26,6 +27,7 @@ class LongitudinalPlannerSP:
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel_controller = AccelPersonalityController()
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -81,6 +83,7 @@ class LongitudinalPlannerSP:
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
+    self.accel_controller.update()
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')

sunnypilot/selfdrive/controls/lib/vibe_personality/tests/test_vibe.py

@@ -0,0 +1,314 @@
+import pytest
+
+# Import the actual modules
+from cereal import log, custom
+from openpilot.common.realtime import DT_MDL
+
+# Import the enums we need for testing
+LongPersonality = log.LongitudinalPersonality
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+
+class MockParams:
+  """Simple mock for Params class"""
+  def __init__(self):
+    self.data = {}
+    self.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+  def get(self, key, encoding=None):
+    return self.data.get(key)
+
+  def get_bool(self, key):
+    return self.bool_data.get(key, True)
+
+  def put(self, key, value):
+    self.data[key] = value
+
+  def put_bool(self, key, value):
+    self.bool_data[key] = value
+
+  def reset_mock(self):
+    self.call_count = 0
+
+  @property
+  def call_count(self):
+    return getattr(self, '_call_count', 0)
+
+  @call_count.setter
+  def call_count(self, value):
+    self._call_count = value
+
+
+@pytest.fixture
+def mock_params():
+  """Create mock params instance"""
+  return MockParams()
+
+
+@pytest.fixture
+def controller(mock_params, monkeypatch):
+  """Create controller instance with mocked Params"""
+  # Patch the Params import in the controller module
+  monkeypatch.setattr('openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality.Params',
+                      lambda: mock_params)
+
+  from openpilot.sunnypilot.selfdrive.controls.lib.vibe_personality.vibe_personality import VibePersonalityController
+  return VibePersonalityController()
+
+
+class TestVibePersonalityController:
+
+  def test_initialization(self, controller):
+    """Test controller initializes with correct defaults"""
+    assert controller.frame == 0
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.long_personality == LongPersonality.standard
+    assert 'accel_personality' in controller.param_keys
+    assert 'long_personality' in controller.param_keys
+
+  def test_frame_increment(self, controller):
+    """Test frame counter increments correctly"""
+    initial_frame = controller.frame
+    controller.update()
+    assert controller.frame == initial_frame + 1
+
+    controller.update()
+    assert controller.frame == initial_frame + 2
+
+  def test_parameter_reading_throttled(self, controller, mock_params):
+    """Test parameters are only read every DT_MDL frames"""
+    # Track calls manually
+    original_get = mock_params.get
+    call_count = 0
+
+    def counting_get(*args, **kwargs):
+      nonlocal call_count
+      call_count += 1
+      return original_get(*args, **kwargs)
+
+    mock_params.get = counting_get
+
+    # First call should read params (frame 0)
+    controller._update_from_params()
+
+    # Reset counter
+    call_count = 0
+
+    # Advance frame but not to threshold
+    controller.frame = 5  # Less than int(1/DT_MDL)
+    controller._update_from_params()
+    assert call_count == 0  # Should not read params
+
+    # Advance to threshold
+    controller.frame = int(1. / DT_MDL)  # Equal to threshold
+    controller._update_from_params()
+    assert call_count >= 2  # Should read both personality params
+
+  def test_accel_personality_management(self, controller, mock_params):
+    """Test acceleration personality setting and cycling"""
+    # Test setting valid personality
+    assert controller.set_accel_personality(AccelPersonality.eco)
+    assert controller.accel_personality == AccelPersonality.eco
+
+    assert controller.set_accel_personality(AccelPersonality.sport)
+    assert controller.accel_personality == AccelPersonality.sport
+
+    # Test setting invalid personality
+    assert not controller.set_accel_personality(999)
+    assert controller.accel_personality == AccelPersonality.sport  # Should remain unchanged
+
+    # Test cycling
+    controller.accel_personality = AccelPersonality.eco
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.normal  # should cycle to normal
+    assert controller.accel_personality == AccelPersonality.normal
+
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.sport  # should cycle to sport
+
+    next_personality = controller.cycle_accel_personality()
+    assert next_personality == AccelPersonality.eco  # should cycle back to eco
+
+  def test_long_personality_management(self, controller, mock_params):
+    """Test longitudinal personality setting and cycling"""
+    # Test setting valid personality
+    assert controller.set_long_personality(LongPersonality.relaxed)
+    assert controller.long_personality == LongPersonality.relaxed
+
+    assert controller.set_long_personality(LongPersonality.aggressive)
+    assert controller.long_personality == LongPersonality.aggressive
+
+    # Test setting invalid personality
+    assert not controller.set_long_personality(999)
+    assert controller.long_personality == LongPersonality.aggressive  # Should remain unchanged
+
+    # Test cycling
+    controller.long_personality = LongPersonality.standard
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.aggressive  # should cycle to aggressive
+    assert controller.long_personality == LongPersonality.aggressive
+
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.relaxed  # should cycle to relaxed
+
+    next_personality = controller.cycle_long_personality()
+    assert next_personality == LongPersonality.standard  # should cycle back to standard
+
+  def test_toggle_functions(self, controller, mock_params):
+    """Test toggle functionality"""
+    # Set initial state to False
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+
+    result = controller.toggle_personality()
+    assert result  # Should toggle to True
+    assert mock_params.bool_data['VibePersonalityEnabled']
+
+    # Set initial state to True
+    mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
+
+    result = controller.toggle_accel_personality()
+    assert not result  # Should toggle to False
+    assert not mock_params.bool_data['VibeAccelPersonalityEnabled']
+
+  def test_enable_checks(self, controller, mock_params):
+    """Test various enable state checks"""
+    # All enabled
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    assert controller.is_enabled()
+    assert controller.is_accel_enabled()
+    assert controller.is_follow_enabled()
+
+    # Main toggle disabled
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+
+    assert not controller.is_enabled()
+    assert not controller.is_accel_enabled()
+    assert not controller.is_follow_enabled()
+
+  def test_accel_limits_calculation(self, controller, mock_params):
+    """Test acceleration limits calculation"""
+    # Enable all features through mock_params bool_data
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    # Test with different speeds and personalities
+    controller.accel_personality = 1  # normal
+    controller.long_personality = 1  # standard
+
+    limits = controller.get_accel_limits(10.0)  # 10 m/s
+    assert limits is not None
+    min_a, max_a = limits
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+    assert min_a < 0  # Should be negative (braking)
+    assert max_a > 0  # Should be positive (acceleration)
+
+    # Test with disabled controller
+    mock_params.bool_data['VibePersonalityEnabled'] = False
+    limits = controller.get_accel_limits(10.0)
+    assert limits is None
+
+  def test_follow_distance_multiplier(self, controller, mock_params):
+    """Test following distance multiplier calculation"""
+    # Enable controller
+    mock_params.bool_data['VibePersonalityEnabled'] = True
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
+
+    # Test with different speeds and personalities
+    controller.long_personality = LongPersonality.relaxed
+
+    multiplier = controller.get_follow_distance_multiplier(15.0)  # 15 m/s
+    assert multiplier is not None
+    assert isinstance(multiplier, float)
+    assert multiplier > 0
+
+    # Test with different personality - aggressive should have shorter distance
+    controller.long_personality = LongPersonality.aggressive
+    aggressive_multiplier = controller.get_follow_distance_multiplier(15.0)
+    assert aggressive_multiplier is not None
+    assert aggressive_multiplier < multiplier  # Aggressive should have shorter distance
+
+    # Test with disabled controller
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = False
+    multiplier = controller.get_follow_distance_multiplier(15.0)
+    assert multiplier is None
+
+  def test_personality_differences(self, controller, mock_params):
+    """Test that different personalities actually produce different values"""
+    # Enable controller
+    mock_params.bool_data['VibePersonalityEnabled'] = True
+    mock_params.bool_data['VibeAccelPersonalityEnabled'] = True
+    mock_params.bool_data['VibeFollowPersonalityEnabled'] = True
+
+    # Test acceleration differences - sport should have higher max acceleration than eco
+    controller.accel_personality = AccelPersonality.eco
+    eco_limits = controller.get_accel_limits(20.0)
+
+    controller.accel_personality = AccelPersonality.sport
+    sport_limits = controller.get_accel_limits(20.0)
+
+    assert sport_limits[1] > eco_limits[1]  # Sport should have higher max acceleration
+
+    # Test following distance differences - relaxed should have longer distance than aggressive
+    controller.long_personality = LongPersonality.relaxed
+    relaxed_dist = controller.get_follow_distance_multiplier(20.0)
+
+    controller.long_personality = LongPersonality.aggressive
+    aggressive_dist = controller.get_follow_distance_multiplier(20.0)
+
+    assert relaxed_dist > aggressive_dist  # Relaxed should have longer following distance
+
+  def test_reset(self, controller):
+    """Test reset functionality"""
+    # Change some values
+    controller.accel_personality = AccelPersonality.sport
+    controller.long_personality = LongPersonality.relaxed
+    controller.frame = 100
+
+    # Reset
+    controller.reset()
+
+    # Check defaults are restored
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.long_personality == LongPersonality.standard
+    assert controller.frame == 0
+
+  def test_edge_cases(self, controller, mock_params):
+    """Test edge cases and error handling"""
+    # Enable all features
+    mock_params.bool_data = {
+      'VibePersonalityEnabled': True,
+      'VibeAccelPersonalityEnabled': True,
+      'VibeFollowPersonalityEnabled': True
+    }
+
+    # Test with zero speed
+    limits = controller.get_accel_limits(0.0)
+    assert limits is not None
+
+    multiplier = controller.get_follow_distance_multiplier(0.0)
+    assert multiplier is not None
+
+    # Test with very high speed
+    limits = controller.get_accel_limits(100.0)
+    assert limits is not None
+
+    multiplier = controller.get_follow_distance_multiplier(100.0)
+    assert multiplier is not None
+
+    # Test interpolation works correctly
+    low_speed_limits = controller.get_accel_limits(5.0)
+    high_speed_limits = controller.get_accel_limits(50.0)
+    assert low_speed_limits[1] > high_speed_limits[1]  # Max accel should decrease with speed

sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py

@@ -0,0 +1,144 @@
+"""
+Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+from cereal import log, custom
+import numpy as np
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+LongPersonality = log.LongitudinalPersonality
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+# Acceleration Profiles mapped to AccelPersonality (eco/normal/sport)
+MAX_ACCEL_PROFILES = {
+    AccelPersonality.eco:       [2.0,  1.99,  1.88, 1.10, .500, .292, .15, .10],   # eco
+    AccelPersonality.normal:    [2.0,  2.00,  1.94, 1.22, .635, .33, .22, .16],   # normal
+    AccelPersonality.sport:     [2.0,  2.00,  2.00, 1.85, .800, .54, .32, .22],    # sport
+}
+MAX_ACCEL_BREAKPOINTS =         [0.,   4.,   6.,   9.,   16.,  25.,  30., 55.]
+
+# Braking profiles mapped to LongPersonality (relaxed/standard/aggressive)
+MIN_ACCEL_PROFILES = {
+    LongPersonality.relaxed:    [-.0006, -.0006, -.010,  -.30,  -1.20],  # gentler braking
+    LongPersonality.standard:   [-.0007, -.0007, -.012,  -.35,  -1.20],  # normal braking
+    LongPersonality.aggressive: [-.0020, -.0008, -.014,  -.40,  -1.20],  # more aggressive braking
+}
+MIN_ACCEL_BREAKPOINTS =         [0.,   3.0,    11.,    14,  50.]
+
+# Follow distance profiles mapped to LongPersonality (relaxed/standard/aggressive)
+FOLLOW_PROFILES = {
+    LongPersonality.relaxed:    [1.55, 1.65, 1.65, 1.80],  # more spread out
+    LongPersonality.standard:   [1.45, 1.45, 1.45, 1.55],  # balanced
+    LongPersonality.aggressive: [1.20, 1.25, 1.28, 1.35],  # tighter
+}
+FOLLOW_BREAKPOINTS =            [0.,   6.,   18.,  36.]
+
+
+class VibePersonalityController:
+    """Controller for acceleration and distance personalities"""
+
+    def __init__(self):
+        self.params = Params()
+        self.frame = 0
+        self.accel_personality = AccelPersonality.normal
+        self.long_personality = LongPersonality.standard
+        self.param_keys = {
+            'accel_personality': 'AccelPersonality',
+            'long_personality': 'LongitudinalPersonality',
+            'enabled': 'VibePersonalityEnabled',
+            'accel_enabled': 'VibeAccelPersonalityEnabled',
+            'follow_enabled': 'VibeFollowPersonalityEnabled'
+        }
+
+    def _update_from_params(self):
+        """Update personalities from params"""
+        if self.frame % int(1. / DT_MDL) != 0:
+            return
+
+        accel_personality_int = int(self.params.get(self.param_keys['accel_personality']))
+        self.accel_personality = accel_personality_int
+
+        long_personality_int = int(self.params.get(self.param_keys['long_personality']))
+        self.long_personality = long_personality_int
+
+    def _get_toggle_state(self, key: str) -> bool:
+        return self.params.get_bool(self.param_keys[key])
+
+    def _set_toggle_state(self, key: str, value: bool):
+        self.params.put_bool(self.param_keys[key], value)
+
+    def set_accel_personality(self, personality: int) -> bool:
+        self.accel_personality = personality
+        self.params.put(self.param_keys['accel_personality'], str(personality))
+        return True
+
+    def cycle_accel_personality(self) -> int:
+        personalities = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]
+        current_idx = personalities.index(self.accel_personality)
+        next_personality = personalities[(current_idx + 1) % len(personalities)]
+        self.set_accel_personality(next_personality)
+        return int(next_personality)
+
+    def get_accel_personality(self) -> int:
+        self._update_from_params()
+        return int(self.accel_personality)
+
+    def set_long_personality(self, personality: int) -> bool:
+        self.long_personality = personality
+        self.params.put(self.param_keys['long_personality'], str(personality))
+        return True
+
+    def cycle_long_personality(self) -> int:
+        personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]
+        current_idx = personalities.index(self.long_personality)
+        next_personality = personalities[(current_idx + 1) % len(personalities)]
+        self.set_long_personality(next_personality)
+        return int(next_personality)
+
+    def get_long_personality(self) -> int:
+        self._update_from_params()
+        return int(self.long_personality)
+
+    def toggle_personality(self): return self._toggle_flag('enabled')
+    def toggle_accel_personality(self): return self._toggle_flag('accel_enabled')
+    def toggle_follow_distance_personality(self): return self._toggle_flag('follow_enabled')
+
+    def _toggle_flag(self, key):
+        current = self._get_toggle_state(key)
+        self._set_toggle_state(key, not current)
+        return not current
+
+    def set_personality_enabled(self, enabled: bool): self._set_toggle_state('enabled', enabled)
+    def is_accel_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('accel_enabled')
+    def is_follow_enabled(self) -> bool: return self._get_toggle_state('enabled') and self._get_toggle_state('follow_enabled')
+    def is_enabled(self) -> bool: return self._get_toggle_state('enabled') and (self._get_toggle_state('accel_enabled') or self._get_toggle_state('follow_enabled'))
+
+    def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
+        """Get acceleration limits based on current personalities."""
+        self._update_from_params()
+        max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
+        min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.long_personality])
+        return float(min_a), float(max_a)
+
+    def get_follow_distance_multiplier(self, v_ego: float) -> float:
+        """Get dynamic following distance based on speed and personality"""
+        self._update_from_params()
+        return float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.long_personality]))
+
+    def get_min_accel(self, v_ego: float) -> float:
+        return self.get_accel_limits(v_ego)[0]
+
+    def get_max_accel(self, v_ego: float) -> float:
+        return self.get_accel_limits(v_ego)[1]
+
+    def reset(self):
+        self.accel_personality = AccelPersonality.normal
+        self.long_personality = LongPersonality.standard
+        self.frame = 0
+
+    def update(self):
+        self.frame += 1