long: accel control

openpilot/cereal/custom.capnp

@@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   aTarget @5 :Float32;
   events @6 :List(OnroadEventSP.Event);
   e2eAlerts @7 :E2eAlerts;
+  accelController @8 :AccelController;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -305,6 +306,18 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
     greenLightAlert @0 :Bool;
     leadDepartAlert @1 :Bool;
   }
+
+  struct AccelController {
+    enabled @0 :Bool;
+    active @1 :Bool;
+    profile @2 :Profile;
+    reserved3 @3 :Void;
+    enum Profile {
+      eco @0;
+      normal @1;
+      sport @2;
+    }
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

openpilot/common/params_keys.h

@@ -241,6 +241,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
 
+    // Accel Controller profiles (Eco / Normal / Sport)
+    {"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
+
     // sunnypilot model params
     {"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
     {"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},

openpilot/common/tests/test_params.py

@@ -117,12 +117,16 @@ class TestParams(OpenpilotTestCase):
   def test_params_default_value(self):
     self.params.remove("LanguageSetting")
     self.params.remove("LongitudinalPersonality")
+    self.params.remove("AccelPersonalityEnabled")
+    self.params.remove("AccelPersonality")
     self.params.remove("LiveParametersV2")
 
     assert self.params.get("LanguageSetting") is None
     assert self.params.get("LanguageSetting", return_default=False) is None
     assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
     assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
+    assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
+    assert self.params.get("AccelPersonality", return_default=True) == 1
     assert self.params.get("LiveParametersV2") is None
     assert self.params.get("LiveParametersV2", return_default=True) is None
 

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -35,8 +35,13 @@ def get_max_accel(v_ego):
 def get_coast_accel(pitch):
   return np.sin(pitch) * -5.65 - 0.3  # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
 
-def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
-  max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
+def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle,
+                      max_accel_override=None, min_accel_override=None):
+  if max_accel_override is not None:
+    max_accel = max_accel_override
+  else:
+    max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
+  min_accel = A_CRUISE_MIN if e2e or min_accel_override is None else min_accel_override
 
   if not e2e:
     a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
@@ -48,7 +53,7 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
       coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
       max_accel = min(max_accel, coast_limit)
 
-  target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
+  target_accel = np.clip(v_cruise - v_ego, min_accel, max_accel)
   j_cruise = np.interp(v_ego, A_CRUISE_MAX_BP, J_CRUISE_VALS)
   target_accel = float(np.clip(target_accel, a_cruise_prev - j_cruise * dt, a_cruise_prev + j_cruise * dt))
 
@@ -68,6 +73,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_cruise = init_a
     self.output_a_target = init_a
     self.output_should_stop = False
+    self.accel_controller_active = False
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -84,7 +90,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
-    if sm['controlsState'].forceDecel:
+    force_decel = sm['controlsState'].forceDecel
+    if force_decel:
       v_cruise = 0.0
 
     long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
@@ -140,9 +147,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     is_e2e = self.is_e2e(sm)
 
+    max_accel_override = self.get_max_accel_override(v_ego)
+    min_accel_override = self.get_min_accel_override(v_ego, is_e2e, force_decel)
+    self.accel_controller_active = max_accel_override is not None or min_accel_override is not None
     self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
-                                     accel_coast, self.allow_throttle)
+                                     accel_coast, self.allow_throttle, max_accel_override, min_accel_override)
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),

openpilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -11,6 +11,15 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
 
+class PlannerSM(dict):
+  def __init__(self, radar_frame: int, services: dict):
+    super().__init__(services)
+    self.frame = radar_frame
+    self.logMonoTime = {"radarState": radar_frame}
+    self.valid = {"radarState": True}
+    self.alive = {"radarState": True}
+
+
 class Plant:
   messaging_initialized = False
 
@@ -132,7 +141,7 @@ class Plant:
     car_control.carControl.orientationNED = [0., float(pitch), 0.]
 
     # ******** get controlsState messages for plotting ***
-    sm = {'radarState': radar.radarState,
+    sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
           'carState': car_state.carState,
           'carControl': car_control.carControl,
           'controlsState': control.controlsState,
@@ -141,7 +150,7 @@ class Plant:
           'modelV2': model.modelV2,
           'carStateSP': car_state_sp.carStateSP,
           'liveMapDataSP': live_map_data_sp.liveMapDataSP,
-          'gpsLocation': gps_data.gpsLocation}
+          'gpsLocation': gps_data.gpsLocation})
     self.planner.update(sm)
     self.acceleration = self.planner.output_a_target
     if self.planner.output_should_stop:

openpilot/selfdrive/ui/layouts/settings/toggles.py

@@ -27,6 +27,13 @@ DESCRIPTIONS = {
     "In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
     "your steering wheel distance button."
   ),
+  "AccelPersonalityEnabled": tr_noop(
+    "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain " +
+    "independent of this setting."
+  ),
+  "AccelPersonality": tr_noop(
+    "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
+  ),
   "IsLdwEnabled": tr_noop(
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
     "without a turn signal activated while driving over 31 mph (50 km/h)."
@@ -106,6 +113,24 @@ class TogglesLayout(Widget):
       icon="speed_limit.png"
     )
 
+    self._accel_controller_enabled = toggle_item(
+      lambda: tr("Enable Accel Controller"),
+      lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
+      self._params.get_bool("AccelPersonalityEnabled"),
+      callback=self._set_accel_controller_enabled,
+      icon="speed_limit.png",
+    )
+
+    self._accel_personality_setting = multiple_button_item(
+      lambda: tr("Acceleration Profile"),
+      lambda: tr(DESCRIPTIONS["AccelPersonality"]),
+      buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
+      button_width=300,
+      callback=self._set_accel_personality,
+      selected_index=self._params.get("AccelPersonality", return_default=True),
+      icon="speed_limit.png"
+    )
+
     self._toggles = {}
     self._locked_toggles = set()
     for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
@@ -135,9 +160,11 @@ class TogglesLayout(Widget):
 
       self._toggles[param] = toggle
 
-      # insert longitudinal personality after NDOG toggle
+      # insert longitudinal personality and Accel Controller settings after NDOG toggle
       if param == "DisengageOnAccelerator":
         self._toggles["LongitudinalPersonality"] = self._long_personality_setting
+        self._toggles["AccelPersonalityEnabled"] = self._accel_controller_enabled
+        self._toggles["AccelPersonality"] = self._accel_personality_setting
 
     self._update_experimental_mode_icon()
     self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
@@ -158,6 +185,7 @@ class TogglesLayout(Widget):
 
   def _update_toggles(self):
     ui_state.update_params()
+    accel_controller_enabled = self._params.get_bool("AccelPersonalityEnabled")
 
     e2e_description = tr(
       "sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
@@ -176,11 +204,15 @@ class TogglesLayout(Widget):
         self._toggles["ExperimentalMode"].action_item.set_enabled(True)
         self._toggles["ExperimentalMode"].set_description(e2e_description)
         self._long_personality_setting.action_item.set_enabled(True)
+        self._accel_controller_enabled.action_item.set_enabled(True)
+        self._accel_personality_setting.action_item.set_enabled(True)
       else:
         # no long for now
         self._toggles["ExperimentalMode"].action_item.set_enabled(False)
         self._toggles["ExperimentalMode"].action_item.set_state(False)
         self._long_personality_setting.action_item.set_enabled(False)
+        self._accel_controller_enabled.action_item.set_enabled(False)
+        self._accel_personality_setting.action_item.set_enabled(False)
         self._params.remove("ExperimentalMode")
 
         unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
@@ -203,6 +235,8 @@ class TogglesLayout(Widget):
     # refresh toggles from params to mirror external changes
     for param in self._toggle_defs:
       self._toggles[param].action_item.set_state(self._params.get_bool(param))
+    self._accel_controller_enabled.action_item.set_state(accel_controller_enabled)
+    self._accel_personality_setting.action_item.set_selected_button(self._params.get("AccelPersonality", return_default=True))
 
     # these toggles need restart, block while engaged
     for toggle_def in self._toggle_defs:
@@ -247,3 +281,9 @@ class TogglesLayout(Widget):
 
   def _set_longitudinal_personality(self, button_index: int):
     self._params.put("LongitudinalPersonality", button_index, block=True)
+
+  def _set_accel_personality(self, button_index: int):
+    self._params.put("AccelPersonality", button_index, block=True)
+
+  def _set_accel_controller_enabled(self, state: bool):
+    self._params.put_bool("AccelPersonalityEnabled", state, block=True)

openpilot/selfdrive/ui/mici/layouts/settings/toggles.py

@@ -42,6 +42,8 @@ class TogglesLayoutMici(NavScroller):
     super().__init__()
 
     self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
+    self._accel_controller_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
+    self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
     self._experimental_btn = BigToggle("experimental mode", initial_state=ui_state.params.get_bool("ExperimentalMode"),
                                        toggle_callback=self._on_experimental_mode)
     is_metric_toggle = BigParamControl("use metric units", "IsMetric")
@@ -53,6 +55,8 @@ class TogglesLayoutMici(NavScroller):
 
     self._scroller.add_widgets([
       self._personality_toggle,
+      self._accel_controller_enabled,
+      self._accel_personality_toggle,
       self._experimental_btn,
       is_metric_toggle,
       ldw_toggle,
@@ -65,6 +69,7 @@ class TogglesLayoutMici(NavScroller):
     # Toggle lists
     self._refresh_toggles = (
       ("ExperimentalMode", self._experimental_btn),
+      ("AccelPersonalityEnabled", self._accel_controller_enabled),
       ("IsMetric", is_metric_toggle),
       ("IsLdwEnabled", ldw_toggle),
       ("AlwaysOnDM", always_on_dm_toggle),
@@ -104,17 +109,23 @@ class TogglesLayoutMici(NavScroller):
       if ui_state.has_longitudinal_control:
         self._experimental_btn.set_visible(True)
         self._personality_toggle.set_visible(True)
+        self._accel_controller_enabled.set_visible(True)
+        self._accel_personality_toggle.set_visible(True)
       else:
         # no long for now
         self._experimental_btn.set_visible(False)
         self._experimental_btn.set_checked(False)
         self._personality_toggle.set_visible(False)
+        self._accel_controller_enabled.set_visible(False)
+        self._accel_personality_toggle.set_visible(False)
         ui_state.params.remove("ExperimentalMode")
 
     # Refresh toggles from params to mirror external changes
     for key, item in self._refresh_toggles:
       item.set_checked(ui_state.params.get_bool(key))
 
+    self._accel_personality_toggle.refresh()
+
   def _on_experimental_mode(self, state: bool):
     if state and not ui_state.params.get_bool("ExperimentalModeConfirmed"):
       # Don't show enabled state until confirm

openpilot/selfdrive/ui/mici/widgets/button.py

@@ -385,13 +385,18 @@ class BigMultiParamToggle(BigMultiToggle):
     self._load_value()
 
   def _load_value(self):
-    self.set_value(self._options[self._params.get(self._param) or 0])
+    value = self._params.get(self._param, return_default=True)
+    index = value if isinstance(value, int) else 0
+    self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
 
   def _handle_mouse_release(self, mouse_pos: MousePos):
     super()._handle_mouse_release(mouse_pos)
     new_idx = self._options.index(self.value)
     self._params.put(self._param, new_idx)
 
+  def refresh(self):
+    self._load_value()
+
 
 class BigParamControl(BigToggle):
   def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):

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

@@ -0,0 +1,74 @@
+"""
+Copyright (c) 2021-, 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 numpy as np
+
+from openpilot.cereal import custom
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot import get_sanitize_int_param
+
+AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
+
+MAX_ACCEL_PROFILES = {
+  AccelProfile.eco:    [1.45, 1.40, 1.20, 0.85, 0.62, 0.36, 0.22, 0.085, 0.055, 0.045],
+  AccelProfile.normal: [2.00, 1.95, 1.80, 1.06, 0.81, 0.69, 0.42, 0.160, 0.10, 0.08],
+  AccelProfile.sport:  [2.00, 1.99, 1.95, 1.45, 1.10, 0.82, 0.53, 0.240, 0.13, 0.09],
+}
+MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 12., 18., 24., 32., 42., 55.]
+
+MIN_ACCEL_PROFILES = {
+  AccelProfile.eco:    [-0.90, -0.95, -1.00, -1.10, -1.2],
+  AccelProfile.normal: [-1.00, -1.05, -1.10, -1.20, -1.3],
+  AccelProfile.sport:  [-1.10, -1.15, -1.20, -1.30, -1.4],
+}
+MIN_ACCEL_BREAKPOINTS = [3., 4.5, 7., 9., 25.]
+
+ACCEL_SMOOTH_ALPHA = 0.90
+DECEL_SMOOTH_ALPHA = 0.40
+
+class AccelController:
+  def __init__(self):
+    self.params = Params()
+    self.frame = 0
+    self.last_max_accel = 2.0
+    self.last_min_accel = -0.01
+    self.first_run = True
+    self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+    self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+
+  def update(self, sm=None) -> None:
+    self.frame += 1
+    if self.frame % int(1.0 / DT_MDL) == 0:
+      self._profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
+      self._enabled = self.params.get_bool("AccelPersonalityEnabled")
+
+  @property
+  def profile(self) -> int:
+    return self._profile
+
+  def is_enabled(self) -> bool:
+    return self._enabled
+
+  def get_max_accel(self, v_ego: float) -> float:
+    v_ego = max(0.0, v_ego)
+    target_max = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self._profile])
+
+    if self.first_run:
+      self.last_max_accel = target_max
+      self.first_run = False
+      return float(target_max)
+
+    self.last_max_accel = ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel
+    return float(self.last_max_accel)
+
+  def get_min_accel(self, v_ego: float) -> float:
+    v_ego = max(0.0, v_ego)
+    target_min = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self._profile])
+    self.last_min_accel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel
+    self.last_min_accel = min(self.last_min_accel, self.last_max_accel - 0.1)
+    return float(self.last_min_accel)

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -0,0 +1,213 @@
+"""
+Copyright (c) 2021-, 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.
+
+Scope is deliberately narrow: a v_ego-keyed acceleration ceiling and cruise-deceleration
+floor per profile. The controller does not modify lead following distance or the MPC lead
+candidate. The floor only ever softens the no-lead cruise candidate (slowing for a lower
+cruise speed, a curve, or a speed limit); it is excluded during forceDecel and e2e, and
+min() against the untouched MPC candidate means a real lead can always still force full
+ACCEL_MIN braking.
+
+Ceiling vs floor apply on different policies: ACC (non-e2e) uses the controller's ceiling
+and floor; blended (e2e) uses the controller's ceiling but always the stock floor
+(A_CRUISE_MIN).
+"""
+import unittest
+
+import numpy as np
+
+from openpilot.common.params import Params
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
+  AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, MIN_ACCEL_PROFILES,
+)
+
+
+class TestAccelControllerCeiling(OpenpilotTestCase):
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    self.params.put("AccelPersonality", AccelProfile.normal, block=True)
+    self.controller = AccelController()
+
+  def test_first_call_snaps_to_table_with_no_smoothing_lag(self):
+    max_a = self.controller.get_max_accel(20.0)
+    expected_max = np.interp(20.0, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[AccelProfile.normal])
+    self.assertAlmostEqual(max_a, expected_max, places=6)
+
+  def test_table_lookup_matches_breakpoints_per_profile(self):
+    for profile, table in MAX_ACCEL_PROFILES.items():
+      self.params.put("AccelPersonality", profile, block=True)
+      controller = AccelController()
+      for v_ego, expected in zip(MAX_ACCEL_BREAKPOINTS, table, strict=True):
+        controller.first_run = True
+        max_a = controller.get_max_accel(v_ego)
+        self.assertAlmostEqual(max_a, expected, places=3)
+
+  def test_smoothing_moves_gradually_not_instantly_on_profile_switch(self):
+    v_ego = 8.0  # breakpoint where eco/normal/sport ceilings differ
+    self.controller.get_max_accel(v_ego)  # settle first_run on normal
+    start = self.controller.last_max_accel
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    self.controller.frame = int(1.0 / DT_MDL) - 1  # force the 1s refresh boundary on next update()
+    self.controller.update()
+    max_a = self.controller.get_max_accel(v_ego)
+    target = MAX_ACCEL_PROFILES[AccelProfile.sport][MAX_ACCEL_BREAKPOINTS.index(v_ego)]
+    self.assertNotEqual(start, target)
+    self.assertGreater(max_a, start)
+    self.assertLess(max_a, target)
+
+  def test_eco_is_selectable_not_treated_as_falsy(self):
+    self.params.put("AccelPersonality", AccelProfile.eco, block=True)
+    controller = AccelController()
+    self.assertEqual(controller.profile, AccelProfile.eco)
+    max_a = controller.get_max_accel(0.0)
+    self.assertAlmostEqual(max_a, MAX_ACCEL_PROFILES[AccelProfile.eco][0], places=3)
+
+  def test_min_accel_never_stronger_than_stock_a_cruise_min(self):
+    for v_ego in [0., 3., 4.5, 7., 9., 15., 25., 40.]:
+      for _ in range(60):
+        min_a = self.controller.get_min_accel(v_ego)
+      self.assertGreaterEqual(min_a, -1.4)  # softer or equal to the softest stock-adjacent floor, never harsher
+      self.assertLess(min_a, 0.0)
+
+  def test_min_accel_ramps_to_stock_strength_by_highway_speed(self):
+    for _ in range(200):
+      min_a = self.controller.get_min_accel(25.0)
+    self.assertAlmostEqual(min_a, MIN_ACCEL_PROFILES[AccelProfile.normal][-1], places=2)
+
+  def test_min_accel_profile_ordering_eco_softest_sport_strongest(self):
+    settled = {}
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      self.params.put("AccelPersonality", profile, block=True)
+      controller = AccelController()
+      for _ in range(60):
+        settled[profile] = controller.get_min_accel(4.5)
+    self.assertGreater(settled[AccelProfile.eco], settled[AccelProfile.normal])
+    self.assertGreater(settled[AccelProfile.normal], settled[AccelProfile.sport])
+
+  def test_min_accel_never_inverts_above_max_accel(self):
+    # Both feed the same np.clip call in get_cruise_accel -- independent smoothing must
+    # never let the floor drift above the ceiling.
+    for v_ego in [0., 3., 8., 20., 45.]:
+      max_a = self.controller.get_max_accel(v_ego)
+      min_a = self.controller.get_min_accel(v_ego)
+      self.assertLessEqual(min_a, max_a - 0.05)
+
+  def test_params_refresh_only_at_one_second_boundary(self):
+    self.controller.frame = 0
+    self.params.put("AccelPersonality", AccelProfile.sport, block=True)
+    self.controller.update()  # frame=1, not a boundary
+    self.assertEqual(self.controller.profile, AccelProfile.normal)
+    self.controller.frame = int(1.0 / DT_MDL) - 1
+    self.controller.update()  # crosses the boundary
+    self.assertEqual(self.controller.profile, AccelProfile.sport)
+
+  def test_enabled_reflects_params(self):
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    controller = AccelController()
+    self.assertFalse(controller.is_enabled())
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    controller.frame = int(1.0 / DT_MDL) - 1
+    controller.update()
+    self.assertTrue(controller.is_enabled())
+
+  def test_max_accel_never_exceeds_profile_ceiling(self):
+    for v_ego in [0., 5., 10., 20., 30., 45., 60.]:
+      max_a = self.controller.get_max_accel(v_ego)
+      table_max = max(max(table) for table in MAX_ACCEL_PROFILES.values())
+      self.assertLessEqual(max_a, table_max + 1e-6)
+
+
+class TestOffEqualsStock(OpenpilotTestCase):
+  def setUp(self):
+    self.params = Params()
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+
+  def test_disabled_controller_is_enabled_returns_false(self):
+    controller = AccelController()
+    self.assertFalse(controller.is_enabled())
+
+  def test_get_cruise_accel_with_none_override_matches_no_kwarg(self):
+    from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel
+    args = (False, 10.0, 8.0, 0.5, 0.0, _fake_cp(), DT_MDL, 1.0, True)
+    self.assertEqual(get_cruise_accel(*args), get_cruise_accel(*args, max_accel_override=None, min_accel_override=None))
+
+  def test_disabled_min_accel_override_is_none(self):
+    planner = _bare_planner()
+    self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=False))
+
+  def test_disabled_max_accel_override_is_none(self):
+    planner = _bare_planner()
+    self.assertIsNone(planner.get_max_accel_override(v_ego=5.0))
+
+  def test_force_decel_excludes_min_accel_override_even_when_enabled(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=True))
+
+  def test_e2e_excludes_min_accel_override_even_when_enabled(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    self.assertIsNone(planner.get_min_accel_override(v_ego=5.0, e2e=True, force_decel=False))
+
+  def test_enabled_min_accel_override_returns_a_float(self):
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    override = planner.get_min_accel_override(v_ego=5.0, e2e=False, force_decel=False)
+    self.assertIsNotNone(override)
+    self.assertLess(override, 0.0)
+
+  def test_enabled_max_accel_override_applies_in_acc_and_blended(self):
+    # Policy: max ceiling comes from AccelController in both ACC and blended (e2e) modes --
+    # only the min floor is blended-vs-stock. get_max_accel_override no longer takes an e2e
+    # arg because of this; the caller applies it unconditionally.
+    self.params.put_bool("AccelPersonalityEnabled", True, block=True)
+    planner = _bare_planner()
+    override = planner.get_max_accel_override(v_ego=5.0)
+    self.assertIsNotNone(override)
+    self.assertGreater(override, 0.0)
+
+  def test_blended_min_accel_uses_stock_not_controller(self):
+    # e2e/blended braking floor is deliberately left at stock's A_CRUISE_MIN, never the
+    # controller's floor -- this is the "acc policy = controller min+max, blended policy =
+    # controller max + stock min" split, final per product decision.
+    # jerk-limiting now applies unconditionally (even in e2e, per upstream's decel-jerk fix), so
+    # dt=10.0 opens the jerk-limit window wide enough that it can't mask the floor/ceiling asserted here.
+    from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel, A_CRUISE_MIN
+    args = {"v_cruise": -100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(),
+            "dt": 10.0, "accel_coast": 1.0, "allow_throttle": True}
+    target = get_cruise_accel(True, **args, min_accel_override=-0.3)
+    self.assertAlmostEqual(target, A_CRUISE_MIN, places=6)
+
+  def test_blended_max_accel_uses_controller_override(self):
+    # jerk-limiting now applies unconditionally (even in e2e) -- dt=10.0 opens the jerk-limit
+    # window wide enough that it can't mask the override ceiling asserted here.
+    from openpilot.selfdrive.controls.lib.longitudinal_planner import get_cruise_accel
+    args = {"v_cruise": 100.0, "v_ego": 20.0, "a_cruise_prev": 0.0, "angle_steers": 0.0, "CP": _fake_cp(),
+            "dt": 10.0, "accel_coast": 1.0, "allow_throttle": True}
+    target = get_cruise_accel(True, **args, max_accel_override=0.4)
+    self.assertAlmostEqual(target, 0.4, places=6)
+
+
+
+def _fake_cp():
+  class _CP:
+    steerRatio = 15.0
+    wheelbase = 2.7
+  return _CP()
+
+
+def _bare_planner():
+  from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
+  planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = AccelController()
+  return planner
+
+
+if __name__ == "__main__":
+  unittest.main()

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,6 +9,7 @@ from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
@@ -23,8 +24,8 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
+    self.accel_controller = AccelController()
     self.events_sp = EventsSP()
-    self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
@@ -43,6 +44,16 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def get_max_accel_override(self, v_ego: float) -> float | None:
+    if not self.accel_controller.is_enabled():
+      return None
+    return self.accel_controller.get_max_accel(v_ego)
+
+  def get_min_accel_override(self, v_ego: float, e2e: bool, force_decel: bool) -> float | None:
+    if e2e or force_decel or not self.accel_controller.is_enabled():
+      return None
+    return self.accel_controller.get_min_accel(v_ego)
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
@@ -74,6 +85,7 @@ class LongitudinalPlannerSP:
     return self.output_v_target, self.output_a_target
 
   def update(self, sm: messaging.SubMaster) -> None:
+    self.accel_controller.update(sm)
     self.events_sp.clear()
     self.dec.update(sm)
     self.e2e_alerts_helper.update(sm, self.events_sp)
@@ -95,6 +107,11 @@ class LongitudinalPlannerSP:
     dec.enabled = self.dec.enabled()
     dec.active = self.dec.active()
 
+    accel_controller = longitudinalPlanSP.accelController
+    accel_controller.enabled = self.accel_controller.is_enabled()
+    accel_controller.active = self.accel_controller_active
+    accel_controller.profile = self.accel_controller.profile
+
     # Smart Cruise Control
     smartCruiseControl = longitudinalPlanSP.smartCruiseControl
     # Vision Control

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_longcontrol_sp.py

@@ -613,8 +613,7 @@ class TestLongControlSP(OpenpilotTestCase):
       run_long_control=True,
       actuator_model=PRIUS_TSS2_ROUTE_MODEL,
     )
-    plant.planner.accel_controller.enabled = True
-    plant.planner.accel_controller.profile = 1
+    plant.planner.accel_controller._enabled = True
     plant.planner.dec._enabled = False
     commands = []
     speeds = []
@@ -622,7 +621,7 @@ class TestLongControlSP(OpenpilotTestCase):
     solver_statuses = []
 
     with (
-      mock.patch.object(plant.planner.accel_controller, "update_params", return_value=None),
+      mock.patch.object(plant.planner.accel_controller, "update", return_value=None),
       mock.patch.object(plant.planner.dec, "_read_params", return_value=None),
     ):
       while plant.current_time < 5.0:

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py

@@ -0,0 +1,394 @@
+"""
+Copyright (c) 2021-, 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 collections import deque
+from collections.abc import Callable
+from dataclasses import dataclass
+import math
+import time
+from typing import Any
+
+import numpy as np
+
+from openpilot.cereal import log, messaging
+from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
+from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
+
+
+LeadObservation = dict[str, Any]
+LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
+ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
+EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
+
+
+@dataclass(frozen=True)
+class ActuatorModel:
+  planner_delay: float
+  transport_delay: float
+  actuator_lag: float
+  command_rate_limit: float
+  stopping_acceleration: float
+  standstill_breakaway_acceleration: float
+  standstill_breakaway_time: float
+
+  def __post_init__(self):
+    nonnegative_fields = {
+      "planner_delay": self.planner_delay,
+      "transport_delay": self.transport_delay,
+      "actuator_lag": self.actuator_lag,
+      "standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
+      "standstill_breakaway_time": self.standstill_breakaway_time,
+    }
+    if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
+      raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
+    if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
+      raise ValueError("command_rate_limit must be finite and positive")
+    if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
+      raise ValueError("stopping_acceleration must be finite and non-positive")
+
+
+# Conservative Prius TSS2 actuator model.
+PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
+  planner_delay=0.05,
+  transport_delay=0.0,
+  actuator_lag=0.20,
+  command_rate_limit=4.0,
+  stopping_acceleration=-2.0,
+  standstill_breakaway_acceleration=1.0,
+  standstill_breakaway_time=0.05,
+)
+
+
+class PlantSP(Plant):
+  """Closed-loop plant with configurable observations and actuator response."""
+
+  def __init__(
+    self,
+    lead_relevancy=False,
+    speed=0.0,
+    distance_lead=2.0,
+    enabled=True,
+    only_lead2=False,
+    only_radar=False,
+    e2e=False,
+    personality=0,
+    force_decel=False,
+    lead_observation_fn: LeadObservationFn | None = None,
+    model_action_fn: ModelActionFn | None = None,
+    ego_observation_fn: EgoObservationFn | None = None,
+    actuator_delay: float | None = None,
+    actuator_lag: float = 0.0,
+    actuator_model: ActuatorModel | None = None,
+    run_long_control: bool = False,
+  ):
+    if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
+      raise ValueError("actuator_delay must be finite and non-negative")
+    if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
+      raise ValueError("actuator_lag must be finite and non-negative")
+
+    self.rate = 1.0 / DT_MDL
+
+    if not Plant.messaging_initialized:
+      Plant.radar = messaging.pub_sock('radarState')
+      Plant.controls_state = messaging.pub_sock('controlsState')
+      Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
+      Plant.car_state = messaging.pub_sock('carState')
+      Plant.plan = messaging.sub_sock('longitudinalPlan')
+      Plant.messaging_initialized = True
+
+    self.v_lead_prev = 0.0
+
+    self.distance = 0.0
+    self.speed = speed
+    self.should_stop = False
+    self.acceleration = 0.0
+    self.a_target = 0.0
+    self.actuator_command = 0.0
+    self.applied_actuator_command = 0.0
+    self.breakaway_confirmed = False
+    self._breakaway_timer = 0.0
+
+    # lead car
+    self.lead_relevancy = lead_relevancy
+    self.distance_lead = distance_lead
+    self.enabled = enabled
+    self.only_lead2 = only_lead2
+    self.only_radar = only_radar
+    self.e2e = e2e
+    self.personality = personality
+    self.force_decel = force_decel
+    self.lead_observation_fn = lead_observation_fn
+    self.model_action_fn = model_action_fn
+    self.ego_observation_fn = ego_observation_fn
+    self.actuator_model = actuator_model
+    self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
+    self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
+    self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
+    self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
+                                      actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None, run_long_control))
+
+    self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
+    self.ts = 1.0 / self.rate
+    time.sleep(0.1)
+    self.sm = messaging.SubMaster(['longitudinalPlan'])
+
+    from opendbc.car.honda.values import CAR
+    from opendbc.car.honda.interface import CarInterface
+
+    CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+    if self.actuator_delay is not None:
+      CP.longitudinalActuatorDelay = self.actuator_delay
+    CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
+    self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
+    self.long_control = LongControl(CP, CP_SP) if run_long_control else None
+
+    if self.actuator_model is not None and self.speed >= 0.01:
+      self.breakaway_confirmed = True
+    self.integration_dt = DT_CTRL if run_long_control else self.ts
+    delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt)
+    self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
+
+  @staticmethod
+  def _lead_message(observation: LeadObservation):
+    lead = log.RadarState.LeadData.new_message()
+    for field, value in observation.items():
+      setattr(lead, field, value)
+    return lead
+
+  def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
+    if self.lead_observation_fn is None:
+      return dict(truth) if present_by_default else None
+
+    observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
+    if observed is None:
+      return None
+
+    complete_observation = dict(truth)
+    complete_observation.update(observed)
+    return complete_observation
+
+  def _update_actuator(self, command: float) -> tuple[float, float]:
+    if self._actuator_delay_queue:
+      self._actuator_delay_queue.append(command)
+      delayed_command = self._actuator_delay_queue.popleft()
+    else:
+      delayed_command = command
+
+    if self.actuator_model is not None:
+      max_command_delta = self.actuator_model.command_rate_limit * self.integration_dt
+      self.applied_actuator_command = float(np.clip(delayed_command,
+                                                   self.applied_actuator_command - max_command_delta,
+                                                   self.applied_actuator_command + max_command_delta))
+
+      if self.speed < 0.01:
+        if self.applied_actuator_command <= 0.0:
+          self.breakaway_confirmed = False
+          self._breakaway_timer = 0.0
+        elif not self.breakaway_confirmed:
+          breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
+          if breakaway_ready:
+            self._breakaway_timer += self.integration_dt
+          else:
+            self._breakaway_timer = 0.0
+
+          self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
+        if not self.breakaway_confirmed:
+          self.acceleration = 0.0
+          return delayed_command, self.acceleration
+      else:
+        self.breakaway_confirmed = True
+
+      response_command = self.applied_actuator_command
+    else:
+      self.applied_actuator_command = delayed_command
+      response_command = delayed_command
+
+    if self.actuator_lag > 0.0:
+      alpha = 1.0 - math.exp(-self.integration_dt / self.actuator_lag)
+      self.acceleration += alpha * (response_command - self.acceleration)
+    else:
+      self.acceleration = response_command
+    return delayed_command, self.acceleration
+
+  def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None:
+    self.speed += self.acceleration * dt
+    if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0:
+      self.speed = self.acceleration = 0.0
+    self.distance += self.speed * dt
+
+  def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
+    # ******** publish a fake model going straight and fake calibration ********
+    # note that this is worst case for MPC, since model will delay long mpc by one time step
+    radar = messaging.new_message('radarState')
+    control = messaging.new_message('controlsState')
+    ss = messaging.new_message('selfdriveState')
+    car_state = messaging.new_message('carState')
+    vehicle_parameters = messaging.new_message('vehicleParameters')
+    car_control = messaging.new_message('carControl')
+    model = messaging.new_message('modelV2')
+    car_state_sp = messaging.new_message('carStateSP')
+    live_map_data_sp = messaging.new_message('liveMapDataSP')
+    gps_data = messaging.new_message('gpsLocation')
+    a_lead = (v_lead - self.v_lead_prev) / self.ts
+    self.v_lead_prev = v_lead
+
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+      if self.only_radar:
+        status = True
+      elif prob_lead > 0.5:
+        status = True
+      else:
+        status = False
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+      prob_lead = 0.0
+      status = False
+
+    truth_lead: LeadObservation = {
+      "dRel": float(d_rel),
+      "yRel": 0.0,
+      "vRel": float(v_rel),
+      "vLead": float(v_lead),
+      "vLeadK": float(v_lead),
+      "aLeadK": float(a_lead),
+      "present": bool(status),
+      # TODO use real radard logic for this
+      "aLeadTau": float(_LEAD_ACCEL_TAU),
+      "modelProb": float(prob_lead),
+      "radar": bool(self.only_radar),
+      "radarTrackId": -1,
+    }
+    lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
+    lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
+    if lead_one_observation is not None:
+      radar.radarState.leadOne = self._lead_message(lead_one_observation)
+    if lead_two_observation is not None:
+      radar.radarState.leadTwo = self._lead_message(lead_two_observation)
+
+    # Simulate model predicting slightly faster speed
+    # this is to ensure lead policy is effective when model
+    # does not predict slowdown in e2e mode
+    position = log.XYZTData.new_message()
+    position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
+    model.modelV2.position = position
+    if self.model_action_fn is None:
+      model_acceleration, model_should_stop = self.acceleration + 0.5, False
+    else:
+      model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
+    model.modelV2.action.desiredAcceleration = float(model_acceleration)
+    model.modelV2.action.shouldStop = bool(model_should_stop)
+    velocity = log.XYZTData.new_message()
+    velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
+    velocity.x[0] = float(self.speed)  # always start at current speed
+    model.modelV2.velocity = velocity
+    acceleration = log.XYZTData.new_message()
+    acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
+    model.modelV2.acceleration = acceleration
+    model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
+
+    control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
+      LongCtrlState.pid if self.enabled else LongCtrlState.off)
+    ss.selfdriveState.experimentalMode = self.e2e
+    ss.selfdriveState.personality = self.personality
+    control.controlsState.forceDecel = self.force_decel
+    true_v_ego = self.speed
+    true_a_ego = self.acceleration
+    published_v_ego = true_v_ego
+    published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
+    if self.ego_observation_fn is not None:
+      published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
+    car_state.carState.vEgo = float(published_v_ego)
+    car_state.carState.aEgo = float(published_a_ego)
+    car_state.carState.standstill = bool(self.speed < 0.01)
+    car_state.carState.vCruise = float(v_cruise * 3.6)
+    car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
+
+    # ******** get controlsState messages for plotting ***
+    sm = PlannerSM(self.rk.frame, {
+      'radarState': radar.radarState,
+      'carState': car_state.carState,
+      'carControl': car_control.carControl,
+      'controlsState': control.controlsState,
+      'selfdriveState': ss.selfdriveState,
+      'vehicleParameters': vehicle_parameters.vehicleParameters,
+      'modelV2': model.modelV2,
+      'carStateSP': car_state_sp.carStateSP,
+      'liveMapDataSP': live_map_data_sp.liveMapDataSP,
+      'gpsLocation': gps_data.gpsLocation,
+    })
+    self.planner.update(sm)
+    self.a_target = self.planner.output_a_target
+    if self.long_control is None:
+      self.actuator_command = self.a_target
+      if self.planner.output_should_stop:
+        stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
+        self.actuator_command = min(stopping_acceleration, self.actuator_command)
+      self._update_actuator(self.actuator_command)
+      self._integrate_ego(self.ts)
+    else:
+      for _ in range(round(self.ts / DT_CTRL)):
+        car_state.carState.vEgo = self.speed
+        car_state.carState.aEgo = self.acceleration
+        car_state.carState.standstill = self.speed < 0.01
+        self.actuator_command = self.long_control.update(
+          self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX),
+        )
+        self._update_actuator(self.actuator_command)
+        self._integrate_ego(DT_CTRL, stop_at_standstill=True)
+    self.should_stop = self.planner.output_should_stop
+    fcw = self.planner.fcw
+    self.distance_lead = self.distance_lead + v_lead * self.ts
+
+    # *** radar model ***
+    if self.lead_relevancy:
+      d_rel = np.maximum(0.0, self.distance_lead - self.distance)
+      v_rel = v_lead - self.speed
+    else:
+      d_rel = 200.0
+      v_rel = 0.0
+
+    # print at 5hz
+    # if (self.rk.frame % (self.rate // 5)) == 0:
+    #   print("%2.2f sec   %6.2f m  %6.2f m/s  %6.2f m/s2   lead_rel: %6.2f m  %6.2f m/s"
+    #         % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
+
+    # ******** update prevs ********
+    self.rk.monitor_time()
+
+    return {
+      "distance": self.distance,
+      "speed": self.speed,
+      "acceleration": self.acceleration,
+      "realized_acceleration": self.acceleration,
+      "a_target": self.a_target,
+      "actuator_command": self.actuator_command,
+      "published_a_ego": published_a_ego,
+      "published_v_ego": published_v_ego,
+      "should_stop": self.should_stop,
+      "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None
+                             else control.controlsState.longControlState.raw),
+      "distance_lead": self.distance_lead,
+      "fcw": fcw,
+      "mpc_source": self.planner.mpc.source,
+      "dec_mode": self.planner.dec.mode(),
+      "controller_active": self.planner.accel_controller_active,
+      "model_action": {
+        "desiredAcceleration": float(model_acceleration),
+        "shouldStop": bool(model_should_stop),
+      },
+      "truth_lead": dict(truth_lead),
+      "lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
+      "lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
+    }

openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py

@@ -0,0 +1,164 @@
+from collections.abc import Callable
+import math
+from typing import cast
+
+from openpilot.common.parameterized import parameterized
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+
+STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
+
+
+def departing_lead(current_time: float) -> float:
+  return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
+
+
+def stopped_lead(_current_time: float) -> float:
+  return 0.0
+
+
+PARITY_SCENARIOS = {
+  "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80},
+  "stop_then_depart": {"lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_cruise": 8.0, "v_lead": departing_lead, "steps": 120},
+}
+
+
+def _drive(cls, *, v_cruise: float, v_lead: Callable[[float], float], steps: int, **kwargs):
+  plant = cls(**kwargs)
+  plant.v_lead_prev = v_lead(0.0)
+  solver_failures = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counting_reset(*args, **kw):
+    nonlocal solver_failures
+    if plant.planner.mpc.solution_status != 0:
+      solver_failures += 1
+    return original_reset(*args, **kw)
+
+  plant.planner.mpc.reset = counting_reset
+  results = []
+  for _ in range(steps):
+    lead_speed = v_lead(plant.current_time)
+    result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
+    results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
+  return results, solver_failures
+
+
+class TestPlantSP(OpenpilotTestCase):
+  @parameterized.expand(PARITY_SCENARIOS, names=("scenario",), ids=lambda scenario: scenario)
+  def test_plant_sp_matches_stock_plant_on_shared_kwargs(self, scenario: str):
+    kwargs = dict(PARITY_SCENARIOS[scenario])
+    v_cruise = cast(float, kwargs.pop("v_cruise"))
+    v_lead = cast(Callable[[float], float], kwargs.pop("v_lead"))
+    steps = cast(int, kwargs.pop("steps"))
+
+    stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+    sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
+
+    assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
+    assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
+
+    for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
+      zip(stock_results, sp_results, strict=True),
+    ):
+      for key in STOCK_STEP_KEYS:
+        if isinstance(stock_result[key], float):
+          self.assertAlmostEqual(sp_result[key], stock_result[key], msg=f"{scenario} frame {frame} key {key}")
+        else:
+          assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
+      assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
+      self.assertAlmostEqual(sp_a_target, stock_a_target, msg=f"{scenario} frame {frame} output_a_target")
+
+    if scenario == "stop_then_depart":
+      departure_frame = round(1.0 / DT_MDL)
+      for results in (stock_results, sp_results):
+        assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
+        assert results[departure_frame - 1][0]["should_stop"]
+        assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
+        assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
+      stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results)
+                           if frame >= departure_frame and not result["should_stop"])
+      sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results)
+                        if frame >= departure_frame and not result["should_stop"])
+      stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results)
+                          if frame >= departure_frame and result["speed"] > 0.05)
+      sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results)
+                       if frame >= departure_frame and result["speed"] > 0.05)
+      assert sp_release == stock_release
+      assert sp_motion == stock_motion
+
+  def test_full_lead_observation_is_independent_from_truth(self):
+    callback_inputs = []
+
+    def observe_lead(current_time, lead_name, truth):
+      callback_inputs.append((current_time, lead_name, truth))
+      if lead_name == "leadOne":
+        return {
+          "dRel": 12.5,
+          "vRel": -4.0,
+          "vLead": 6.0,
+          "vLeadK": 5.5,
+          "aLeadK": -1.25,
+          "aLeadTau": 0.7,
+          "present": True,
+          "modelProb": 0.9,
+          "radarTrackId": 42,
+        }
+      return None
+
+    plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
+    result = plant.step(v_lead=8.0)
+
+    assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
+    self.assertAlmostEqual(callback_inputs[0][2]["dRel"], 50.0)
+    self.assertAlmostEqual(result["truth_lead"]["dRel"], 50.0)
+    self.assertAlmostEqual(result["lead_one_observation"]["dRel"], 12.5)
+    assert result["lead_one_observation"]["radarTrackId"] == 42
+    assert result["lead_two_observation"] is None
+    self.assertAlmostEqual(result["distance_lead"], 50.0 + 8.0 * DT_MDL)
+
+  def test_model_action_realized_acceleration_and_source_logging(self):
+    def model_action(current_time, v_ego, a_ego):
+      return -1.25, True
+
+    plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
+    first = plant.step()
+    second = plant.step()
+
+    assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
+    self.assertAlmostEqual(first["published_a_ego"], 0.0)
+    self.assertAlmostEqual(second["published_a_ego"], first["realized_acceleration"])
+    assert first["acceleration"] == first["realized_acceleration"]
+    assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
+    assert first["mpc_source"] is not None
+    assert first["dec_mode"] in ("acc", "blended")
+    assert "controller_active" in first
+    assert first["lead_one_observation"] is not None
+    assert first["truth_lead"] == first["lead_one_observation"]
+
+  def test_default_model_action_matches_stock_plant(self):
+    result = PlantSP(speed=10.0).step()
+
+    self.assertAlmostEqual(result["model_action"]["desiredAcceleration"], 0.5)
+    assert not result["model_action"]["shouldStop"]
+
+  def test_configurable_transport_delay_and_first_order_lag(self):
+    plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
+
+    self.assertAlmostEqual(plant.planner.CP.longitudinalActuatorDelay, 2 * DT_MDL)
+    delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
+    assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
+
+    expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
+    assert delayed_commands[2][0] == -1.0
+    self.assertAlmostEqual(delayed_commands[2][1], expected_acceleration)
+
+  @parameterized.expand(
+    [(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
+    names=("delay", "lag"),
+  )
+  def test_invalid_actuator_dynamics(self, delay, lag):
+    with self.assertRaises(ValueError):
+      PlantSP(actuator_delay=delay, actuator_lag=lag)

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -652,6 +652,53 @@
                 }
               ]
             },
+            {
+              "key": "AccelPersonalityEnabled",
+              "widget": "toggle",
+              "title": "Enable Accel Controller",
+              "description": "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain independent of this setting.",
+              "visibility": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
+            {
+              "key": "AccelPersonality",
+              "widget": "multiple_button",
+              "title": "Acceleration Profile",
+              "description": "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles.",
+              "options": [
+                {
+                  "value": 0,
+                  "label": "Eco"
+                },
+                {
+                  "value": 1,
+                  "label": "Normal"
+                },
+                {
+                  "value": 2,
+                  "label": "Sport"
+                }
+              ],
+              "enablement": [
+                {
+                  "type": "capability",
+                  "field": "has_longitudinal_control",
+                  "equals": true
+                }
+              ]
+            },
             {
               "key": "IntelligentCruiseButtonManagement",
               "widget": "toggle",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml

@@ -43,6 +43,29 @@ sections:
       label: Relaxed
     enablement:
     - $ref: '#/macros/longitudinal'
+  - key: AccelPersonalityEnabled
+    widget: toggle
+    title: Enable Accel Controller
+    description: Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking,
+      and stopping behavior remain independent of this setting.
+    visibility:
+    - $ref: '#/macros/longitudinal'
+    enablement:
+    - $ref: '#/macros/longitudinal'
+  - key: AccelPersonality
+    widget: multiple_button
+    title: Acceleration Profile
+    description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
+      profiles.
+    options:
+    - value: 0
+      label: Eco
+    - value: 1
+      label: Normal
+    - value: 2
+      label: Sport
+    enablement:
+    - $ref: '#/macros/longitudinal'
   - key: IntelligentCruiseButtonManagement
     widget: toggle
     title: Intelligent Cruise Button Management (ICBM) (Alpha)

openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py

@@ -276,6 +276,29 @@ class TestKnownPanels(OpenpilotTestCase):
     enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"}
     assert "NeuralNetworkLateralControl" in enhanced_enable_keys
 
+  def test_accel_controller_profile_mapping_and_enablement(self, schema):
+    cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
+    items = {item["key"]: item for item in _iter_panel_items(cruise)}
+
+    assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
+    assert items["AccelPersonality"]["options"] == [
+      {"value": 0, "label": "Eco"},
+      {"value": 1, "label": "Normal"},
+      {"value": 2, "label": "Sport"},
+    ]
+    assert {
+      "type": "capability",
+      "field": "has_longitudinal_control",
+      "equals": True,
+    } in items["AccelPersonalityEnabled"]["enablement"]
+    assert {
+      "type": "capability",
+      "field": "has_longitudinal_control",
+      "equals": True,
+    } in items["AccelPersonality"]["enablement"]
+    profile_enable_keys = {rule.get("key") for rule in items["AccelPersonality"]["enablement"] if rule.get("type") == "param"}
+    assert "AccelPersonalityEnabled" not in profile_enable_keys
+
 
 class TestKnownVehicleSettings(OpenpilotTestCase):
   def test_hyundai_has_longitudinal_tuning(self, schema):