From 75b2eca5dfe496a2c54abad218fa354f61a0264b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 4 Jun 2025 23:25:15 -0400 Subject: [PATCH 1/9] mypy my bt --- .../speed_limit_controller.py | 24 +++++++++---------- .../speed_limit_resolver.py | 7 ++++-- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index f2c704f606..e5762673ef 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -110,16 +110,16 @@ class SpeedLimitController: @property def speed_limit_offseted(self) -> float: - return self._speed_limit + self.speed_limit_offset + return float(self._speed_limit + self.speed_limit_offset) @property def speed_limit_offset(self) -> float: if self._offset_type == OffsetType.default: return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) elif self._offset_type == OffsetType.fixed: - return self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) + return float(self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) elif self._offset_type == OffsetType.percentage: - return self._offset_value * 0.01 * self._speed_limit + return float(self._offset_value * 0.01 * self._speed_limit) return 0. @property @@ -127,22 +127,22 @@ class SpeedLimitController: if self._warning_offset_type == OffsetType.default: return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) elif self._warning_offset_type == OffsetType.fixed: - return self._warning_offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) + return float(self._warning_offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) elif self._warning_offset_type == OffsetType.percentage: - return self._warning_offset_value * 0.01 * self._speed_limit + return float(self._warning_offset_value * 0.01 * self._speed_limit) return 0. @property def speed_limit(self) -> float: - return self._speed_limit + return float(self._speed_limit) @property def distance(self) -> float: - return self._distance + return float(self._distance) @property def source(self) -> Source: - return self._source + return Source(self._source) def _update_v_cruise_setpoint_prev(self) -> None: self._v_cruise_setpoint_prev = self._v_cruise_setpoint @@ -272,18 +272,18 @@ class SpeedLimitController: def get_current_acceleration_as_target(self) -> float: """ When state is inactive or tempInactive, preserve current acceleration """ - return self._a_ego + return float(self._a_ego) def get_adapting_state_target_acceleration(self) -> float: """ In adapting state, calculate target acceleration based on speed limit and current velocity """ if self.distance > 0: - return (self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance) + return float((self.speed_limit_offseted ** 2 - self._v_ego ** 2) / (2. * self.distance)) - return self._v_offset / ModelConstants.T_IDXS[CONTROL_N] + return float(self._v_offset / ModelConstants.T_IDXS[CONTROL_N]) def get_active_state_target_acceleration(self) -> float: """ In active state, aim to keep speed constant around control time horizon """ - return self._v_offset / ModelConstants.T_IDXS[CONTROL_N] + return float(self._v_offset / ModelConstants.T_IDXS[CONTROL_N]) def _update_events(self, events_sp: EventsSP) -> None: if self._speed_limit > 0 and self._warning_type == 2 and \ diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index d04b8ae923..65c1a97c9f 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -7,9 +7,12 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers class SpeedLimitResolver: + _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources + _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources + def __init__(self, policy: Policy): - self._limit_solutions = {} # Store for speed limit solutions from different sources - self._distance_solutions = {} # Store for distance to current speed limit start for different sources + self._limit_solutions = {} + self._distance_solutions = {} self._policy = policy self._policy_to_sources_map = { From 57dc9152bb48b0a2bd158fa90d585d0263cea27f Mon Sep 17 00:00:00 2001 From: James Vecellio-Grant <159560811+Discountchubbs@users.noreply.github.com> Date: Wed, 4 Jun 2025 21:10:26 -0700 Subject: [PATCH 2/9] Networking: Fix Prime Level Five For Users in Roaming Only Networks (#986) Fix prime level five for users in roaming only networks. --- selfdrive/ui/qt/network/networking.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/ui/qt/network/networking.cc b/selfdrive/ui/qt/network/networking.cc index 41527fe692..7754e6e5ba 100644 --- a/selfdrive/ui/qt/network/networking.cc +++ b/selfdrive/ui/qt/network/networking.cc @@ -79,7 +79,8 @@ Networking::Networking(QWidget* parent, bool show_advanced) : QFrame(parent) { } void Networking::setPrimeType(PrimeState::Type type) { - an->setGsmVisible(type == PrimeState::PRIME_TYPE_NONE || type == PrimeState::PRIME_TYPE_LITE); + an->setGsmVisible(type == PrimeState::PRIME_TYPE_NONE || type == PrimeState::PRIME_TYPE_UNKNOWN || \ + type == PrimeState::PRIME_TYPE_PURPLE || type == PrimeState::PRIME_TYPE_LITE); wifi->ipv4_forward = (type == PrimeState::PRIME_TYPE_NONE || type == PrimeState::PRIME_TYPE_LITE); } From 0c506c868d43251f82bf33580f5ba44078d7a4dc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 00:21:58 -0400 Subject: [PATCH 3/9] Controls: Pause lateral control based on blinker state and vehicle speed (#983) * Controls: Pause lateral control based on blinker state and vehicle speed * in its own module * tests * cleanup tests * ui * always refresh on show panel * remove default * change to 20 default * need to update params live * shorter * off by default * lol lint * use ExpandableToggleRow * not needed * shorter * ci ui preview delay for all offroad * more * Revert "more" This reverts commit 809cfd99dac4183e318f3f7dff8e7c14f8223d62. * Revert "ci ui preview delay for all offroad" This reverts commit ab38292fa84165081e32fcfc7b44f7f7afdfe791. --------- Co-authored-by: nayan8teen --- common/params_keys.h | 2 + selfdrive/ui/sunnypilot/SConscript | 1 + .../lateral/blinker_pause_lateral_settings.cc | 27 ++++ .../lateral/blinker_pause_lateral_settings.h | 25 ++++ .../qt/offroad/settings/lateral_panel.cc | 13 ++ .../qt/offroad/settings/lateral_panel.h | 2 + .../selfdrive/controls/controlsd_ext.py | 10 +- .../controls/lib/blinker_pause_lateral.py | 34 +++++ .../lib/tests/test_blinker_pause_lateral.py | 130 ++++++++++++++++++ system/manager/manager.py | 2 + 10 files changed, 243 insertions(+), 3 deletions(-) create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/blinker_pause_lateral_settings.h create mode 100644 sunnypilot/selfdrive/controls/lib/blinker_pause_lateral.py create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_blinker_pause_lateral.py diff --git a/common/params_keys.h b/common/params_keys.h index a28a02693b..6474fb5840 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 0353c9babd..796107da77 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 0000000000..b43c6162c6 --- /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 0000000000..7f02337b9c --- /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 0beae6e24f..ae954d7539 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 cf003dfaf3..7b5e73fceb 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 4978c01332..9a7f0cb5fc 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 0000000000..fb86280358 --- /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 0000000000..6ba1dcef41 --- /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 71f2bc525a..46b849108c 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"), From 06eaa56a9d1d6cc1d7eebc6dc69380671605bc46 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 01:25:18 -0400 Subject: [PATCH 4/9] policy param catch exceptions --- .../speed_limit_controller/speed_limit_controller.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index e5762673ef..83a65f77d2 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -23,7 +23,7 @@ class SpeedLimitController: def __init__(self, CP): self._params = Params() self._CP = CP - self._policy = Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8'))) + self._policy = self._read_policy_param() self._resolver = SpeedLimitResolver(self._policy) self._last_params_update = 0.0 self._last_op_enabled_time = 0.0 @@ -155,7 +155,7 @@ class SpeedLimitController: self._warning_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8')) self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8')) self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8')) - self._policy = Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8'))) + self._policy = self._read_policy_param() self._is_metric = self._params.get_bool("IsMetric") self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH self._resolver.change_policy(self._policy) @@ -164,6 +164,12 @@ class SpeedLimitController: debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}') self._last_params_update = self._current_time + def _read_policy_param(self) -> Policy: + try: + return Policy(int(self._params.get("SpeedLimitControlPolicy", encoding='utf8'))) + except (ValueError, TypeError): + return Policy.car_state_priority + def _update_calculations(self) -> None: # Update current velocity offset (error) self._v_offset = self.speed_limit_offseted - self._v_ego From 1929a37b1ff2fe48e8d11633145da4f6b17396e8 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 01:49:28 -0400 Subject: [PATCH 5/9] handle all params with exceptions --- .../speed_limit_controller.py | 44 +++++++++++++------ 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 83a65f77d2..b259c8c351 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -1,3 +1,4 @@ +from collections.abc import Callable import numpy as np import time @@ -48,12 +49,12 @@ class SpeedLimitController: self._gas_pressed = False self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8')) - self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8')) - self._warning_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8')) - self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8')) - self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8')) - self._engage_type = np.clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm) + self._offset_type = self._read_int_param("SpeedLimitOffsetType") + self._offset_value = self._read_int_param("SpeedLimitValueOffset") + self._warning_type = self._read_int_param("SpeedLimitWarningType") + self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType") + self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") + self._engage_type = self._read_engage_type_param() self._brake_pressed = False self._brake_pressed_prev = False self._current_time = 0. @@ -150,18 +151,17 @@ class SpeedLimitController: def _update_params(self) -> None: if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD: self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._offset_type = int(self._params.get("SpeedLimitOffsetType", encoding='utf8')) - self._offset_value = float(self._params.get("SpeedLimitValueOffset", encoding='utf8')) - self._warning_type = int(self._params.get("SpeedLimitWarningType", encoding='utf8')) - self._warning_offset_type = int(self._params.get("SpeedLimitWarningOffsetType", encoding='utf8')) - self._warning_offset_value = float(self._params.get("SpeedLimitWarningValueOffset", encoding='utf8')) + self._offset_type = self._read_int_param("SpeedLimitOffsetType") + self._offset_value = self._read_int_param("SpeedLimitValueOffset") + self._warning_type = self._read_int_param("SpeedLimitWarningType") + self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType") + self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") self._policy = self._read_policy_param() self._is_metric = self._params.get_bool("IsMetric") self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH self._resolver.change_policy(self._policy) - self._engage_type = Engage.auto if self._pcm_cruise_op_long else \ - np.clip(int(self._params.get("SpeedLimitEngageType", encoding='utf8')), Engage.auto, Engage.user_confirm) - debug(f'Updated Speed limit params. enabled: {self._is_enabled}, with offset: {self._offset_type}') + self._engage_type = self._read_engage_type_param() + self._last_params_update = self._current_time def _read_policy_param(self) -> Policy: @@ -170,6 +170,22 @@ class SpeedLimitController: except (ValueError, TypeError): return Policy.car_state_priority + def _read_engage_type_param(self) -> Engage: + if self._pcm_cruise_op_long: + return Engage.auto + + return Engage(self._read_int_param("SpeedLimitEngageType", validator=lambda x: np.clip(x, Engage.auto, Engage.user_confirm))) + + def _read_int_param(self, key: str, default: int = 0, validator: Callable[[int], int] = None) -> int: + try: + val = int(self._params.get(key, encoding='utf8')) + + if validator is not None: + return validator(val) + return val + except (ValueError, TypeError): + return default + def _update_calculations(self) -> None: # Update current velocity offset (error) self._v_offset = self.speed_limit_offseted - self._v_ego From bbe6c684d88d77840b620bb5381737fc8c8e3ba1 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 01:50:15 -0400 Subject: [PATCH 6/9] more --- .../lib/speed_limit_controller/speed_limit_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index b259c8c351..f21d05f046 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -309,12 +309,12 @@ class SpeedLimitController: def _update_events(self, events_sp: EventsSP) -> None: if self._speed_limit > 0 and self._warning_type == 2 and \ - self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): + self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): events_sp.add(EventNameSP.speedLimitPreActive) if not self.is_active: if self._state == SpeedLimitControlState.preActive and self._state_prev != SpeedLimitControlState.preActive and \ - self._v_cruise_rounded != self._speed_limit_offsetted_rounded: + self._v_cruise_rounded != self._speed_limit_offsetted_rounded: events_sp.add(EventNameSP.speedLimitPreActive) else: if self._engage_type == Engage.user_confirm: From 74a2a01a0fb9510a388a0f7575d52704417806b9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 01:57:25 -0400 Subject: [PATCH 7/9] single method --- .../speed_limit_controller.py | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index f21d05f046..27aaf8f5cb 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -49,10 +49,10 @@ class SpeedLimitController: self._gas_pressed = False self._pcm_cruise_op_long = CP.openpilotLongitudinalControl and CP.pcmCruise - self._offset_type = self._read_int_param("SpeedLimitOffsetType") + self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType")) self._offset_value = self._read_int_param("SpeedLimitValueOffset") self._warning_type = self._read_int_param("SpeedLimitWarningType") - self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType") + self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType")) self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") self._engage_type = self._read_engage_type_param() self._brake_pressed = False @@ -115,23 +115,11 @@ class SpeedLimitController: @property def speed_limit_offset(self) -> float: - if self._offset_type == OffsetType.default: - return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) - elif self._offset_type == OffsetType.fixed: - return float(self._offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) - elif self._offset_type == OffsetType.percentage: - return float(self._offset_value * 0.01 * self._speed_limit) - return 0. + return self._get_offset(self._offset_type, self._offset_value) @property def speed_limit_warning_offset(self) -> float: - if self._warning_offset_type == OffsetType.default: - return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) - elif self._warning_offset_type == OffsetType.fixed: - return float(self._warning_offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) - elif self._warning_offset_type == OffsetType.percentage: - return float(self._warning_offset_value * 0.01 * self._speed_limit) - return 0. + return self._get_offset(self._warning_offset_type, self._warning_offset_value) @property def speed_limit(self) -> float: @@ -145,16 +133,26 @@ class SpeedLimitController: def source(self) -> Source: return Source(self._source) + def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float: + if offset_type == OffsetType.default: + return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) + elif offset_type == OffsetType.fixed: + return float(offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) + elif offset_type == OffsetType.percentage: + return float(offset_value * 0.01 * self._speed_limit) + else: + raise NotImplementedError("Offset not supported") + def _update_v_cruise_setpoint_prev(self) -> None: self._v_cruise_setpoint_prev = self._v_cruise_setpoint def _update_params(self) -> None: if self._current_time > self._last_params_update + PARAMS_UPDATE_PERIOD: self._is_enabled = self._params.get_bool("SpeedLimitControl") - self._offset_type = self._read_int_param("SpeedLimitOffsetType") + self._offset_type = OffsetType(self._read_int_param("SpeedLimitOffsetType")) self._offset_value = self._read_int_param("SpeedLimitValueOffset") self._warning_type = self._read_int_param("SpeedLimitWarningType") - self._warning_offset_type = self._read_int_param("SpeedLimitWarningOffsetType") + self._warning_offset_type = OffsetType(self._read_int_param("SpeedLimitWarningOffsetType")) self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") self._policy = self._read_policy_param() self._is_metric = self._params.get_bool("IsMetric") From 4b5cd3eef2188e46ab6c46fd76eb958866ce16d9 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 01:59:12 -0400 Subject: [PATCH 8/9] define types in init --- .../speed_limit_controller.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 27aaf8f5cb..1774b64b71 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -21,6 +21,10 @@ ENABLED_STATES = (SpeedLimitControlState.preActive, SpeedLimitControlState.tempI class SpeedLimitController: + _speed_limit: float + _distance: float + _source: Source + def __init__(self, CP): self._params = Params() self._CP = CP @@ -111,7 +115,7 @@ class SpeedLimitController: @property def speed_limit_offseted(self) -> float: - return float(self._speed_limit + self.speed_limit_offset) + return self._speed_limit + self.speed_limit_offset @property def speed_limit_offset(self) -> float: @@ -123,23 +127,23 @@ class SpeedLimitController: @property def speed_limit(self) -> float: - return float(self._speed_limit) + return self._speed_limit @property def distance(self) -> float: - return float(self._distance) + return self._distance @property def source(self) -> Source: - return Source(self._source) + return self._source def _get_offset(self, offset_type: OffsetType, offset_value: int) -> float: if offset_type == OffsetType.default: return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) elif offset_type == OffsetType.fixed: - return float(offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS)) + return offset_value * (CV.KPH_TO_MS if self._is_metric else CV.MPH_TO_MS) elif offset_type == OffsetType.percentage: - return float(offset_value * 0.01 * self._speed_limit) + return offset_value * 0.01 * self._speed_limit else: raise NotImplementedError("Offset not supported") From 36e910d5006f41732af1cb52e5f356f81e338b49 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Thu, 5 Jun 2025 02:00:34 -0400 Subject: [PATCH 9/9] rename --- .../speed_limit_controller.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 1774b64b71..0ec7445ebb 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -66,7 +66,7 @@ class SpeedLimitController: self._v_cruise_prev_rounded = 0. self._speed_limit_offsetted_rounded = 0. self._speed_limit_warning_offsetted_rounded = 0. - self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH + self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH # Mapping functions to state transitions self.state_transition_strategy = { @@ -160,7 +160,7 @@ class SpeedLimitController: self._warning_offset_value = self._read_int_param("SpeedLimitWarningValueOffset") self._policy = self._read_policy_param() self._is_metric = self._params.get_bool("IsMetric") - self._ms_to_local = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH + self._speed_factor = CV.MS_TO_KPH if self._is_metric else CV.MS_TO_MPH self._resolver.change_policy(self._policy) self._engage_type = self._read_engage_type_param() @@ -208,11 +208,11 @@ class SpeedLimitController: self._op_enabled_prev = self._op_enabled self._brake_pressed_prev = self._brake_pressed - self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._ms_to_local)) - self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._ms_to_local)) - self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._ms_to_local)) + self._v_cruise_rounded = int(round(self._v_cruise_setpoint * self._speed_factor)) + self._v_cruise_prev_rounded = int(round(self._v_cruise_setpoint_prev * self._speed_factor)) + self._speed_limit_offsetted_rounded = 0 if self._speed_limit == 0 else int(round((self._speed_limit + self.speed_limit_offset) * self._speed_factor)) self._speed_limit_warning_offsetted_rounded = 0 if self._speed_limit == 0 else \ - int(round((self._speed_limit + self.speed_limit_warning_offset) * self._ms_to_local)) + int(round((self._speed_limit + self.speed_limit_warning_offset) * self._speed_factor)) def transition_state_from_inactive(self) -> None: """ Make state transition from inactive state """ @@ -311,7 +311,7 @@ class SpeedLimitController: def _update_events(self, events_sp: EventsSP) -> None: if self._speed_limit > 0 and self._warning_type == 2 and \ - self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._ms_to_local)): + self._speed_limit_warning_offsetted_rounded < int(round(self._v_ego * self._speed_factor)): events_sp.add(EventNameSP.speedLimitPreActive) if not self.is_active: