accel controller

common/params.cc

@@ -209,7 +209,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START},
     {"UpdaterLastFetchTime", PERSISTENT},
     {"Version", PERSISTENT},
-
+    {"AccelProfile", PERSISTENT | BACKUP},
     {"AccMadsCombo", PERSISTENT | BACKUP},
     {"AmapKey1", PERSISTENT | BACKUP},
     {"AmapKey2", PERSISTENT | BACKUP},

selfdrive/controls/lib/longitudinal_planner.py

@@ -19,7 +19,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
 from openpilot.selfdrive.controls.lib.vision_turn_controller import VisionTurnController
 from openpilot.selfdrive.controls.lib.turn_speed_controller import TurnSpeedController
-from openpilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController
+from openpilot.selfdrive.controls.lib.sunnypilot.dynamic_experimental_controller import DynamicExperimentalController
+from openpilot.selfdrive.controls.lib.sunnypilot.accel_controller import AccelController
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.common.swaglog import cloudlog
 
@@ -100,6 +101,7 @@ class LongitudinalPlanner:
     self.events = Events()
     self.turn_speed_controller = TurnSpeedController()
     self.dynamic_experimental_controller = DynamicExperimentalController()
+    self.accel_controller = AccelController()
 
   def read_param(self):
     try:
@@ -127,6 +129,7 @@ class LongitudinalPlanner:
     if self.param_read_counter % 50 == 0:
       self.read_param()
     self.param_read_counter += 1
+    self.accel_controller.set_profile(self.params.get("AccelProfile", encoding='utf-8'))
     if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode:
       self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState'], sm['navInstruction'].maneuverDistance)
     else:
@@ -152,6 +155,22 @@ class LongitudinalPlanner:
       accel_limits = [ACCEL_MIN, ACCEL_MAX]
       accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
 
+    # override accel using Accel controller
+    if self.accel_controller.is_enabled():
+      # get min, max from accel controller
+      min_limit, max_limit = self.accel_controller.get_accel_limits(v_ego, accel_limits)
+      if self.mpc.mode == 'acc':
+        # voacc car, just give it max min (-1.2) so I can brake harder
+        if self.CP.radarUnavailable:
+          accel_limits = [A_CRUISE_MIN, max_limit]
+        else:
+          accel_limits = [min_limit, max_limit]
+        # recalculate limit turn according to the new min, max
+        accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
+      else:
+        # blended, just give it max min (-3.5) and max from accel controller
+        accel_limits = accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
+
     if reset_state:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active

selfdrive/controls/lib/sunnypilot/accel_controller.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+# The MIT License
+#
+# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in
+# all copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+# THE SOFTWARE.
+
+# Last update: June 5, 2024
+
+from openpilot.common.numpy_fast import interp
+
+DP_ACCEL_STOCK = 0
+DP_ACCEL_ECO = 1
+DP_ACCEL_NORMAL = 2
+DP_ACCEL_SPORT = 3
+
+# accel profile by @arne182 modified by cgw
+_DP_CRUISE_MIN_V =       [-1.00,  -1.00,  -0.99,  -0.90,  -0.90,  -0.88,  -0.88,  -0.82]
+_DP_CRUISE_MIN_V_ECO =   [-1.00,  -1.00,  -0.98,  -0.88,  -0.88,  -0.86,  -0.86,  -0.80]
+_DP_CRUISE_MIN_V_SPORT = [-1.01,  -1.01,  -1.00,  -0.92,  -0.92,  -0.90,  -0.90,  -0.84]
+_DP_CRUISE_MIN_BP =      [0.,     0.05,    0.4,    0.5,    8.33,   16.,   30.,   40.]
+
+_DP_CRUISE_MAX_V =       [2.4, 2.4, 2.4, 1.60, 1.05, .81,  .625, .42,  .348, .12]
+_DP_CRUISE_MAX_V_ECO =   [1.6, 1.6, 1.6, 1.0, .60, .50, .40, .25, .15, .05]
+_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.8, 2.4,  1.4,  1.0,  .89,  .75,  .50,  .2]
+_DP_CRUISE_MAX_BP =      [0.,  1.,  6.,  8.,   11.,  15.,  20.,  25.,  30.,  55.]
+
+
+class AccelController:
+
+    def __init__(self):
+        # self._params = Params()
+        self._profile = DP_ACCEL_STOCK
+
+    def set_profile(self, profile):
+        try:
+            self._profile = int(profile) if int(profile) in [DP_ACCEL_STOCK, DP_ACCEL_ECO, DP_ACCEL_NORMAL, DP_ACCEL_SPORT] else DP_ACCEL_STOCK
+        except:
+            self._profile = DP_ACCEL_STOCK
+
+    def _dp_calc_cruise_accel_limits(self, v_ego):
+        if self._profile == DP_ACCEL_ECO:
+            min_v = _DP_CRUISE_MIN_V_ECO
+            max_v = _DP_CRUISE_MAX_V_ECO
+        elif self._profile == DP_ACCEL_SPORT:
+            min_v = _DP_CRUISE_MIN_V_SPORT
+            max_v = _DP_CRUISE_MAX_V_SPORT
+        else:
+            min_v = _DP_CRUISE_MIN_V
+            max_v = _DP_CRUISE_MAX_V
+
+        a_cruise_min = interp(v_ego, _DP_CRUISE_MIN_BP, min_v)
+        a_cruise_max = interp(v_ego, _DP_CRUISE_MAX_BP, max_v)
+        return a_cruise_min, a_cruise_max
+
+    def get_accel_limits(self, v_ego, accel_limits):
+        return accel_limits if self._profile == DP_ACCEL_STOCK else self._dp_calc_cruise_accel_limits(v_ego)
+
+    def is_enabled(self):
+        return self._profile != DP_ACCEL_STOCK

selfdrive/ui/qt/offroad/settings.cc

@@ -118,6 +118,13 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
                                           380);
   long_personality_setting->showDescription();
 
+  // accel controller
+  std::vector<QString> accel_profile_texts{tr("OP"), tr("ECO"), tr("NOR"), tr("SPT")};
+  ButtonParamControl* accel_profile_setting = new ButtonParamControl("AccelProfile", tr("Acceleration Profile"),
+                                          tr("OP - Stock tune.\nECO - Eco tune.\nNOR - Normal tune.\nSPT - Sport tune."),
+                                          "",
+                                          accel_profile_texts);
+
   // set up uiState update for personality setting
   QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
 
@@ -133,6 +140,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     // insert longitudinal personality after NDOG toggle
     if (param == "DisengageOnAccelerator") {
       addItem(long_personality_setting);
+      addItem(accel_profile_setting);
     }
   }
 

system/manager/manager.py

@@ -111,6 +111,7 @@ def manager_init() -> None:
     ("CustomDrivingModel", "0"),
     ("DrivingModelGeneration", "4"),
     ("LastSunnylinkPingTime", "0"),
+    ("AccelProfile", "0"),
   ]
   if not PC:
     default_params.append(("LastUpdateTime", datetime.datetime.now(datetime.UTC).replace(tzinfo=None).isoformat().encode('utf8')))