From e8df8fad00af198ba405fccc3bb1d80272f85d0f Mon Sep 17 00:00:00 2001 From: rav4 kumar Date: Tue, 2 Jul 2024 04:45:18 +0000 Subject: [PATCH] Longitudinal: Acceleration Personality --- CHANGELOGS.md | 3 + cereal/custom.capnp | 8 +++ common/params.cc | 1 + selfdrive/controls/controlsd.py | 10 +++ .../controls/lib/longitudinal_planner.py | 19 +++++- .../lib/sunnypilot/accel_controller.py | 68 +++++++++++++++++++ .../dynamic_experimental_controller.py | 0 selfdrive/ui/qt/offroad/settings.cc | 22 ++++++ selfdrive/ui/qt/offroad/settings.h | 1 + selfdrive/ui/qt/onroad_settings.cc | 51 ++++++++++++++ selfdrive/ui/qt/onroad_settings.h | 3 + selfdrive/ui/ui.h | 2 + system/manager/manager.py | 1 + 13 files changed, 188 insertions(+), 1 deletion(-) create mode 100644 selfdrive/controls/lib/sunnypilot/accel_controller.py rename selfdrive/controls/lib/{ => sunnypilot}/dynamic_experimental_controller.py (100%) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 984c94b010..690034ece9 100644 --- a/CHANGELOGS.md +++ b/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 diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 1e74981450..3b36532ac9 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params.cc b/common/params.cc index 07741914d0..15a0d4d2d6 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,6 +210,7 @@ std::unordered_map keys = { {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + {"AccelPersonality", PERSISTENT | BACKUP}, {"AccMadsCombo", PERSISTENT | BACKUP}, {"AmapKey1", PERSISTENT | BACKUP}, {"AmapKey2", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bf1875b000..dc676f58af 100755 --- a/selfdrive/controls/controlsd.py +++ b/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") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f134a0d1c3..3d3b5cde9c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py new file mode 100644 index 0000000000..6561fdb8c0 --- /dev/null +++ b/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 diff --git a/selfdrive/controls/lib/dynamic_experimental_controller.py b/selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py similarity index 100% rename from selfdrive/controls/lib/dynamic_experimental_controller.py rename to selfdrive/controls/lib/sunnypilot/dynamic_experimental_controller.py diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 00595a32e7..f9f852a3b4 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/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 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(accel_personality)); + } + uiState()->scene.accel_personality = accel_personality; + } } void TogglesPanel::expandToggleDescription(const QString ¶m) { @@ -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."); diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 0e550850e8..06fbe8bfb9 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -96,6 +96,7 @@ private: Params params; std::map toggles; ButtonParamControl *long_personality_setting; + ButtonParamControl *accel_personality_setting; ParamWatcher *param_watcher; }; diff --git a/selfdrive/ui/qt/onroad_settings.cc b/selfdrive/ui/qt/onroad_settings.cc index a9c0affff6..d5fd2d890d 100644 --- a/selfdrive/ui/qt/onroad_settings.cc +++ b/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 = ""; diff --git a/selfdrive/ui/qt/onroad_settings.h b/selfdrive/ui/qt/onroad_settings.h index 3b89013e53..fe881d1f07 100644 --- a/selfdrive/ui/qt/onroad_settings.h +++ b/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); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 32a0890ad5..19387d625e 100644 --- a/selfdrive/ui/ui.h +++ b/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; diff --git a/system/manager/manager.py b/system/manager/manager.py index 3db3c02b9b..388198736c 100755 --- a/system/manager/manager.py +++ b/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"),