Longitudinal: Acceleration Personality

CHANGELOGS.md

@@ -4,6 +4,9 @@ sunnypilot - 0.9.8.0 (2024-xx-xx)
 ************************
 * UPDATED: Synced with commaai's openpilot
   * master commit b45caf4 (June 14, 2024)
+* NEW❗: Longitudinal: Acceleration Personality thanks to kegman, rav4kumar, and arne1282!
+  * Select from three distinct acceleration personalities: Eco, Normal, and Sport
+  * Acceleration personalities are integrated directly into the model's acceleration matrix and can be activated in real-time!
 * NEW❗: Longitudinal: Dynamic Personality thanks to rav4kumar!
   * Dynamically adjusts following distance and reaction based on your "Driving Personality" setting
   * Personalities adapt in real-time to your speed and the distance to the lead car

cereal/custom.capnp

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

common/params.cc

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

selfdrive/controls/controlsd.py

@@ -189,6 +189,8 @@ class Controls:
 
     self.dynamic_personality = self.params.get_bool("DynamicPersonality")
 
+    self.accel_personality = self.read_accel_personality_param()
+
     self.can_log_mono_time = 0
 
     self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0)
@@ -861,6 +863,7 @@ class Controls:
     controlsStateSP.lateralState = lat_tuning
     controlsStateSP.personality = self.personality
     controlsStateSP.dynamicPersonality = self.dynamic_personality
+    controlsStateSP.accelPersonality = self.accel_personality
 
     if self.enable_nnff and lat_tuning == 'torque':
       controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp
@@ -909,12 +912,19 @@ class Controls:
     except (ValueError, TypeError):
       return custom.LongitudinalPersonalitySP.standard
 
+  def read_accel_personality_param(self):
+    try:
+      return int(self.params.get("AccelPersonality"))
+    except (ValueError, TypeError):
+      return custom.AccelerationPersonality.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
       self.personality = self.read_personality_param()
       self.dynamic_personality = self.params.get_bool("DynamicPersonality")
+      self.accel_personality = self.read_accel_personality_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_personality=sm['controlsStateSP'].accelPersonality):
+      # 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: July 1, 2024
+
+from cereal import custom
+from openpilot.common.numpy_fast import interp
+
+AccelPersonality = custom.AccelerationPersonality
+
+# accel personality 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._personality = AccelPersonality.stock
+
+  def _dp_calc_cruise_accel_limits(self, v_ego: float) -> tuple[float, float]:
+    if self._personality == AccelPersonality.eco:
+      min_v = _DP_CRUISE_MIN_V_ECO
+      max_v = _DP_CRUISE_MAX_V_ECO
+    elif self._personality == AccelPersonality.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]) -> tuple[float, float]:
+    return accel_limits if self._personality == AccelPersonality.stock else self._dp_calc_cruise_accel_limits(v_ego)
+
+  def is_enabled(self, accel_personality: int = AccelPersonality.stock) -> bool:
+    self._personality = accel_personality
+    return self._personality != AccelPersonality.stock

selfdrive/ui/qt/offroad/settings.cc

@@ -125,6 +125,16 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
                                           380);
   long_personality_setting->showDescription();
 
+  // accel controller
+  std::vector<QString> accel_personality_texts{tr("Sport"), tr("Normal"), tr("Eco"), tr("Stock")};
+  accel_personality_setting = new ButtonParamControl("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."),
+                                          "../assets/offroad/icon_blank.png",
+                                          accel_personality_texts);
+  accel_personality_setting->showDescription();
+
   // set up uiState update for personality setting
   QObject::connect(uiState(), &UIState::uiUpdate, this, &TogglesPanel::updateState);
 
@@ -140,6 +150,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
     // insert longitudinal personality after NDOG toggle
     if (param == "DisengageOnAccelerator") {
       addItem(long_personality_setting);
+      addItem(accel_personality_setting);
     }
   }
 
@@ -173,6 +184,14 @@ void TogglesPanel::updateState(const UIState &s) {
     }
     uiState()->scene.personality = personality;
   }
+
+  if (sm.updated("controlsStateSP")) {
+    auto accel_personality = sm["controlsStateSP"].getControlsStateSP().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) {
@@ -228,6 +247,7 @@ void TogglesPanel::updateToggles() {
       experimental_mode_toggle->setEnabled(true);
       experimental_mode_toggle->setDescription(e2e_description);
       long_personality_setting->setEnabled(true);
+      accel_personality_setting->setEnabled(true);
       op_long_toggle->setEnabled(true);
       custom_stock_long_toggle->setEnabled(false);
       params.remove("CustomStockLong");
@@ -237,12 +257,14 @@ void TogglesPanel::updateToggles() {
       op_long_toggle->setEnabled(false);
       experimental_mode_toggle->setEnabled(false);
       long_personality_setting->setEnabled(false);
+      accel_personality_setting->setEnabled(false);
       params.remove("ExperimentalLongitudinalEnabled");
       params.remove("ExperimentalMode");
     } else {
       // no long for now
       experimental_mode_toggle->setEnabled(false);
       long_personality_setting->setEnabled(false);
+      accel_personality_setting->setEnabled(false);
       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

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

selfdrive/ui/qt/onroad_settings.cc

@@ -65,6 +65,10 @@ OnroadSettings::OnroadSettings(bool closeable, QWidget *parent) : QFrame(parent)
   options_layout->addWidget(gac_widget = new OptionWidget(this));
   QObject::connect(gac_widget, &OptionWidget::updateParam, this, &OnroadSettings::changeGapAdjustCruise);
 
+  // Acceleration Personality
+  options_layout->addWidget(ap_widget = new OptionWidget(this));
+  QObject::connect(ap_widget, &OptionWidget::updateParam, this, &OnroadSettings::changeAccelerationPersonality);
+
   // Dynamic Personality
   options_layout->addWidget(dynamic_personality_widget = new OptionWidget(this));
   QObject::connect(dynamic_personality_widget, &OptionWidget::updateParam, this, &OnroadSettings::changeDynamicPersonality);
@@ -136,6 +140,18 @@ void OnroadSettings::changeGapAdjustCruise() {
   refresh();
 }
 
+void OnroadSettings::changeAccelerationPersonality() {
+  UIScene &scene = uiState()->scene;
+  const auto cp = (*uiState()->sm)["carParams"].getCarParams();
+  bool can_change = hasLongitudinalControl(cp);
+  if (can_change) {
+    scene.longitudinal_accel_personality--;
+    scene.longitudinal_accel_personality = scene.longitudinal_accel_personality < 0 ? 3 : scene.longitudinal_accel_personality;
+    params.put("AccelPersonality", std::to_string(scene.longitudinal_accel_personality));
+  }
+  refresh();
+}
+
 void OnroadSettings::changeDynamicPersonality() {
   UIScene &scene = uiState()->scene;
   const auto cp = (*uiState()->sm)["carParams"].getCarParams();
@@ -190,6 +206,7 @@ void OnroadSettings::showEvent(QShowEvent *event) {
 void OnroadSettings::refresh() {
   param_watcher->addParam("DynamicLaneProfile");
   param_watcher->addParam("LongitudinalPersonality");
+  param_watcher->addParam("AccelPersonality");
   param_watcher->addParam("DynamicPersonality");
   param_watcher->addParam("DynamicExperimentalControl");
   param_watcher->addParam("EnableSlc");
@@ -198,6 +215,7 @@ void OnroadSettings::refresh() {
   // Update live params on Feature Status on camera view
   scene.dynamic_lane_profile = std::atoi(params.get("DynamicLaneProfile").c_str());
   scene.longitudinal_personality = std::atoi(params.get("LongitudinalPersonality").c_str());
+  scene.longitudinal_accel_personality = std::atoi(params.get("AccelPersonality").c_str());
   scene.dynamic_personality = params.getBool("DynamicPersonality");
   scene.dynamic_experimental_control = params.getBool("DynamicExperimentalControl");
   scene.speed_limit_control_enabled = params.getBool("EnableSlc");
@@ -217,6 +235,10 @@ void OnroadSettings::refresh() {
   gac_widget->updateGapAdjustCruise("LongitudinalPersonality");
   gac_widget->setVisible(hasLongitudinalControl(cp));
 
+  // Acceleration Personality
+  ap_widget->updateAccelerationPersonality("AccelPersonality");
+  ap_widget->setVisible(hasLongitudinalControl(cp));
+
   // Dynamic Personality
   dynamic_personality_widget->updateDynamicPersonality("DynamicPersonality");
   dynamic_personality_widget->setVisible(hasLongitudinalControl(cp));
@@ -330,6 +352,35 @@ void OptionWidget::updateGapAdjustCruise(QString param) {
   setStyleSheet(styleSheet());
 }
 
+void OptionWidget::updateAccelerationPersonality(QString param) {
+  auto icon_color = "#3B4356";
+  auto title_text = "";
+  auto subtitle_text = "Acceleration Personality";
+  auto ap = atoi(params.get(param.toStdString()).c_str());
+
+  if (ap == 0) {
+    title_text = "Sport";
+    icon_color = "#ff4b4b";
+  } else if (ap == 1) {
+    title_text = "Normal";
+    icon_color = "#fcff4b";
+  } else if (ap == 2) {
+    title_text = "Eco";
+    icon_color = "#4bff66";
+  } else if (ap == 3) {
+    title_text = "Stock";
+    icon_color = "#6a0ac9";
+  }
+
+  icon->setStyleSheet(QString("QLabel#icon { background-color: %1; border-radius: 34px; }").arg(icon_color));
+
+  title->setText(title_text);
+  subtitle->setText(subtitle_text);
+  subtitle->setVisible(true);
+
+  setStyleSheet(styleSheet());
+}
+
 void OptionWidget::updateDynamicPersonality(QString param) {
   auto icon_color = "#3B4356";
   auto title_text = "";

selfdrive/ui/qt/onroad_settings.h

@@ -19,6 +19,7 @@ public:
   explicit OnroadSettings(bool closeable = false, QWidget *parent = nullptr);
   void changeDynamicLaneProfile();
   void changeGapAdjustCruise();
+  void changeAccelerationPersonality();
   void changeDynamicPersonality();
   void changeDynamicExperimentalControl();
   void changeSpeedLimitControl();
@@ -32,6 +33,7 @@ private:
   QVBoxLayout *options_layout;
   OptionWidget *dlp_widget;
   OptionWidget *gac_widget;
+  OptionWidget *ap_widget;
   OptionWidget *dynamic_personality_widget;
   OptionWidget *dec_widget;
   OptionWidget *slc_widget;
@@ -48,6 +50,7 @@ public:
   explicit OptionWidget(QWidget *parent = nullptr);
   void updateDynamicLaneProfile(QString param);
   void updateGapAdjustCruise(QString param);
+  void updateAccelerationPersonality(QString param);
   void updateDynamicPersonality(QString param);
   void updateDynamicExperimentalControl(QString param);
   void updateSpeedLimitControl(QString param);

selfdrive/ui/ui.h

@@ -138,6 +138,7 @@ typedef struct UIScene {
 
   bool navigate_on_openpilot_deprecated = false;
   cereal::LongitudinalPersonality personality;
+  cereal::AccelerationPersonality accel_personality;
 
   float light_sensor = -1;
   bool started, ignition, is_metric, map_on_left, longitudinal_control;
@@ -162,6 +163,7 @@ typedef struct UIScene {
 
   bool gac;
   int longitudinal_personality;
+  int longitudinal_accel_personality;
 
   bool map_visible;
   int dev_ui_info;

system/manager/manager.py

@@ -43,6 +43,7 @@ def manager_init() -> None:
     ("OpenpilotEnabledToggle", "1"),
     ("LongitudinalPersonality", str(custom.LongitudinalPersonalitySP.standard)),
 
+    ("AccelPersonality", str(custom.AccelerationPersonality.stock)),
     ("AccMadsCombo", "1"),
     ("AutoLaneChangeTimer", "0"),
     ("AutoLaneChangeBsmDelay", "1"),