diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 5f0e6a3b17..362734b7a3 100644 --- a/CHANGELOGS.md +++ b/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) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 399679d03c..b557ef659b 100644 --- a/cereal/custom.capnp +++ b/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; diff --git a/common/params.cc b/common/params.cc index 21445fede8..172f99b2e5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -210,6 +210,7 @@ std::unordered_map keys = { {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + {"AccelProfile", PERSISTENT | BACKUP}, {"AccMadsCombo", PERSISTENT | BACKUP}, {"AmapKey1", PERSISTENT | BACKUP}, {"AmapKey2", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0b0bfb89f0..96178ccdd7 100755 --- a/selfdrive/controls/controlsd.py +++ b/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") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 636d78dfc3..9e554865f0 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_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 diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py new file mode 100644 index 0000000000..8ee1cb6746 --- /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: 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 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 63831f39a6..d846ebe999 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/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 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); } } diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 0e550850e8..0a0a7a9431 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_profile_setting; ParamWatcher *param_watcher; }; diff --git a/system/manager/manager.py b/system/manager/manager.py index 3aacc7d633..973eb4091d 100755 --- a/system/manager/manager.py +++ b/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')))