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 01/11] 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'))) From 6098dd300a3ef4b3cc797eefd8d6fce38e7f277c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 11:18:47 -0400 Subject: [PATCH 02/11] formatting --- common/params.cc | 1 + .../controls/lib/longitudinal_planner.py | 2 +- .../lib/sunnypilot/accel_controller.py | 53 +++++++++---------- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/common/params.cc b/common/params.cc index 473ea89a77..49a8cba9b9 100644 --- a/common/params.cc +++ b/common/params.cc @@ -209,6 +209,7 @@ std::unordered_map keys = { {"UpdaterTargetBranch", CLEAR_ON_MANAGER_START}, {"UpdaterLastFetchTime", PERSISTENT}, {"Version", PERSISTENT}, + {"AccelProfile", PERSISTENT | BACKUP}, {"AccMadsCombo", PERSISTENT | BACKUP}, {"AmapKey1", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6b7c56d82e..11bf18c363 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -19,8 +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.sunnypilot.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 diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index e6911b4a6c..26ad7f6692 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -21,7 +21,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. -# Last update: June 5, 2024 +# Last updated: June 5, 2024 from openpilot.common.numpy_fast import interp @@ -43,34 +43,33 @@ _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 __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 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 - 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 - 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 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 + def is_enabled(self): + return self._profile != DP_ACCEL_STOCK From 6e6de6742e5c7071507101ec4589d1243ff8ebda Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 11:51:01 -0400 Subject: [PATCH 03/11] some refactors --- .../controls/lib/longitudinal_planner.py | 6 ++-- .../lib/sunnypilot/accel_controller.py | 33 +++++++++++-------- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 11bf18c363..382af48397 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -106,8 +106,10 @@ class LongitudinalPlanner: def read_param(self): try: self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) + self.accel_controller.set_profile(int(self.params.get("AccelProfile"))) except AttributeError: self.dynamic_experimental_controller = DynamicExperimentalController() + self.accel_controller = AccelController() @staticmethod def parse_model(model_msg, model_error): @@ -129,7 +131,6 @@ 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: @@ -169,7 +170,8 @@ class LongitudinalPlanner: 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] + accel_limits = [ACCEL_MIN, ACCEL_MAX] + accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] if reset_state: self.v_desired_filter.x = v_ego diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index 26ad7f6692..7c40529dec 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -25,11 +25,6 @@ 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] @@ -42,22 +37,32 @@ _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 DPAccel: + STOCK = 0 + ECO = 1 + NORMAL = 2 + SPORT = 3 + + @classmethod + def accel_val(cls): + return cls.STOCK, cls.ECO, cls.NORMAL, cls.SPORT + + class AccelController: def __init__(self): - # self._params = Params() - self._profile = DP_ACCEL_STOCK + self._profile = DPAccel.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 + self._profile = profile if profile in DPAccel.accel_val() else DPAccel.STOCK + except (ValueError, TypeError): + self._profile = DPAccel.STOCK def _dp_calc_cruise_accel_limits(self, v_ego): - if self._profile == DP_ACCEL_ECO: + if self._profile == DPAccel.ECO: min_v = _DP_CRUISE_MIN_V_ECO max_v = _DP_CRUISE_MAX_V_ECO - elif self._profile == DP_ACCEL_SPORT: + elif self._profile == DPAccel.SPORT: min_v = _DP_CRUISE_MIN_V_SPORT max_v = _DP_CRUISE_MAX_V_SPORT else: @@ -69,7 +74,7 @@ class AccelController: 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) + return accel_limits if self._profile == DPAccel.STOCK else self._dp_calc_cruise_accel_limits(v_ego) def is_enabled(self): - return self._profile != DP_ACCEL_STOCK + return self._profile != DPAccel.STOCK From 7d751da1b836e64e795fb59b4bf79e1116ec8e7b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 11:58:11 -0400 Subject: [PATCH 04/11] enforce type hint --- selfdrive/controls/lib/sunnypilot/accel_controller.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index 7c40529dec..95e5174660 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -52,13 +52,13 @@ class AccelController: def __init__(self): self._profile = DPAccel.STOCK - def set_profile(self, profile): + def set_profile(self, profile: int): try: self._profile = profile if profile in DPAccel.accel_val() else DPAccel.STOCK except (ValueError, TypeError): self._profile = DPAccel.STOCK - def _dp_calc_cruise_accel_limits(self, v_ego): + def _dp_calc_cruise_accel_limits(self, v_ego: float): if self._profile == DPAccel.ECO: min_v = _DP_CRUISE_MIN_V_ECO max_v = _DP_CRUISE_MAX_V_ECO @@ -71,9 +71,10 @@ class AccelController: 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): + def get_accel_limits(self, v_ego: float, accel_limits: list[float]): return accel_limits if self._profile == DPAccel.STOCK else self._dp_calc_cruise_accel_limits(v_ego) def is_enabled(self): From 13475140ec17d7f2ffa493fe940a3fe462e6af21 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 12:01:06 -0400 Subject: [PATCH 05/11] inline --- selfdrive/controls/lib/longitudinal_planner.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 382af48397..29e21378ce 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -156,16 +156,13 @@ class LongitudinalPlanner: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] - # override accel using Accel controller + # 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] + # 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: From 7a1533eae05823d5c5bc7d134f4645bde066759c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 12:06:15 -0400 Subject: [PATCH 06/11] ui change --- selfdrive/ui/qt/offroad/settings.cc | 9 +++++---- selfdrive/ui/qt/offroad/settings.h | 1 + 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4fd4b77aa1..b39b4fc6d1 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -119,11 +119,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { 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."), - "", + 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); 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; }; From 6afa49be9182c10a32592c6014109a005307c2fe Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 12:31:48 -0400 Subject: [PATCH 07/11] use custom cereal --- cereal/custom.capnp | 8 +++++ selfdrive/controls/controlsd.py | 10 ++++++ .../controls/lib/longitudinal_planner.py | 3 +- .../lib/sunnypilot/accel_controller.py | 33 ++++++------------- 4 files changed, 29 insertions(+), 25 deletions(-) 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/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21bf73cf9a..c4ed4bd71b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -187,6 +187,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) @@ -858,6 +860,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 @@ -906,11 +909,18 @@ 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 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 29e21378ce..c74f93c006 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -106,7 +106,6 @@ class LongitudinalPlanner: def read_param(self): try: self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) - self.accel_controller.set_profile(int(self.params.get("AccelProfile"))) except AttributeError: self.dynamic_experimental_controller = DynamicExperimentalController() self.accel_controller = AccelController() @@ -157,7 +156,7 @@ class LongitudinalPlanner: accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] # override accel using Accel Controller - if self.accel_controller.is_enabled(): + 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': diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index 95e5174660..ec006d3310 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -23,8 +23,11 @@ # 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.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] @@ -37,32 +40,15 @@ _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 DPAccel: - STOCK = 0 - ECO = 1 - NORMAL = 2 - SPORT = 3 - - @classmethod - def accel_val(cls): - return cls.STOCK, cls.ECO, cls.NORMAL, cls.SPORT - - class AccelController: def __init__(self): - self._profile = DPAccel.STOCK - - def set_profile(self, profile: int): - try: - self._profile = profile if profile in DPAccel.accel_val() else DPAccel.STOCK - except (ValueError, TypeError): - self._profile = DPAccel.STOCK + self._profile = AccelProfile.stock def _dp_calc_cruise_accel_limits(self, v_ego: float): - if self._profile == DPAccel.ECO: + if self._profile == AccelProfile.eco: min_v = _DP_CRUISE_MIN_V_ECO max_v = _DP_CRUISE_MAX_V_ECO - elif self._profile == DPAccel.SPORT: + elif self._profile == AccelProfile.sport: min_v = _DP_CRUISE_MIN_V_SPORT max_v = _DP_CRUISE_MAX_V_SPORT else: @@ -75,7 +61,8 @@ class AccelController: 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 == DPAccel.STOCK else self._dp_calc_cruise_accel_limits(v_ego) + return accel_limits if self._profile == AccelProfile.stock else self._dp_calc_cruise_accel_limits(v_ego) - def is_enabled(self): - return self._profile != DPAccel.STOCK + def is_enabled(self, accel_profile: int = AccelProfile.stock): + self._profile = accel_profile + return self._profile != AccelProfile.stock From c9ac0993b7d3c9e73619b69838a34f8a1b2ab265 Mon Sep 17 00:00:00 2001 From: Brian Brown Date: Mon, 1 Jul 2024 17:59:11 +0000 Subject: [PATCH 08/11] Update CHANGELOGS.md --- CHANGELOGS.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 005afc9e8d..686d87178e 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) From d7545ddeb746c876094d9895f9ddcd8fca2119a5 Mon Sep 17 00:00:00 2001 From: Brian Brown Date: Mon, 1 Jul 2024 18:00:23 +0000 Subject: [PATCH 09/11] fix space --- CHANGELOGS.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOGS.md b/CHANGELOGS.md index 686d87178e..dc50a9257a 100644 --- a/CHANGELOGS.md +++ b/CHANGELOGS.md @@ -1,7 +1,7 @@ sunnypilot - 0.9.8.0 (2024-xx-xx) ======================== * Always on driver monitoring toggle -*NEW❗: Acceleration Profile thanks to kegman, rav4kumar, and arne1282 +* 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! From 0ae9eb274cfd588a97628cf883377f9eafbea37d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 15:34:35 -0400 Subject: [PATCH 10/11] in plannerd --- selfdrive/controls/plannerd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 11192b8791..640251f163 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -35,7 +35,7 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'longitudinalPlanSP'] + lateral_planner_svs) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'longitudinalPlan', 'navInstruction', 'longitudinalPlanSP', - 'liveMapDataSP', 'e2eLongStateSP'] + lateral_planner_svs, + 'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP'] + lateral_planner_svs, poll='modelV2', ignore_avg_freq=['radarState']) while True: From 69ae0445b7291e17c1a507222d3a6721456a93b2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 16:20:37 -0400 Subject: [PATCH 11/11] update tuning --- .../controls/lib/sunnypilot/accel_controller.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/sunnypilot/accel_controller.py b/selfdrive/controls/lib/sunnypilot/accel_controller.py index ec006d3310..8ee1cb6746 100644 --- a/selfdrive/controls/lib/sunnypilot/accel_controller.py +++ b/selfdrive/controls/lib/sunnypilot/accel_controller.py @@ -29,13 +29,13 @@ from openpilot.common.numpy_fast import interp AccelProfile = custom.AccelerationProfile # 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_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.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 = [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.]