sunnylink desc

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;

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, "1"}},
     {"ApiCache_DriveStats", {PERSISTENT, JSON}},
     {"AutoLaneChangeBsmDelay", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"AutoLaneChangeTimer", {PERSISTENT | BACKUP, INT, "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
+
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -228,6 +230,7 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
+    self.accel_controller = AccelPersonalityController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -332,6 +335,13 @@ class LongitudinalMpc:
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
+
+    if self.accel_controller.is_enabled():
+      min_accel = self.accel_controller.get_min_accel(v_ego)
+    else:
+      min_accel = CRUISE_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 +360,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,11 @@ 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)
+        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 bool(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/accel_personality/tests/test_accel_controller.py

@@ -0,0 +1,286 @@
+"""
+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.
+"""
+
+import pytest
+import numpy as np
+from cereal import custom
+from sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import (
+  AccelPersonalityController,
+  MAX_ACCEL_PROFILES,
+  MIN_ACCEL_PROFILES,
+  MAX_ACCEL_BREAKPOINTS,
+  MIN_ACCEL_BREAKPOINTS,
+)
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+
+class TestAccelPersonalityController:
+
+  @pytest.fixture
+  def mock_params(self, mocker):
+    params = mocker.Mock()
+    params.get = mocker.Mock(return_value=None)
+    params.get_bool = mocker.Mock(return_value=False)
+    params.put = mocker.Mock()
+    params.put_bool = mocker.Mock()
+    return params
+
+  @pytest.fixture
+  def controller(self, mock_params, mocker):
+    mocker.patch('sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller.Params', return_value=mock_params)
+    ctrl = AccelPersonalityController()
+    ctrl.params = mock_params
+    return ctrl
+
+  def test_initialization_defaults(self, controller):
+    assert controller.frame == 0
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.param_keys == {
+      'personality': 'AccelPersonality',
+      'enabled': 'AccelPersonalityEnabled'
+    }
+
+  @pytest.mark.parametrize("personality,expected", [
+    (AccelPersonality.eco, AccelPersonality.eco),
+    (AccelPersonality.normal, AccelPersonality.normal),
+    (AccelPersonality.sport, AccelPersonality.sport),
+  ])
+  def test_load_personality_valid(self, mocker, personality, expected):
+    mock_params = mocker.Mock()
+    mock_params.get = mocker.Mock(return_value=str(personality).encode())
+    mock_params.get_bool = mocker.Mock(return_value=False)
+    mocker.patch('sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller.Params', return_value=mock_params)
+    controller = AccelPersonalityController()
+    assert controller.accel_personality == expected
+
+  def test_load_personality_invalid_value(self, mocker):
+    mock_params = mocker.Mock()
+    mock_params.get = mocker.Mock(return_value=b'999')
+    mock_params.get_bool = mocker.Mock(return_value=False)
+    mocker.patch('sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller.Params', return_value=mock_params)
+    controller = AccelPersonalityController()
+    assert controller.accel_personality == AccelPersonality.normal
+
+  def test_load_personality_parse_error(self, mocker):
+    mock_params = mocker.Mock()
+    mock_params.get = mocker.Mock(return_value=b'invalid_data')
+    mock_params.get_bool = mocker.Mock(return_value=False)
+    mocker.patch('sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller.Params', return_value=mock_params)
+    controller = AccelPersonalityController()
+    assert controller.accel_personality == AccelPersonality.normal
+
+  def test_load_personality_none(self, mocker):
+    mock_params = mocker.Mock()
+    mock_params.get = mocker.Mock(return_value=None)
+    mock_params.get_bool = mocker.Mock(return_value=False)
+    mocker.patch('sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller.Params', return_value=mock_params)
+    controller = AccelPersonalityController()
+    assert controller.accel_personality == AccelPersonality.normal
+
+  def test_get_accel_personality(self, controller):
+    controller.accel_personality = AccelPersonality.sport
+    result = controller.get_accel_personality()
+    assert result == AccelPersonality.sport
+    assert isinstance(result, int)
+
+  @pytest.mark.parametrize("personality", [
+    AccelPersonality.eco,
+    AccelPersonality.normal,
+    AccelPersonality.sport,
+  ])
+  def test_set_accel_personality_valid(self, controller, mock_params, personality):
+    controller.set_accel_personality(personality)
+    assert controller.accel_personality == personality
+    mock_params.put.assert_called_once_with('AccelPersonality', str(personality))
+
+  def test_set_accel_personality_invalid(self, controller, mock_params):
+    original = controller.accel_personality
+    controller.set_accel_personality(999)
+    assert controller.accel_personality == original
+    mock_params.put.assert_not_called()
+
+  def test_cycle_accel_personality_full_cycle(self, controller):
+    controller.accel_personality = AccelPersonality.eco
+
+    result = controller.cycle_accel_personality()
+    assert result == AccelPersonality.normal
+    assert controller.accel_personality == AccelPersonality.normal
+
+    result = controller.cycle_accel_personality()
+    assert result == AccelPersonality.sport
+    assert controller.accel_personality == AccelPersonality.sport
+
+    result = controller.cycle_accel_personality()
+    assert result == AccelPersonality.eco
+    assert controller.accel_personality == AccelPersonality.eco
+
+  def test_cycle_accel_personality_return_type(self, controller):
+    result = controller.cycle_accel_personality()
+    assert isinstance(result, int)
+
+  @pytest.mark.parametrize("personality", [
+    AccelPersonality.eco,
+    AccelPersonality.normal,
+    AccelPersonality.sport,
+  ])
+  def test_get_accel_limits_at_zero_speed(self, controller, personality):
+    controller.accel_personality = personality
+    min_a, max_a = controller.get_accel_limits(0.0)
+
+    expected_max = MAX_ACCEL_PROFILES[personality][0]
+    expected_min = MIN_ACCEL_PROFILES[personality][0]
+
+    assert abs(max_a - expected_max) < 1e-6
+    assert abs(min_a - expected_min) < 1e-6
+
+  @pytest.mark.parametrize("v_ego,personality", [
+    (0.0, AccelPersonality.eco),
+    (10.0, AccelPersonality.normal),
+    (25.0, AccelPersonality.sport),
+    (50.0, AccelPersonality.eco),
+    (4.0, AccelPersonality.normal),
+    (30.0, AccelPersonality.sport),
+  ])
+  def test_get_accel_limits_interpolation(self, controller, v_ego, personality):
+    controller.accel_personality = personality
+    min_a, max_a = controller.get_accel_limits(v_ego)
+
+    expected_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[personality])
+    expected_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[personality])
+
+    assert abs(max_a - expected_max) < 1e-6
+    assert abs(min_a - expected_min) < 1e-6
+
+  def test_get_accel_limits_return_types(self, controller):
+    min_a, max_a = controller.get_accel_limits(10.0)
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+
+  def test_get_min_accel(self, controller):
+    controller.accel_personality = AccelPersonality.sport
+    v_ego = 15.0
+    min_a = controller.get_min_accel(v_ego)
+    expected = controller.get_accel_limits(v_ego)[0]
+    assert min_a == expected
+    assert isinstance(min_a, float)
+
+  def test_get_max_accel(self, controller):
+    controller.accel_personality = AccelPersonality.eco
+    v_ego = 20.0
+    max_a = controller.get_max_accel(v_ego)
+    expected = controller.get_accel_limits(v_ego)[1]
+    assert max_a == expected
+    assert isinstance(max_a, float)
+
+  def test_is_enabled_true(self, controller, mock_params):
+    mock_params.get_bool.return_value = True
+    assert controller.is_enabled() is True
+
+  def test_is_enabled_false(self, controller, mock_params):
+    mock_params.get_bool.return_value = False
+    assert controller.is_enabled() is False
+
+  def test_is_enabled_calls_params(self, controller, mock_params):
+    controller.is_enabled()
+    mock_params.get_bool.assert_called_once_with('AccelPersonalityEnabled')
+
+  @pytest.mark.parametrize("enabled", [True, False])
+  def test_set_enabled(self, controller, mock_params, enabled):
+    controller.set_enabled(enabled)
+    mock_params.put_bool.assert_called_once_with('AccelPersonalityEnabled', enabled)
+
+  def test_toggle_enabled_from_false(self, controller, mock_params):
+    mock_params.get_bool.return_value = False
+    result = controller.toggle_enabled()
+    assert result is True
+    mock_params.put_bool.assert_called_once_with('AccelPersonalityEnabled', True)
+
+  def test_toggle_enabled_from_true(self, controller, mock_params):
+    mock_params.get_bool.return_value = True
+    result = controller.toggle_enabled()
+    assert result is False
+    mock_params.put_bool.assert_called_once_with('AccelPersonalityEnabled', False)
+
+  def test_reset(self, controller):
+    controller.accel_personality = AccelPersonality.sport
+    controller.frame = 100
+    controller.reset()
+    assert controller.accel_personality == AccelPersonality.normal
+    assert controller.frame == 0
+
+  def test_update_increments_frame(self, controller):
+    initial_frame = controller.frame
+    controller.update()
+    assert controller.frame == initial_frame + 1
+
+  def test_update_multiple_calls(self, controller):
+    for i in range(1, 11):
+      controller.update()
+      assert controller.frame == i
+
+  @pytest.mark.parametrize("v_ego", [0, 5, 10, 15, 20, 25, 30, 40, 50, 55])
+  @pytest.mark.parametrize("personality", [
+    AccelPersonality.eco,
+    AccelPersonality.normal,
+    AccelPersonality.sport,
+  ])
+  def test_accel_limits_physical_constraints(self, controller, v_ego, personality):
+    controller.accel_personality = personality
+    min_a, max_a = controller.get_accel_limits(v_ego)
+    assert min_a < 0
+    assert max_a > 0
+    assert min_a < max_a
+
+  def test_max_accel_decreases_with_speed(self, controller):
+    test_speeds = [0, 10, 20, 30, 40, 50]
+    for personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]:
+      controller.accel_personality = personality
+      max_accels = [controller.get_max_accel(v) for v in test_speeds]
+      assert max_accels[-1] < max_accels[0]
+
+  @pytest.mark.parametrize("v_ego", [5, 10, 15, 20, 25])
+  def test_acceleration_personality_ordering(self, controller, v_ego):
+    controller.accel_personality = AccelPersonality.eco
+    _, eco_max = controller.get_accel_limits(v_ego)
+
+    controller.accel_personality = AccelPersonality.normal
+    _, normal_max = controller.get_accel_limits(v_ego)
+
+    controller.accel_personality = AccelPersonality.sport
+    _, sport_max = controller.get_accel_limits(v_ego)
+
+    assert sport_max >= normal_max
+    assert normal_max >= eco_max
+
+  @pytest.mark.parametrize("v_ego", [5, 10, 15, 20])
+  def test_braking_personality_ordering(self, controller, v_ego):
+    controller.accel_personality = AccelPersonality.eco
+    eco_min, _ = controller.get_accel_limits(v_ego)
+
+    controller.accel_personality = AccelPersonality.sport
+    sport_min, _ = controller.get_accel_limits(v_ego)
+
+    assert sport_min <= eco_min
+
+  def test_accel_limits_at_max_speed(self, controller):
+    max_speed = MAX_ACCEL_BREAKPOINTS[-1]
+    min_a, max_a = controller.get_accel_limits(max_speed)
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+
+  def test_accel_limits_beyond_max_speed(self, controller):
+    beyond_max = MAX_ACCEL_BREAKPOINTS[-1] + 10
+    min_a, max_a = controller.get_accel_limits(beyond_max)
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+
+  def test_accel_limits_very_low_speed(self, controller):
+    min_a, max_a = controller.get_accel_limits(0.5)
+    assert isinstance(min_a, float)
+    assert isinstance(max_a, float)
+    assert min_a >= -2.0

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/sunnylink/params_metadata.json

@@ -1,4 +1,26 @@
 {
+  "AccelPersonality": {
+    "title": "Acceleration Personality",
+    "description": "",
+    "options": [
+      {
+        "value": 0,
+        "label": "Sport"
+      },
+      {
+        "value": 1,
+        "label": "Normal"
+      },
+      {
+        "value": 2,
+        "label": "Eco"
+      }
+    ]
+  },
+  "AccelPersonalityEnabled": {
+    "title": "Enable Acceleration Personality",
+    "description": "Controls acceleration behavior: Eco (efficient), Normal (balanced), Sport (responsive)."
+  },
   "AccessToken": {
     "title": "AccessTokenIsNice",
     "description": ""