From 972951de3efc6caaab152f77f2a058fb04b38c90 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 29 Jun 2024 11:52:15 -0700 Subject: [PATCH] accel controller --- common/params.cc | 2 +- .../controls/lib/longitudinal_planner.py | 21 ++++- .../lib/sunnypilot/accel_controller.py | 76 +++++++++++++++++++ .../dynamic_experimental_controller.py | 0 selfdrive/ui/qt/offroad/settings.cc | 8 ++ system/manager/manager.py | 1 + 6 files changed, 106 insertions(+), 2 deletions(-) create mode 100644 selfdrive/controls/lib/sunnypilot/accel_controller.py rename selfdrive/controls/lib/{ => sunnypilot}/dynamic_experimental_controller.py (100%) diff --git a/common/params.cc b/common/params.cc index 73906d35f4..473ea89a77 100644 --- a/common/params.cc +++ b/common/params.cc @@ -209,7 +209,7 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, - + {"AccelProfile", PERSISTENT | BACKUP}, {"AccMadsCombo", PERSISTENT | BACKUP}, {"AmapKey1", PERSISTENT | BACKUP}, {"AmapKey2", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9f508fb76b..6b7c56d82e 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.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 diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py new file mode 100644 index 0000000000..e6911b4a6c --- /dev/null +++ b/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 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 4c9a2e9085..4fd4b77aa1 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/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 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); } } diff --git a/system/manager/manager.py b/system/manager/manager.py index f3a5b1b366..d431170b8d 100755 --- a/system/manager/manager.py +++ b/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')))