diff --git a/common/params_keys.h b/common/params_keys.h index fceee5b1c..e30492de4 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -139,6 +139,9 @@ inline static std::unordered_map keys = { {"CarParamsSPCache", CLEAR_ON_MANAGER_START}, {"CarParamsSPPersistent", PERSISTENT}, {"CarPlatformBundle", PERSISTENT}, + {"CustomAccIncrementsEnabled", PERSISTENT | BACKUP}, + {"CustomAccLongPressIncrement", PERSISTENT | BACKUP}, + {"CustomAccShortPressIncrement", PERSISTENT | BACKUP}, {"DeviceBootMode", PERSISTENT | BACKUP}, {"EnableGithubRunner", PERSISTENT | BACKUP}, {"MaxTimeOffroad", PERSISTENT | BACKUP}, diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index de68bd20e..ec21b788e 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -307,6 +307,7 @@ class Car: # sunnypilot self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") + self.v_cruise_helper.read_custom_set_speed_params() time.sleep(0.1) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index b31615a77..a2145415e 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -3,6 +3,7 @@ import numpy as np from cereal import car from openpilot.common.conversions import Conversions as CV +from openpilot.sunnypilot.selfdrive.car.cruise_ext import VCruiseHelperSP # WARNING: this value was determined based on the model's training distribution, @@ -28,8 +29,9 @@ CRUISE_INTERVAL_SIGN = { } -class VCruiseHelper: +class VCruiseHelper(VCruiseHelperSP): def __init__(self, CP): + VCruiseHelperSP.__init__(self) self.CP = CP self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -117,7 +119,7 @@ class VCruiseHelper: if not self.button_change_states[button_type]["enabled"]: return - v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + long_press, v_cruise_delta = VCruiseHelperSP.update_v_cruise_delta(self, long_press, v_cruise_delta) if long_press and self.v_cruise_kph % v_cruise_delta != 0: # partial interval self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta else: diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 5d67f4157..41cc09141 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -49,6 +49,10 @@ lateral_panel_qt_src = [ "sunnypilot/qt/offroad/settings/lateral/neural_network_lateral_control.cc", ] +longitudinal_panel_qt_src = [ + "sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc", +] + network_src = [ "sunnypilot/qt/network/sunnylink/sunnylink_client.cc", "sunnypilot/qt/network/sunnylink/services/base_device_service.cc", @@ -83,7 +87,7 @@ brand_settings_qt_src = [ sp_widgets_src = widgets_src + network_src -sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + osm_panel_qt_src +sp_qt_src = qt_src + lateral_panel_qt_src + vehicle_panel_qt_src + brand_settings_qt_src + longitudinal_panel_qt_src + osm_panel_qt_src sp_qt_util = qt_util Export('sp_widgets_src', 'sp_qt_src', "sp_qt_util") diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc new file mode 100644 index 000000000..29cadeaa6 --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.cc @@ -0,0 +1,47 @@ +/** + * 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/longitudinal/custom_acc_increment.h" + +CustomAccIncrement::CustomAccIncrement(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent) + : ExpandableToggleRow(param, title, desc, icon, parent) { + auto *accFrame = new QFrame(this); + auto *accFrameLayout = new QGridLayout(); + accFrame->setLayout(accFrameLayout); + accFrameLayout->setSpacing(0); + + auto *shortPressControl = new AccIncrementOptionControl("CustomAccShortPressIncrement", {1, 10}, 1); + connect(shortPressControl, &OptionControlSP::updateLabels, shortPressControl, &AccIncrementOptionControl::refresh); + + auto *longPressControl = new AccIncrementOptionControl("CustomAccLongPressIncrement", {1, 3}, 1, &customLongValues); + connect(longPressControl, &OptionControlSP::updateLabels, longPressControl, &AccIncrementOptionControl::refresh); + + shortPressControl->setFixedWidth(280); + longPressControl->setFixedWidth(280); + accFrameLayout->addWidget(shortPressControl, 0, 0, Qt::AlignLeft); + accFrameLayout->addWidget(longPressControl, 0, 1, Qt::AlignRight); + + addItem(accFrame); +} + +AccIncrementOptionControl::AccIncrementOptionControl(const QString ¶m, const MinMaxValue &range, const int per_value_change, const QMap *valMap) + : OptionControlSP(param, "", "", "", range, per_value_change, true, valMap) { + param_name = param.toStdString(); + refresh(); +} + +void AccIncrementOptionControl::refresh() { + std::string val = params.get(param_name); + std::string label = ""; + label += param_name == "CustomAccShortPressIncrement" ? "Short Press" : "Long Press"; + label += "
" + val; + label += param_name == "CustomAccShortPressIncrement" + ? (val == "1" ? " (Default)" : "") + : (val == "5" ? " (Default)" : ""); + label += ""; + setLabel(QString::fromStdString(label)); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h new file mode 100644 index 000000000..707d4143b --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h @@ -0,0 +1,41 @@ +/** + * 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 CustomAccIncrement : public ExpandableToggleRow { + Q_OBJECT + +public: + CustomAccIncrement(const QString ¶m, const QString &title, const QString &desc, const QString &icon, QWidget *parent = nullptr); + +private: + QMap customLongValues = { + {"1", "1"}, + {"2", "5"}, // Default + {"3", "10"} + }; +}; + +class AccIncrementOptionControl : public OptionControlSP { + Q_OBJECT + +public: + AccIncrementOptionControl(const QString ¶m, const MinMaxValue &range, int per_value_change, const QMap *valMap = nullptr); + void refresh(); + +protected: + std::string param_name; + +private: + Params params; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc index 9d8a4d9c1..15ed10bf9 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.cc @@ -8,4 +8,72 @@ #include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h" LongitudinalPanel::LongitudinalPanel(QWidget *parent) : QWidget(parent) { + main_layout = new QStackedLayout(this); + ListWidget *list = new ListWidget(this, false); + + cruisePanelScreen = new QWidget(this); + QVBoxLayout *vlayout = new QVBoxLayout(cruisePanelScreen); + vlayout->setContentsMargins(0, 0, 0, 0); + + cruisePanelScroller = new ScrollViewSP(list, this); + vlayout->addWidget(cruisePanelScroller); + + customAccIncrement = new CustomAccIncrement("CustomAccIncrementsEnabled", tr("Custom ACC Speed Increments"), "", "", this); + list->addItem(customAccIncrement); + + QObject::connect(uiState(), &UIState::offroadTransition, this, &LongitudinalPanel::refresh); + + main_layout->addWidget(cruisePanelScreen); + main_layout->setCurrentWidget(cruisePanelScreen); + refresh(offroad); +} + +void LongitudinalPanel::showEvent(QShowEvent *event) { + main_layout->setCurrentWidget(cruisePanelScreen); + refresh(offroad); +} + +void LongitudinalPanel::refresh(bool _offroad) { + auto cp_bytes = params.get("CarParamsPersistent"); + if (!cp_bytes.empty()) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); + cereal::CarParams::Reader CP = cmsg.getRoot(); + + has_longitudinal_control = hasLongitudinalControl(CP); + is_pcm_cruise = CP.getPcmCruise(); + } else { + has_longitudinal_control = false; + is_pcm_cruise = false; + } + + QString accEnabledDescription = tr("Enable custom Short & Long press increments for cruise speed increase/decrease."); + QString accNoLongDescription = tr("This feature can only be used with openpilot longitudinal control enabled."); + QString accPcmCruiseDisabledDescription = tr("This feature is not supported on this platform due to vehicle limitations."); + QString onroadOnlyDescription = tr("Start the vehicle to check vehicle compatibility."); + + if (offroad) { + customAccIncrement->setDescription(onroadOnlyDescription); + customAccIncrement->showDescription(); + } else { + if (has_longitudinal_control) { + if (is_pcm_cruise) { + customAccIncrement->setDescription(accPcmCruiseDisabledDescription); + customAccIncrement->showDescription(); + } else { + customAccIncrement->setDescription(accEnabledDescription); + } + } else { + params.remove("CustomAccIncrementsEnabled"); + customAccIncrement->toggleFlipped(false); + customAccIncrement->setDescription(accNoLongDescription); + customAccIncrement->showDescription(); + } + } + + // enable toggle when long is available and is not PCM cruise + customAccIncrement->setEnabled(has_longitudinal_control && !is_pcm_cruise && !offroad); + customAccIncrement->refresh(); + + offroad = _offroad; } diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h index cd68e3ec2..58e94f333 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal_panel.h @@ -7,11 +7,26 @@ #pragma once +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/longitudinal/custom_acc_increment.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" class LongitudinalPanel : public QWidget { Q_OBJECT public: explicit LongitudinalPanel(QWidget *parent = nullptr); + void showEvent(QShowEvent *event) override; + void refresh(bool _offroad); + +private: + Params params; + bool has_longitudinal_control = false; + bool is_pcm_cruise = false; + bool offroad = false; + + QStackedLayout *main_layout = nullptr; + ScrollViewSP *cruisePanelScroller = nullptr; + QWidget *cruisePanelScreen = nullptr; + CustomAccIncrement *customAccIncrement = nullptr; }; diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 536917a1a..d25a86c6d 100755 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -246,10 +246,10 @@ def setup_settings_steering_alc(click, pm: PubMaster, scroll=None): click(970, 534) time.sleep(UI_DELAY) -def setup_settings_driving(click, pm: PubMaster, scroll=None): +def setup_settings_cruise(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) - scroll(-1, 278, 962) - click(278, 962) + scroll(-400, 278, 962) + click(278, 324) time.sleep(UI_DELAY) def setup_settings_visuals(click, pm: PubMaster, scroll=None): @@ -312,7 +312,7 @@ CASES.update({ "settings_steering": setup_settings_steering, "settings_steering_mads": setup_settings_steering_mads, "settings_steering_alc": setup_settings_steering_alc, - "settings_driving": setup_settings_driving, + "settings_cruise": setup_settings_cruise, "settings_visuals": setup_settings_visuals, "settings_trips": setup_settings_trips, "settings_vehicle": setup_settings_vehicle, diff --git a/sunnypilot/selfdrive/car/cruise_ext.py b/sunnypilot/selfdrive/car/cruise_ext.py new file mode 100644 index 000000000..6f0419ed1 --- /dev/null +++ b/sunnypilot/selfdrive/car/cruise_ext.py @@ -0,0 +1,47 @@ +""" +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. +""" +import numpy as np + +from cereal import car +from openpilot.common.params import Params + +ButtonType = car.CarState.ButtonEvent.Type + + +class VCruiseHelperSP: + def __init__(self) -> None: + self.params = Params() + + self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") + self.short_increment = self.read_int_param("CustomAccShortPressIncrement", 1) + self.long_increment = self.read_int_param("CustomAccLongPressIncrement", 5) + + def read_int_param(self, key: str, default: int = 0) -> int: + try: + return int(self.params.get(key, encoding='utf8')) + except (ValueError, TypeError): + return default + + def read_custom_set_speed_params(self) -> None: + self.custom_acc_enabled = self.params.get_bool("CustomAccIncrementsEnabled") + self.short_increment = self.read_int_param("CustomAccShortPressIncrement", 1) + self.long_increment = self.read_int_param("CustomAccLongPressIncrement", 5) + + def update_v_cruise_delta(self, long_press: bool, v_cruise_delta: float) -> tuple[bool, float]: + if not self.custom_acc_enabled: + v_cruise_delta = v_cruise_delta * (5 if long_press else 1) + return long_press, v_cruise_delta + + # Apply user-specified multipliers to the base increment + short_increment = np.clip(self.short_increment, 1, 10) + long_increment = np.clip(self.long_increment, 1, 10) + + actual_increment = long_increment if long_press else short_increment + round_to_nearest = actual_increment in (5, 10) + v_cruise_delta = v_cruise_delta * actual_increment + + return round_to_nearest, v_cruise_delta diff --git a/sunnypilot/selfdrive/car/tests/test_custom_cruise.py b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py new file mode 100644 index 000000000..2f9d97684 --- /dev/null +++ b/sunnypilot/selfdrive/car/tests/test_custom_cruise.py @@ -0,0 +1,149 @@ +import pytest +from parameterized import parameterized_class + +from cereal import car +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params +from openpilot.selfdrive.car.cruise import V_CRUISE_INITIAL +from openpilot.selfdrive.car.tests.test_cruise_speed import TestVCruiseHelper + +ButtonEvent = car.CarState.ButtonEvent +ButtonType = car.CarState.ButtonEvent.Type + + +@parameterized_class(('pcm_cruise',), [(False,)]) +class TestCustomAccIncrements(TestVCruiseHelper): + def setup_method(self): + TestVCruiseHelper.setup_method(self) + self.params = Params() + self.reset_custom_params() + + def reset_custom_params(self) -> None: + """Reset to default custom ACC parameters""" + self.params.put_bool("CustomAccIncrementsEnabled", False) + self.params.put("CustomAccShortPressIncrement", "1") + self.params.put("CustomAccLongPressIncrement", "5") + self.v_cruise_helper.read_custom_set_speed_params() + + def press_button_short(self, button_type: car.CarState.ButtonEvent.Type) -> None: + """Simulate a short button press (press + release)""" + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + + CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + + def press_button_long(self, button_type: car.CarState.ButtonEvent.Type) -> None: + """Simulate a long button press (50+ frames)""" + CS = car.CarState(cruiseState={"available": True}) + CS.buttonEvents = [ButtonEvent(type=button_type, pressed=True)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + + # Hold for 50 frames to trigger long press + CS.buttonEvents = [] + for _ in range(50): + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + + CS.buttonEvents = [ButtonEvent(type=button_type, pressed=False)] + self.v_cruise_helper.update_v_cruise(CS, enabled=True, is_metric=True) + + def set_custom_increments(self, enabled: bool, short_inc: int, long_inc: int) -> None: + """Set custom ACC increment parameters""" + self.params.put_bool("CustomAccIncrementsEnabled", enabled) + self.params.put("CustomAccShortPressIncrement", str(short_inc)) + self.params.put("CustomAccLongPressIncrement", str(long_inc)) + self.v_cruise_helper.read_custom_set_speed_params() + + def test_default_behavior_when_disabled(self): + """Test that default increments are used when custom ACC is disabled""" + self.set_custom_increments(enabled=False, short_inc=5, long_inc=10) + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + + # Short press should increment by 1 (default) + self.press_button_short(ButtonType.accelCruise) + assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1 + + @pytest.mark.parametrize("increment", (1, 2, 3, 4, 5, 6, 7, 8, 9, 10)) + def test_custom_short_press_increments(self, increment): + """Test custom short press increments (1-10)""" + self.set_custom_increments(enabled=True, short_inc=increment, long_inc=5) + self.enable(50 * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + self.press_button_short(ButtonType.accelCruise) + + if increment in (5, 10): + # Should round to nearest increment + expected_speed = ((initial_speed // increment) + 1) * increment + else: + expected_speed = initial_speed + increment + + assert self.v_cruise_helper.v_cruise_kph == expected_speed + + @pytest.mark.parametrize("increment", (1, 5, 10)) + def test_custom_long_press_increments(self, increment): + """Test custom long press increments (1, 5, 10)""" + self.set_custom_increments(enabled=True, short_inc=1, long_inc=increment) + self.enable(50 * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + self.press_button_long(ButtonType.accelCruise) + + if increment in (5, 10): + # Should round to nearest increment + expected_speed = ((initial_speed // increment) + 1) * increment + else: + expected_speed = initial_speed + increment + + assert self.v_cruise_helper.v_cruise_kph == expected_speed + + @pytest.mark.parametrize("button_type", [ButtonType.accelCruise, ButtonType.decelCruise]) + def test_accel_decel_symmetry(self, button_type): + """Test that acceleration and deceleration work symmetrically""" + self.set_custom_increments(enabled=True, short_inc=3, long_inc=5) + self.enable(50 * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + self.press_button_short(button_type) + + expected_change = 3 if button_type == ButtonType.accelCruise else -3 + assert self.v_cruise_helper.v_cruise_kph == initial_speed + expected_change + + def test_rounding_behavior(self): + """Test rounding behavior for 5 and 10 increments""" + test_cases = [ + (47, 5, 50), # 47 -> 50 (round up to next 5) + (45, 5, 50), # 45 -> 50 (already at 5, increment by 5) + (43, 10, 50), # 43 -> 50 (round up to next 10) + (40, 10, 50), # 40 -> 50 (already at 10, increment by 10) + ] + + for initial, increment, expected in test_cases: + self.set_custom_increments(enabled=True, short_inc=increment, long_inc=increment) + self.reset_cruise_speed_state() + self.enable(initial * CV.KPH_TO_MS, False, False) + + self.press_button_short(ButtonType.accelCruise) + assert self.v_cruise_helper.v_cruise_kph == expected + + def test_invalid_values_fallback(self): + """Test that invalid values fallback to safe defaults""" + # Test invalid short increment + self.set_custom_increments(enabled=True, short_inc=-1, long_inc=5) + self.enable(50 * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + self.press_button_short(ButtonType.accelCruise) + assert self.v_cruise_helper.v_cruise_kph == initial_speed + 1 # Should fallback to 1 + + # Test invalid long increment + self.reset_cruise_speed_state() + self.set_custom_increments(enabled=True, short_inc=1, long_inc=99) + self.enable(50 * CV.KPH_TO_MS, False, False) + + initial_speed = self.v_cruise_helper.v_cruise_kph + self.press_button_long(ButtonType.accelCruise) + assert self.v_cruise_helper.v_cruise_kph == initial_speed + 10 # Should fallback to 10 diff --git a/system/manager/manager.py b/system/manager/manager.py index 31bb43e3c..1778c4e0d 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -49,6 +49,9 @@ def manager_init() -> None: ("BlindSpot", "0"), ("BlinkerMinLateralControlSpeed", "20"), # MPH or km/h ("BlinkerPauseLateralControl", "0"), + ("CustomAccIncrementsEnabled", "0"), + ("CustomAccLongPressIncrement", "5"), + ("CustomAccShortPressIncrement", "1"), ("DeviceBootMode", "0"), ("DynamicExperimentalControl", "0"), ("HyundaiLongitudinalTuning", "0"),