Merge branch 'accel-profile' into master-dev-c3

CHANGELOGS.md

@@ -1,6 +1,10 @@
 sunnypilot - 0.9.8.0 (2024-xx-xx)
 ========================
 * Always on driver monitoring toggle
+* NEW❗: Acceleration Profile thanks to kegman, rav4kumar, and arne1282
+  * 3 new modes acceleration profiles for you pick Eco, Stock and Support
+  * Acceleration button some vehicle including most TSS1/2/2.5 and HKG vehicle
+  * These coded right into the model acceleration martix can be actived in real time!
 ************************
 * UPDATED: Synced with commaai's openpilot
   * master commit b45caf4 (June 14, 2024)

cereal/custom.capnp

@@ -18,9 +18,17 @@ enum LongitudinalPersonalitySP {
   relaxed @3;
 }
 
+enum AccelerationProfile {
+  stock @0;
+  eco @1;
+  normal @2;
+  sport @3;
+}
+
 struct ControlsStateSP @0x81c2f05a394cf4af {
   lateralState @0 :Text;
   personality @8 :LongitudinalPersonalitySP;
+  accelProfile @9 :AccelerationProfile;
 
   lateralControlState :union {
     indiState @1 :LateralINDIState;

common/params.cc

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

selfdrive/controls/controlsd.py

@@ -188,6 +188,8 @@ class Controls:
     model_capabilities = ModelCapabilities.get_by_gen(self.model_gen)
     self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution
 
+    self.accel_profile = self.read_accel_profile_param()
+
     self.can_log_mono_time = 0
 
     self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
@@ -859,6 +861,7 @@ class Controls:
 
     controlsStateSP.lateralState = lat_tuning
     controlsStateSP.personality = self.personality
+    controlsStateSP.accelProfile = self.accel_profile
 
     if self.enable_nnff and lat_tuning == 'torque':
       controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp
@@ -907,12 +910,19 @@ class Controls:
     except (ValueError, TypeError):
       return custom.LongitudinalPersonalitySP.standard
 
+  def read_accel_profile_param(self):
+    try:
+      return int(self.params.get("AccelProfile"))
+    except (ValueError, TypeError):
+      return custom.AccelerationProfile.stock
+
   def params_thread(self, evt):
     while not evt.is_set():
       self.is_metric = self.params.get_bool("IsMetric")
       self.experimental_mode = self.params.get_bool("ExperimentalMode") and (self.CP.openpilotLongitudinalControl or
                                                                              (not self.CP.pcmCruiseSpeed and self.custom_stock_planner_speed))
       self.personality = self.read_personality_param()
+      self.accel_profile = self.read_accel_profile_param()
       if self.CP.notCar:
         self.joystick_mode = self.params.get_bool("JoystickDebugMode")
 

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.accel_controller import AccelController
+from openpilot.selfdrive.controls.lib.sunnypilot.dynamic_experimental_controller import DynamicExperimentalController
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.common.swaglog import cloudlog
 
@@ -100,12 +101,14 @@ class LongitudinalPlanner:
     self.events = Events()
     self.turn_speed_controller = TurnSpeedController()
     self.dynamic_experimental_controller = DynamicExperimentalController()
+    self.accel_controller = AccelController()
 
   def read_param(self):
     try:
       self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl"))
     except AttributeError:
       self.dynamic_experimental_controller = DynamicExperimentalController()
+      self.accel_controller = AccelController()
 
   @staticmethod
   def parse_model(model_msg, model_error):
@@ -152,6 +155,20 @@ 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(accel_profile=sm['controlsStateSP'].accelProfile):
+      # 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
+        accel_limits = [A_CRUISE_MIN, max_limit] if self.CP.radarUnavailable else [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_MIN, ACCEL_MAX]
+        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,68 @@
+#!/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 updated: June 5, 2024
+
+from cereal import custom
+from openpilot.common.numpy_fast import interp
+
+AccelProfile = custom.AccelerationProfile
+
+# accel profile by @arne182 modified by cgw
+_DP_CRUISE_MIN_V =       [-1.03,  -0.79,  -0.77,  -0.77,  -0.75,  -0.75,  -0.88,  -0.82]
+_DP_CRUISE_MIN_V_ECO =   [-1.02,  -0.78,  -0.75,  -0.75,  -0.73,  -0.73,  -0.80,  -0.80]
+_DP_CRUISE_MIN_V_SPORT = [-1.04,  -0.81,  -0.79,  -0.79,  -0.77,  -0.77,  -0.90,  -0.84]
+_DP_CRUISE_MIN_BP =      [0.,     0.05,   0.1,    0.5,    8.33,   16.,    30.,    40.]
+
+_DP_CRUISE_MAX_V =       [2.5, 2.5, 2.5, 1.70, 1.05, .81,  .625, .42,  .348, .12]
+_DP_CRUISE_MAX_V_ECO =   [2.0, 2.0, 2.0, 1.4, .80,   .68,  .53,  .32,  .20,  .085]
+_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._profile = AccelProfile.stock
+
+  def _dp_calc_cruise_accel_limits(self, v_ego: float):
+    if self._profile == AccelProfile.eco:
+      min_v = _DP_CRUISE_MIN_V_ECO
+      max_v = _DP_CRUISE_MAX_V_ECO
+    elif self._profile == AccelProfile.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: float, accel_limits: list[float]):
+    return accel_limits if self._profile == AccelProfile.stock else self._dp_calc_cruise_accel_limits(v_ego)
+
+  def is_enabled(self, accel_profile: int = AccelProfile.stock):
+    self._profile = accel_profile
+    return self._profile != AccelProfile.stock

selfdrive/ui/qt/offroad/settings.cc

@@ -124,6 +124,14 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
                                           380);
   long_personality_setting->showDescription();
 
+  // accel controller
+  std::vector<QString> accel_profile_texts{tr("Stock"), tr("Eco"), tr("Normal"), tr("Sport")};
+  accel_profile_setting = new ButtonParamControl("AccelProfile", tr("Acceleration Profile"),
+                                          tr("Stock - Stock tune.\nEco - Eco tune.\nNormal - Normal tune.\nSport - Sport tune."),
+                                          "../assets/offroad/icon_blank.png",
+                                          accel_profile_texts);
+  accel_profile_setting->showDescription();
+
   // set up uiState update for personality setting
   QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
 
@@ -139,6 +147,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     // insert longitudinal personality after NDOG toggle
     if (param == "DisengageOnAccelerator") {
       addItem(long_personality_setting);
+      addItem(accel_profile_setting);
     }
   }
 

selfdrive/ui/qt/offroad/settings.h

@@ -96,6 +96,7 @@ private:
   Params params;
   std::map<std::string, ParamControl*> toggles;
   ButtonParamControl *long_personality_setting;
+  ButtonParamControl *accel_profile_setting;
 
   ParamWatcher *param_watcher;
 };

system/manager/manager.py

@@ -115,6 +115,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')))