refactor

cereal/custom.capnp

@@ -95,6 +95,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d {
 
 struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
   dec @0 :DynamicExperimentalControl;
+  accelPersonality @1 :AccelerationPersonality;
 
   struct DynamicExperimentalControl {
     state @0 :DynamicExperimentalControlState;
@@ -106,6 +107,13 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
       blended @1;
     }
   }
+
+  enum AccelerationPersonality {
+    sport @0;
+    normal @1;
+    eco @2;
+    stock @3;
+  }
 }
 
 struct OnroadEventSP @0xda96579883444c35 {

common/params_keys.h

@@ -171,4 +171,5 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"HyundaiRadarTracksToggle", PERSISTENT},
 
     {"DynamicExperimentalControl", PERSISTENT},
+    {"AccelPersonality", PERSISTENT},
 };

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -10,6 +10,9 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.modeld.constants import index_function
 from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
 
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
+
+
 if __name__ == '__main__':  # generating code
   from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
 else:
@@ -228,6 +231,7 @@ class LongitudinalMpc:
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.reset()
     self.source = SOURCES[2]
+    self.accel_controller = AccelController()
 
   def reset(self):
     # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
@@ -332,6 +336,8 @@ class LongitudinalMpc:
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
+    a_cruise_min = self.accel_controller._get_min_accel_for_speed(v_ego)
+
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
@@ -350,7 +356,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

@@ -126,6 +126,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     else:
       accel_clip = [ACCEL_MIN, ACCEL_MAX]
 
+    # Override accel using Accel Controller if enabled
+    if self.accel_controller.is_personality_enabled:
+      max_limit = self.accel_controller._get_max_accel_for_speed(v_ego)
+      if self.mpc.mode == 'acc':
+        # Use the accel controller limits directly
+        accel_clip = [ACCEL_MIN, max_limit]
+        # Recalculate limit turn according to the new max limit
+        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)
+
     if reset_state:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active

selfdrive/ui/qt/offroad/settings.cc

@@ -78,6 +78,16 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
                                           "../assets/offroad/icon_speed_limit.png",
                                           longi_button_texts);
 
+  // accel controller
+  std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco"), tr("Stock")};
+  accel_personality_setting = new ButtonParamControlSP("AccelPersonality", tr("Acceleration Personality"),
+                                          tr("Normal is recommended. In sport mode, sunnypilot will provide aggressive acceleration for a dynamic driving experience. "
+                                             "In eco mode, sunnypilot will apply smoother and more relaxed acceleration. On supported cars, you can cycle through these "
+                                             "acceleration personality within Onroad Settings on the driving screen."),
+                                          "",
+                                          accel_personality_texts);
+  accel_personality_setting->showDescription();
+
   // set up uiState update for personality setting
   QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
 
@@ -93,6 +103,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     // insert longitudinal personality after NDOG toggle
     if (param == "DisengageOnAccelerator") {
       addItem(long_personality_setting);
+      addItem(accel_personality_setting);
     }
   }
 
@@ -113,6 +124,13 @@ void TogglesPanel::updateState(const UIState &s) {
     }
     uiState()->scene.personality = personality;
   }
+  if (sm.updated("longitudinalPlanSP")) {
+    auto accel_personality = sm["longitudinalPlanSP"].getLongitudinalPlanSP().getAccelPersonality();
+    if (accel_personality != s.scene.accel_personality && s.scene.started && isVisible()) {
+      accel_personality_setting->setCheckedButton(static_cast<int>(accel_personality));
+    }
+    uiState()->scene.accel_personality = accel_personality;
+  }
 }
 
 void TogglesPanel::expandToggleDescription(const QString &param) {
@@ -150,10 +168,12 @@ void TogglesPanel::updateToggles() {
       experimental_mode_toggle->setEnabled(true);
       experimental_mode_toggle->setDescription(e2e_description);
       long_personality_setting->setEnabled(true);
+      accel_personality_setting->setEnabled(true);
     } else {
       // no long for now
       experimental_mode_toggle->setEnabled(false);
       long_personality_setting->setEnabled(false);
+      accel_personality_setting->setEnabled(true);
       params.remove("ExperimentalMode");
 
       const QString unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.");

selfdrive/ui/qt/offroad/settings.h

@@ -85,6 +85,7 @@ protected:
   Params params;
   std::map<std::string, ParamControl*> toggles;
   ButtonParamControl *long_personality_setting;
+  ButtonParamControl *accel_personality_setting;
 
   virtual void updateToggles();
 };

selfdrive/ui/ui.h

@@ -60,6 +60,7 @@ typedef struct UIScene {
   cereal::PandaState::PandaType pandaType;
 
   cereal::LongitudinalPersonality personality;
+  cereal::LongitudinalPlanSP::AccelerationPersonality accel_personality;
 
   float light_sensor = -1;
   bool started, ignition, is_metric;

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

@@ -0,0 +1,75 @@
+"""
+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
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_profiles import (
+  get_max_accel_hermite,
+  get_min_accel_hermite
+)
+
+
+AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
+
+class AccelController:
+  def __init__(self):
+    self.params = Params()
+    self.personality = AccelPersonality.stock
+    self.frame = 0
+
+  def _update_personality_from_param(self):
+    if self.frame % int(1. / DT_MDL) == 0:
+      personality_str = self.params.get("AccelPersonality", encoding='utf-8')
+      if personality_str is not None:
+        personality_int = int(personality_str)
+        if personality_int in [AccelPersonality.stock, AccelPersonality.normal, AccelPersonality.eco, AccelPersonality.sport]:
+          self.personality = personality_int
+
+  def _get_max_accel_for_speed(self, v_ego: float) -> float:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.eco:
+      mode = "eco"
+    elif self.personality == AccelPersonality.sport:
+      mode = "sport"
+    else:
+      mode = "normal"
+
+    return get_max_accel_hermite(v_ego, mode)
+
+  def _get_min_accel_for_speed(self, v_ego: float) -> float:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.eco:
+      mode = "eco"
+    elif self.personality == AccelPersonality.sport:
+      mode = "sport"
+    elif self.personality == AccelPersonality.normal:
+      mode = "normal"
+    else:
+      mode = "stock"
+
+    return get_min_accel_hermite(v_ego, mode)
+
+  def get_accel_limits(self, v_ego: float, accel_limits: list[float]) -> tuple[float, float]:
+    self._update_personality_from_param()
+
+    if self.personality == AccelPersonality.stock:
+      return (accel_limits[0], accel_limits[1])
+    else:
+      max_accel = self._get_max_accel_for_speed(v_ego)
+      min_accel = self._get_min_accel_for_speed(v_ego)
+      return (min_accel, max_accel)
+
+  def is_personality_enabled(self, accel_personality: int = AccelPersonality.stock) -> bool:
+    self.personality = accel_personality
+    self._update_personality_from_param()
+    return bool(self.personality != AccelPersonality.stock)
+
+  def update(self):
+    self.frame += 1

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

@@ -0,0 +1,79 @@
+"""
+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 numpy as np
+
+# Profiles
+MAX_ACCEL_PROFILES = {
+  "eco":                [2.00, 2.00, 1.98, 1.54, 0.83, .572, .455, .365, .32, .10],
+  "normal":             [2.00, 2.00, 1.99, 1.66, 1.06, .66,  .58, .49, .37, .15],
+  "sport":              [2.00, 2.00, 2.00, 1.95, 1.25, .88,  .70, .60, .45, .20],
+}
+MAX_ACCEL_BREAKPOINTS = [0.,   1.,   6.,   8.,   11.,  16.,  20., 25., 30., 55.]
+
+MIN_ACCEL_PROFILES = {
+  "eco":                [-.06,  -.06,  -.11,  -.11, -.071, -.071, -.072, -.65, -.65],
+  "normal":             [-.07,  -.07,  -.12,  -.12, -.072, -.072, -.073, -.70, -.70],
+  "sport":              [-1.2,  -1.2,  -1.2,  -1.2, -1.2,  -1.2,  -1.2,  -1.2, -1.2],
+  "stock":              [-1.2,  -1.2,  -1.2,  -1.2, -1.2,  -1.2,  -1.2,  -1.2, -1.2],
+}
+MIN_ACCEL_BREAKPOINTS = [0.,    0.5,    0.6,    1.2,    8.,    12.,   14.,   25.,  40.]
+
+# Precompute slopes for Cubic Bézier curves
+def compute_symmetric_slopes(x, y):
+  n = len(x)
+  if n < 2:
+    raise ValueError("At least two points are required to compute slopes.")
+  m = np.zeros(n)
+  for i in range(n):
+    if i == 0:
+      m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i])
+    elif i == n-1:
+      m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1])
+    else:
+      m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2
+  return m
+
+
+MAX_ACCEL_SLOPES = {
+  mode: compute_symmetric_slopes(MAX_ACCEL_BREAKPOINTS, values)
+  for mode, values in MAX_ACCEL_PROFILES.items()
+}
+
+MIN_ACCEL_SLOPES = {
+  mode: compute_symmetric_slopes(MIN_ACCEL_BREAKPOINTS, values)
+  for mode, values in MIN_ACCEL_PROFILES.items()
+}
+
+# Hermite interpolation function
+def hermite_interpolate(x, xp, yp, slopes, mode):
+  # Clip x inside the domain
+  x = np.clip(x, xp[0], xp[-1])
+
+  # Find segment
+  idx = np.searchsorted(xp, x) - 1
+  idx = np.clip(idx, 0, len(slopes[mode]) - 1)
+
+  x0, x1 = xp[idx], xp[idx+1]
+  y0, y1 = yp[idx], yp[idx+1]
+  m0, m1 = slopes[mode][idx], slopes[mode][idx+1]
+
+  t = (x - x0) / (x1 - x0)
+  h00 = 2*t**3 - 3*t**2 + 1
+  h10 = t**3 - 2*t**2 + t
+  h01 = -2*t**3 + 3*t**2
+  h11 = t**3 - t**2
+
+  interpolated = (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1)
+  return interpolated
+
+# Final functions to call
+def get_max_accel_hermite(v_ego: float, mode: str = "normal") -> float:
+  return float(hermite_interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[mode], MAX_ACCEL_SLOPES, mode))
+
+def get_min_accel_hermite(v_ego: float, mode: str = "normal") -> float:
+  return float(hermite_interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[mode], MIN_ACCEL_SLOPES, mode))

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -9,13 +9,14 @@ from cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.sunnypilot.models.helpers import get_active_model_runner
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
-
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, mpc):
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.accel_controller = AccelController()
     self.is_stock = get_active_model_runner() == custom.ModelManagerSP.Runner.stock
 
   def get_mpc_mode(self) -> str | None:
@@ -26,6 +27,7 @@ class LongitudinalPlannerSP:
 
   def update(self, sm: messaging.SubMaster) -> None:
     self.dec.update(sm)
+    self.accel_controller.update()
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')

system/manager/manager.py

@@ -5,7 +5,7 @@ import signal
 import sys
 import traceback
 
-from cereal import log
+from cereal import log, custom
 import cereal.messaging as messaging
 import openpilot.system.sentry as sentry
 from openpilot.common.params import Params, ParamKeyType
@@ -42,6 +42,7 @@ def manager_init() -> None:
   ]
 
   sunnypilot_default_params: list[tuple[str, str | bytes]] = [
+    ("AccelPersonality", str(custom.LongitudinalPlanSP.AccelerationPersonality.stock)),
     ("AutoLaneChangeTimer", "0"),
     ("AutoLaneChangeBsmDelay", "0"),
     ("DynamicExperimentalControl", "0"),