diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5c0a004fa6..d42db9dfc4 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params_keys.h b/common/params_keys.h index 6a5c4cb8e8..a3063cc0d2 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -133,6 +133,8 @@ inline static std::unordered_map keys = { {"Version", {PERSISTENT, STRING}}, // --- sunnypilot params --- // + {"AccelPersonality", {PERSISTENT | BACKUP, INT, std::to_string(static_cast(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 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"}}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3f9d8245bd..c9897500a5 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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), diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0501f669f1..566204cd4a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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: diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py new file mode 100644 index 0000000000..f35f687ef2 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py new file mode 100644 index 0000000000..2b697cc8af --- /dev/null +++ b/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 \ No newline at end of file diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 68b50c3e19..08f7274186 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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') diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/__init__.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/tests/test_vibe.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/tests/test_vibe.py new file mode 100644 index 0000000000..557d352531 --- /dev/null +++ b/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 diff --git a/sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py b/sunnypilot/selfdrive/controls/lib/vibe_personality/vibe_personality.py new file mode 100644 index 0000000000..eeb3c87ca7 --- /dev/null +++ b/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