From 34bbdf4d7fa9966a05ef2a783c0bf7d7d3dc242c Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Mon, 24 Mar 2025 19:26:48 -0700 Subject: [PATCH] Controls: Automatic lane change (#653) * init alc controller * only for sunny * rebase fix * ui * add ui preview * Update common/params_keys.h Co-authored-by: Jason Wen * Update selfdrive/ui/sunnypilot/SConscript Co-authored-by: Jason Wen * Update selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h Co-authored-by: Jason Wen * Update selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc Co-authored-by: Jason Wen * review sugg * code review * renaming * move around * style * fix types and params exceptions handling * take out magic numbers * more * rename * shorter * make sure reset happens at the end of every DH loop * split into multiple updates * just 3 seconds * use default states * oops * more readable * oops * some space and lines * run in DH loop directly * adjust ui preview * nudgeless should process immediately * check option instead * more explicit * even more explicit * tests * brake pedal release should not allow auto lane change (caught by test) * unnecessary * no continuous auto lane change * Revert "unnecessary" This reverts commit 93d135b54a812e94d23951ca8ea52ff944cfd29a. * more tests * less * less less * update again * more cleanup * better * AutoLaneChangeState -> AutoLaneChangeMode * update * lint * unused * test all states * license --------- Co-authored-by: Jason Wen --- common/params_keys.h | 2 + selfdrive/controls/lib/desire_helper.py | 9 +- selfdrive/ui/sunnypilot/SConscript | 1 + .../settings/lateral/lane_change_settings.cc | 111 ++++++++++ .../settings/lateral/lane_change_settings.h | 51 +++++ .../qt/offroad/settings/lateral_panel.cc | 24 +++ .../qt/offroad/settings/lateral_panel.h | 3 + selfdrive/ui/tests/test_ui/run.py | 7 + .../controls/lib/auto_lane_change.py | 115 ++++++++++ .../lib/tests/test_auto_lane_change.py | 198 ++++++++++++++++++ system/manager/manager.py | 2 + 11 files changed, 522 insertions(+), 1 deletion(-) create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc create mode 100644 selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h create mode 100644 sunnypilot/selfdrive/controls/lib/auto_lane_change.py create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py diff --git a/common/params_keys.h b/common/params_keys.h index 710a354a68..be038f5a2a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -120,6 +120,8 @@ inline static std::unordered_map keys = { // --- sunnypilot params --- // {"ApiCache_DriveStats", PERSISTENT}, + {"AutoLaneChangeBsmDelay", PERSISTENT}, + {"AutoLaneChangeTimer", PERSISTENT}, {"CarParamsSP", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"CarParamsSPCache", CLEAR_ON_MANAGER_START}, {"CarParamsSPPersistent", PERSISTENT}, diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 90b6858649..34ea4ff1bb 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,6 +1,7 @@ from cereal import log from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController LaneChangeState = log.LaneChangeState LaneChangeDirection = log.LaneChangeDirection @@ -39,8 +40,10 @@ class DesireHelper: self.keep_pulse_timer = 0.0 self.prev_one_blinker = False self.desire = log.Desire.none + self.alc = AutoLaneChangeController(self) def update(self, carstate, lateral_active, lane_change_prob): + self.alc.update_params() v_ego = carstate.vEgo one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN @@ -67,10 +70,12 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + self.alc.update_lane_change(blindspot_detected, carstate.brakePressed) + if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none - elif torque_applied and not blindspot_detected: + elif (torque_applied or self.alc.auto_lane_change_allowed) and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting # LaneChangeState.laneChangeStarting @@ -112,3 +117,5 @@ class DesireHelper: self.keep_pulse_timer = 0.0 elif self.desire in (log.Desire.keepLeft, log.Desire.keepRight): self.desire = log.Desire.none + + self.alc.update_state() diff --git a/selfdrive/ui/sunnypilot/SConscript b/selfdrive/ui/sunnypilot/SConscript index 24c98c9868..30748098a3 100644 --- a/selfdrive/ui/sunnypilot/SConscript +++ b/selfdrive/ui/sunnypilot/SConscript @@ -35,6 +35,7 @@ qt_src = [ ] lateral_panel_qt_src = [ + "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/lane_change_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc new file mode 100644 index 0000000000..53717508bf --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.cc @@ -0,0 +1,111 @@ +/** + * 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. + * + * Created by kumar on March 10, 2025 + */ + +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" +#include +#include +#include +#include + +LaneChangeSettings::LaneChangeSettings(QWidget* parent) : QWidget(parent) { + QVBoxLayout* main_layout = new QVBoxLayout(this); + main_layout->setContentsMargins(50, 20, 50, 20); + main_layout->setSpacing(20); + + // Back button + PanelBackButton* back = new PanelBackButton(tr("Back")); + connect(back, &QPushButton::clicked, [=]() { emit backPress(); }); + main_layout->addWidget(back, 0, Qt::AlignLeft); + + ListWidgetSP *list = new ListWidgetSP(this, false); + // param, title, desc, icon + std::vector> toggle_defs{ + { + "AutoLaneChangeBsmDelay", + tr("Auto Lane Change: Delay with Blind Spot"), + tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring (BSM) detects a obstructing vehicle, ensuring safe maneuvering."), + "../assets/offroad/icon_blank.png", + }, + }; + + // Controls: Auto Lane Change Timer + autoLaneChangeTimer = new AutoLaneChangeTimer(); + autoLaneChangeTimer->setUpdateOtherToggles(true); + autoLaneChangeTimer->showDescription(); + connect(autoLaneChangeTimer, &OptionControlSP::updateLabels, autoLaneChangeTimer, &AutoLaneChangeTimer::refresh); + connect(autoLaneChangeTimer, &AutoLaneChangeTimer::updateOtherToggles, this, &LaneChangeSettings::updateToggles); + list->addItem(autoLaneChangeTimer); + + for (auto &[param, title, desc, icon] : toggle_defs) { + auto toggle = new ParamControlSP(param, title, desc, icon, this); + list->addItem(toggle); + toggles[param.toStdString()] = toggle; + } + + main_layout->addWidget(new ScrollViewSP(list, this)); +} + +void LaneChangeSettings::showEvent(QShowEvent *event) { + updateToggles(); +} + +void LaneChangeSettings::updateToggles() { + if (!isVisible()) { + return; + } + + auto auto_lane_change_bsm_delay_toggle = toggles["AutoLaneChangeBsmDelay"]; + auto autoLaneChangeTimer_param = std::atoi(params.get("AutoLaneChangeTimer").c_str()); + + 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(); + + if (!CP.getEnableBsm()) { + params.remove("AutoLaneChangeBsmDelay"); + } + auto_lane_change_bsm_delay_toggle->setEnabled(CP.getEnableBsm() && (autoLaneChangeTimer_param > 0)); + auto_lane_change_bsm_delay_toggle->refresh(); + } else { + auto_lane_change_bsm_delay_toggle->setEnabled(false); + } +} + +// Auto Lane Change Timer (ALCT) +AutoLaneChangeTimer::AutoLaneChangeTimer() : OptionControlSP( + "AutoLaneChangeTimer", + tr("Auto Lane Change by Blinker"), + tr("Set a timer to delay the auto lane change operation when the blinker is used. " + "No nudge on the steering wheel is required to auto lane change if a timer is set. Default is Nudge.\n" + "Please use caution when using this feature. Only use the blinker when traffic and road conditions permit."), + "../assets/offroad/icon_blank.png", + {-1, 5}) { + + refresh(); +} + +void AutoLaneChangeTimer::refresh() { + QString option = QString::fromStdString(params.get("AutoLaneChangeTimer")); + const QString second = tr("s"); + + static const QMap options = { + {"-1", tr("Off")}, + {"0", tr("Nudge")}, + {"1", tr("Nudgeless")}, + {"2", "0.5 " + second}, + {"3", "1 " + second}, + {"4", "2 " + second}, + {"5", "3 " + second}, + }; + + setLabel(options.value(option, tr("Nudge"))); +} diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h new file mode 100644 index 0000000000..5bd458c47d --- /dev/null +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral/lane_change_settings.h @@ -0,0 +1,51 @@ +/** + * 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 +#include + +#include "selfdrive/ui/sunnypilot/ui.h" +#include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" +#include "selfdrive/ui/sunnypilot/qt/widgets/controls.h" + +class AutoLaneChangeTimer : public OptionControlSP { + Q_OBJECT + +public: + AutoLaneChangeTimer(); + + void refresh(); + +signals: + void toggleUpdated(); + +private: + Params params; +}; + + +class LaneChangeSettings : public QWidget { + Q_OBJECT + +public: + explicit LaneChangeSettings(QWidget* parent = nullptr); + void showEvent(QShowEvent *event) override; + +signals: + void backPress(); + +public slots: + void updateToggles(); + +private: + Params params; + std::map toggles; + + AutoLaneChangeTimer *autoLaneChangeTimer; +}; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc index aaf3201f2a..3fde7fd5fc 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.cc @@ -42,6 +42,29 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { }); list->addItem(madsSettingsButton); + list->addItem(vertical_space()); + list->addItem(horizontal_line()); + list->addItem(vertical_space()); + + // Lane Change Settings + laneChangeSettingsButton = new PushButtonSP(tr("Customize Lane Change")); + laneChangeSettingsButton->setObjectName("lane_change_btn"); + connect(laneChangeSettingsButton, &QPushButton::clicked, [=]() { + sunnypilotScroller->setLastScrollPosition(); + main_layout->setCurrentWidget(laneChangeWidget); + }); + + laneChangeWidget = new LaneChangeSettings(this); + connect(laneChangeWidget, &LaneChangeSettings::backPress, [=]() { + sunnypilotScroller->restoreScrollPosition(); + main_layout->setCurrentWidget(sunnypilotScreen); + }); + list->addItem(laneChangeSettingsButton); + + list->addItem(vertical_space(0)); + list->addItem(horizontal_line()); + + // Neural Network Lateral Control nnlcToggle = new NeuralNetworkLateralControl(); list->addItem(nnlcToggle); @@ -65,6 +88,7 @@ LateralPanel::LateralPanel(SettingsWindowSP *parent) : QFrame(parent) { main_layout->addWidget(sunnypilotScreen); main_layout->addWidget(madsWidget); + main_layout->addWidget(laneChangeWidget); setStyleSheet(R"( #back_btn { diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h index 0857e6ebd5..1b77939ee8 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/lateral_panel.h @@ -13,6 +13,7 @@ #include "selfdrive/ui/sunnypilot/ui.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" #include "selfdrive/ui/qt/util.h" #include "selfdrive/ui/sunnypilot/qt/offroad/settings/settings.h" #include "selfdrive/ui/sunnypilot/qt/widgets/scrollview.h" @@ -38,5 +39,7 @@ private: ParamControl *madsToggle; PushButtonSP *madsSettingsButton; MadsSettings *madsWidget = nullptr; + PushButtonSP *laneChangeSettingsButton; + LaneChangeSettings *laneChangeWidget = nullptr; NeuralNetworkLateralControl *nnlcToggle = nullptr; }; diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index c28b578d95..6d7e6352ba 100755 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -233,6 +233,12 @@ def setup_settings_steering_mads(click, pm: PubMaster, scroll=None): click(970, 250) time.sleep(UI_DELAY) +def setup_settings_steering_alc(click, pm: PubMaster, scroll=None): + setup_settings_device(click, pm) + click(278, 852) + click(970, 534) + time.sleep(UI_DELAY) + def setup_settings_trips(click, pm: PubMaster, scroll=None): setup_settings_device(click, pm) click(278, 962) @@ -284,6 +290,7 @@ CASES.update({ "settings_sunnylink_sponsor_button": setup_settings_sunnylink_sponsor_button, "settings_steering": setup_settings_steering, "settings_steering_mads": setup_settings_steering_mads, + "settings_steering_alc": setup_settings_steering_alc, "settings_trips": setup_settings_trips, "settings_vehicle": setup_settings_vehicle, }) diff --git a/sunnypilot/selfdrive/controls/lib/auto_lane_change.py b/sunnypilot/selfdrive/controls/lib/auto_lane_change.py new file mode 100644 index 0000000000..29056d5ac5 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/auto_lane_change.py @@ -0,0 +1,115 @@ +""" +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 log + +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL + + +class AutoLaneChangeMode: + OFF = -1 + NUDGE = 0 # default + NUDGELESS = 1 + HALF_SECOND = 2 + ONE_SECOND = 3 + TWO_SECONDS = 4 + THREE_SECONDS = 5 + + +AUTO_LANE_CHANGE_TIMER = { + AutoLaneChangeMode.OFF: 0.0, # Off + AutoLaneChangeMode.NUDGE: 0.0, # Nudge + AutoLaneChangeMode.NUDGELESS: 0.05, # Nudgeless + AutoLaneChangeMode.HALF_SECOND: 0.5, # 0.5-second delay + AutoLaneChangeMode.ONE_SECOND: 1.0, # 1-second delay + AutoLaneChangeMode.TWO_SECONDS: 2.0, # 2-second delay + AutoLaneChangeMode.THREE_SECONDS: 3.0, # 3-second delay +} + +ONE_SECOND_DELAY = -1 + + +class AutoLaneChangeController: + def __init__(self, desire_helper): + self.DH = desire_helper + self.params = Params() + + self.lane_change_wait_timer = 0.0 + self.param_read_counter = 0 + self.lane_change_delay = 0.0 + + self.lane_change_set_timer = AutoLaneChangeMode.NUDGE + self.lane_change_bsm_delay = False + + self.prev_brake_pressed = False + self.auto_lane_change_allowed = False + self.prev_lane_change = False + + self.read_params() + + def reset(self) -> None: + # Auto reset if parent state indicates we should + if self.DH.lane_change_state == log.LaneChangeState.off and \ + self.DH.lane_change_direction == log.LaneChangeDirection.none: + self.lane_change_wait_timer = 0.0 + self.prev_brake_pressed = False + self.prev_lane_change = False + + def read_params(self) -> None: + self.lane_change_bsm_delay = self.params.get_bool("AutoLaneChangeBsmDelay") + try: + self.lane_change_set_timer = int(self.params.get("AutoLaneChangeTimer", encoding="utf8")) + except (ValueError, TypeError): + self.lane_change_set_timer = AutoLaneChangeMode.NUDGE + + def update_params(self) -> None: + if self.param_read_counter % 50 == 0: + self.read_params() + self.param_read_counter += 1 + + def update_lane_change_timers(self, blindspot_detected: bool) -> None: + self.lane_change_delay = AUTO_LANE_CHANGE_TIMER.get(self.lane_change_set_timer, + AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGE]) + + self.lane_change_wait_timer += DT_MDL + + if self.lane_change_bsm_delay and blindspot_detected and self.lane_change_delay > 0: + if self.lane_change_delay == AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]: + self.lane_change_wait_timer = ONE_SECOND_DELAY + else: + self.lane_change_wait_timer = self.lane_change_delay + ONE_SECOND_DELAY + + def update_allowed(self) -> bool: + # Auto lane change allowed if: + # 1. A valid delay is set (non-zero) + # 2. Brake wasn't previously pressed + # 3. We've waited long enough + + if self.lane_change_set_timer in (AutoLaneChangeMode.OFF, AutoLaneChangeMode.NUDGE): + return False + + if self.prev_brake_pressed: + return False + + if self.prev_lane_change: + return False + + return bool(self.lane_change_wait_timer > self.lane_change_delay) + + def update_lane_change(self, blindspot_detected: bool, brake_pressed: bool) -> None: + if brake_pressed and not self.prev_brake_pressed: + self.prev_brake_pressed = brake_pressed + + self.update_lane_change_timers(blindspot_detected) + + self.auto_lane_change_allowed = self.update_allowed() + + def update_state(self): + if self.DH.lane_change_state == log.LaneChangeState.laneChangeStarting: + self.prev_lane_change = True + + self.reset() diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py b/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py new file mode 100644 index 0000000000..c1b3053207 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_auto_lane_change.py @@ -0,0 +1,198 @@ +""" +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 parameterized import parameterized + +from openpilot.common.realtime import DT_MDL +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection +from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeController, AutoLaneChangeMode, \ + AUTO_LANE_CHANGE_TIMER, ONE_SECOND_DELAY + +AUTO_LANE_CHANGE_TIMER_COMBOS = [ + (AutoLaneChangeMode.NUDGELESS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.NUDGELESS]), + (AutoLaneChangeMode.HALF_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.HALF_SECOND]), + (AutoLaneChangeMode.ONE_SECOND, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.ONE_SECOND]), + (AutoLaneChangeMode.TWO_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.TWO_SECONDS]), + (AutoLaneChangeMode.THREE_SECONDS, AUTO_LANE_CHANGE_TIMER[AutoLaneChangeMode.THREE_SECONDS]) +] + + +class TestAutoLaneChangeController: + def setup_method(self): + self.DH = DesireHelper() + self.alc = AutoLaneChangeController(self.DH) + + def _reset_states(self): + self.alc.lane_change_bsm_delay = False + self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE + self.lane_change_wait_timer = 0.0 + self.prev_brake_pressed = False + self.prev_lane_change = False + + def test_reset(self): + """Test that reset correctly sets timers back to default.""" + # Set some non-default values + self.alc.lane_change_wait_timer = 2.0 + self.alc.prev_brake_pressed = True + + # Set the DesireHelper to trigger a reset + self.DH.lane_change_state = LaneChangeState.off + self.DH.lane_change_direction = LaneChangeDirection.none + + # Call reset + self.alc.reset() + + # Check values were reset + assert self.alc.lane_change_wait_timer == 0.0 + assert not self.alc.prev_brake_pressed + + @parameterized.expand([(AutoLaneChangeMode.OFF, ), (AutoLaneChangeMode.NUDGE, )]) + def test_off_and_nudge_mode(self, timer_state): + """Test the default OFF and NUDGE mode behavior.""" + self._reset_states() + # Setup mode + self.alc.lane_change_bsm_delay = False # BSM delay off + self.alc.lane_change_set_timer = timer_state + + # Update controller + num_updates = int(5.0 / DT_MDL) + for _ in range(num_updates): # Run for 5 seconds + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # Mode should not allow lane change immediately + assert not self.alc.auto_lane_change_allowed + + def test_nudgeless_mode(self): + """Test the NUDGELESS mode behavior.""" + self._reset_states() + # Setup NUDGELESS mode + self.alc.lane_change_bsm_delay = False # BSM delay off + self.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGELESS + + # Update controller once to read params + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # Update multiple times to exceed the timer threshold + for _ in range(1): # Should exceed 0.1s with multiple DT_MDL updates + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # Now lane change should be allowed + assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay + assert self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_timers(self, timer_state, timer_delay): + self._reset_states() + self.alc.lane_change_bsm_delay = False # BSM delay off + self.alc.lane_change_set_timer = timer_state + + # Update controller once + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # The timer should still be below the threshold after one update + assert not self.alc.auto_lane_change_allowed + + # Update enough times to exceed the threshold (seconds / DT_MDL) + num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # Now lane change should be allowed + assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay + assert self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_brake_pressed_disables_auto_lane_change(self, timer_state, timer_delay): + """Test that pressing the brake disables auto lane change.""" + self._reset_states() + # Setup auto lane change mode + self.alc.lane_change_bsm_delay = False + self.alc.lane_change_set_timer = timer_state + num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold + + # Update with brake pressed for 1 second + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=True) + + # Even though it is an auto lane change mode, lane change should be disallowed due to brake pressed prior initiating lane change + assert not self.alc.auto_lane_change_allowed + + # Check that prev_brake_pressed is saved + assert self.alc.prev_brake_pressed + + # Even releasing brake shouldn't allow auto lane change + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + assert not self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_blindspot_detected_with_bsm_delay(self, timer_state, timer_delay): + """Test behavior when blindspot is detected with BSM delay enabled.""" + # Blindspot detected - should prevent auto lane change + self._reset_states() + self.alc.lane_change_bsm_delay = True # BSM delay on + self.alc.lane_change_set_timer = timer_state + + # Update with blindspot detected - this should prevent auto lane change + self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + # Keep updating with blindspot detected - should still prevent auto lane change + num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_blindspot_detected_then_undetected_with_bsm_delay(self, timer_state, timer_delay): + """Test behavior when blindspot is detected then undetected with BSM delay enabled.""" + # Blindspot clears - should allow auto lane change after sufficient time + self._reset_states() + self.alc.lane_change_bsm_delay = True + self.alc.lane_change_set_timer = timer_state + + # First update with blindspot detected to set the negative timer + self.alc.update_lane_change(blindspot_detected=True, brake_pressed=False) + assert not self.alc.auto_lane_change_allowed + + # Now update with blindspot cleared - should start incrementing timer from negative value + num_updates = int((timer_delay + abs(ONE_SECOND_DELAY)) / DT_MDL) + 1 + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # After sufficient updates with no blindspot, auto lane change should be allowed + assert self.alc.auto_lane_change_allowed + + @parameterized.expand(AUTO_LANE_CHANGE_TIMER_COMBOS) + def test_disallow_continuous_auto_lane_change(self, timer_state, timer_delay): + self._reset_states() + self.alc.lane_change_bsm_delay = False # BSM delay off + self.alc.lane_change_set_timer = timer_state + num_updates = int(timer_delay / DT_MDL) + 1 # Add one extra updates to ensure we exceed the threshold + + # Update enough times to exceed the threshold (seconds / DT_MDL) + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + # Now lane change should be allowed + assert self.alc.lane_change_wait_timer > self.alc.lane_change_delay + assert self.alc.auto_lane_change_allowed + + # Simulate lane change is initiated + self.DH.lane_change_state = LaneChangeState.laneChangeStarting + self.alc.update_state() + + # Simulate lane change is completed, and one_blinker stays on + self.DH.lane_change_state = LaneChangeState.preLaneChange + self.alc.update_state() + + # Update enough times to exceed the threshold (seconds / DT_MDL) + for _ in range(num_updates): + self.alc.update_lane_change(blindspot_detected=False, brake_pressed=False) + + assert not self.alc.auto_lane_change_allowed diff --git a/system/manager/manager.py b/system/manager/manager.py index bcc2ff5c28..eb6c60c9ee 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -43,6 +43,8 @@ def manager_init() -> None: ] sunnypilot_default_params: list[tuple[str, str | bytes]] = [ + ("AutoLaneChangeTimer", "0"), + ("AutoLaneChangeBsmDelay", "0"), ("DynamicExperimentalControl", "0"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"),