diff --git a/common/params_keys.h b/common/params_keys.h index a28a02693..6474fb584 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -125,6 +125,8 @@ inline static std::unordered_map keys = { {"ApiCache_DriveStats", PERSISTENT}, {"AutoLaneChangeBsmDelay", PERSISTENT}, {"AutoLaneChangeTimer", PERSISTENT}, + {"BlinkerMinLateralControlSpeed", PERSISTENT | BACKUP}, + {"BlinkerPauseLateralControl", PERSISTENT | BACKUP}, {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsSPCache", CLEAR_ON_MANAGER_START}, {"CarParamsSPPersistent", PERSISTENT}, diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 0353c9bab..796107da7 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -40,6 +40,7 @@ qt_src = [ ] lateral_panel_qt_src = [ + "sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc", "sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc", "sunnypilot/qt/offroad/settings/lateral/mads_settings.cc", "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc", diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc new file mode 100644 index 000000000..b43c6162c --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc @@ -0,0 +1,27 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h" + +BlinkerPauseLateralSettings::BlinkerPauseLateralSettings(const QString ¶m, const QString &title, const QString &description, const QString &icon, QWidget *parent) + : ExpandableToggleRow(param, title, description, icon, parent) { + + pauseLateralSpeed = new OptionControlSP("BlinkerMinLateralControlSpeed", "", "", "", {0, 255}, 5); + connect(pauseLateralSpeed, &OptionControlSP::updateLabels, this, &BlinkerPauseLateralSettings::refresh); + addItem(pauseLateralSpeed); + + refresh(); +} + +void BlinkerPauseLateralSettings::refresh() { + const QString option = QString::fromStdString(params.get("BlinkerMinLateralControlSpeed")); + const bool is_metric = params.getBool("IsMetric"); + const QString unit = is_metric ? "km/h" : "mph"; + + pauseLateralSpeed->setLabel(option + " " + unit); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h new file mode 100644 index 000000000..7f02337b9 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h @@ -0,0 +1,25 @@ +/** + * Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + * + * This file is part of sunnypilot and is licensed under the MIT License. + * See the LICENSE.md file in the root directory for more details. + */ + +#pragma once + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/expandable_row.h" + +class BlinkerPauseLateralSettings : public ExpandableToggleRow { + Q_OBJECT + +public: + BlinkerPauseLateralSettings(const QString ¶m, const QString &title, const QString &description, const QString &icon, QWidget *parent = nullptr); + void refresh(); + +private: + Params params; + OptionControlSP *pauseLateralSpeed; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index 0beae6e24..ae954d753 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -64,6 +64,17 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { list->addItem(vertical_space(0)); list->addItem(horizontal_line()); + // Blinker Pause Lateral Control + blinkerPauseLateralSettings = new BlinkerPauseLateralSettings( + "BlinkerPauseLateralControl", + tr("Pause Lateral Control with Blinker"), + tr("Pause lateral control with blinker when traveling below the desired speed selected."), + "", + this); + list->addItem(blinkerPauseLateralSettings); + + list->addItem(horizontal_line()); + // Neural Network Lateral Control nnlcToggle = new NeuralNetworkLateralControl(); list->addItem(nnlcToggle); @@ -139,5 +150,7 @@ void LateralPanel::updateToggles(bool _offroad) { madsSettingsButton->setEnabled(madsToggle->isToggled()); + blinkerPauseLateralSettings->refresh(); + offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index cf003dfaf..7b5e73fce 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -11,6 +11,7 @@ #include #include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/mads_settings.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h" @@ -43,6 +44,7 @@ private: PushButtonSP *laneChangeSettingsButton; LaneChangeSettings *laneChangeWidget = nullptr; NeuralNetworkLateralControl *nnlcToggle = nullptr; + BlinkerPauseLateralSettings *blinkerPauseLateralSettings = nullptr; const QString MADS_BASE_DESC = tr("Enables independent engagements of Automatic Lane Centering (ALC) and Adaptive Cruise Control (ACC)."); diff --git a/sunnypilot/selfdrive/controls/controlsd_ext.py b/sunnypilot/selfdrive/controls/controlsd_ext.py index 4978c0133..9a7f0cb5f 100644 --- a/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -11,12 +11,14 @@ from opendbc.car import structs from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.sunnypilot.selfdrive.controls.lib.param_store import ParamStore +from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral class ControlsExt: def __init__(self, CP: structs.CarParams, params: Params): self.CP = CP self.params = params + self.blinker_pause_lateral = BlinkerPauseLateral() self.param_store = ParamStore(self.CP) self.get_params_sp() @@ -29,11 +31,13 @@ class ControlsExt: def get_params_sp(self) -> None: self.param_store.update(self.params) + self.blinker_pause_lateral.get_params() + + def get_lat_active(self, sm: messaging.SubMaster) -> bool: + if self.blinker_pause_lateral.update(sm['carState']): + return False - @staticmethod - def get_lat_active(sm: messaging.SubMaster) -> bool: ss_sp = sm['selfdriveStateSP'] - if ss_sp.mads.available: return bool(ss_sp.mads.active) diff --git a/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py b/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py new file mode 100644 index 000000000..fb8628035 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py @@ -0,0 +1,34 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import car + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params + + +class BlinkerPauseLateral: + def __init__(self): + self.params = Params() + + self.enabled = self.params.get_bool("BlinkerPauseLateralControl") + self.is_metric = self.params.get_bool("IsMetric") + self.min_speed = 0 + + def get_params(self) -> None: + self.enabled = self.params.get_bool("BlinkerPauseLateralControl") + self.is_metric = self.params.get_bool("IsMetric") + self.min_speed = int(self.params.get("BlinkerMinLateralControlSpeed", encoding='utf8')) + + def update(self, CS: car.CarState) -> bool: + if not self.enabled: + return False + + one_blinker = CS.leftBlinker != CS.rightBlinker + speed_factor = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS + min_speed_ms = self.min_speed * speed_factor + + return bool(one_blinker and CS.vEgo < min_speed_ms) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py b/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py new file mode 100644 index 000000000..6ba1dcef4 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py @@ -0,0 +1,130 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from cereal import car + +from openpilot.common.conversions import Conversions as CV +from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral + + +class TestBlinkerPauseLateral: + + def setup_method(self): + self.blinker_pause_lateral = BlinkerPauseLateral() + self._reset_states() + + def _reset_states(self): + self.blinker_pause_lateral.enabled = True + self.blinker_pause_lateral.is_metric = False + self.blinker_pause_lateral.min_speed = 20 # MPH + + self.CS = car.CarState.new_message() + self.CS.vEgo = 0 + self.CS.leftBlinker = False + self.CS.rightBlinker = False + + def _test_should_blinker_pause_lateral(self, expected_results) -> None: + for left in (True, False): + for right in (True, False): + self.CS.leftBlinker = left + self.CS.rightBlinker = right + + result = self.blinker_pause_lateral.update(self.CS) + assert result == expected_results[(left, right)] + + def test_below_min_speed_blinker(self): + self.CS.vEgo = 4.5 # ~10 MPH + + expected_results = { + (False, False): False, + (True, False): True, + (False, True): True, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_above_min_speed_blinker(self): + self.CS.vEgo = 13.4 # ~30 MPH + + expected_results = { + (False, False): False, + (True, False): False, + (False, True): False, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_just_below_min_speed(self): + self.CS.vEgo = (20 * CV.MPH_TO_MS) - 0.01 + + expected_results = { + (False, False): False, + (True, False): True, + (False, True): True, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_disabled(self): + self.blinker_pause_lateral.enabled = False + self.CS.vEgo = 4.5 # ~10 MPH + + expected_results = { + (False, False): False, + (True, False): False, + (False, True): False, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_metric_units_below_min_speed(self): + self.blinker_pause_lateral.is_metric = True + self.CS.vEgo = 5.0 # ~18 km/h + + expected_results = { + (False, False): False, + (True, False): True, + (False, True): True, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_metric_units_above_threshold(self): + self.blinker_pause_lateral.is_metric = True + self.CS.vEgo = 6.0 # ~21.6 km/h + + expected_results = { + (False, False): False, + (True, False): False, + (False, True): False, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + def test_change_min_speed_threshold(self): + self.blinker_pause_lateral.min_speed = 30 # MPH + + # below min speed + self.CS.vEgo = 11.2 # ~25 MPH + + expected_results = { + (False, False): False, + (True, False): True, + (False, True): True, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) + + # above min speed + self.CS.vEgo = 15.6 # ~35 MPH + + expected_results = { + (False, False): False, + (True, False): False, + (False, True): False, + (True, True): False + } + self._test_should_blinker_pause_lateral(expected_results) diff --git a/system/manager/manager.py b/system/manager/manager.py index 71f2bc525..46b849108 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -44,6 +44,8 @@ def manager_init() -> None: sunnypilot_default_params: list[tuple[str, str | bytes]] = [ ("AutoLaneChangeTimer", "0"), ("AutoLaneChangeBsmDelay", "0"), + ("BlinkerMinLateralControlSpeed", "20"), # MPH or km/h + ("BlinkerPauseLateralControl", "0"), ("DynamicExperimentalControl", "0"), ("HyundaiLongitudinalTuning", "0"), ("Mads", "1"),