diff --git a/common/params_keys.h b/common/params_keys.h index c8cc8790b..1c99154c0 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -425,6 +425,7 @@ inline static std::unordered_map keys = { {"PauseAOLOnBrake", {PERSISTENT, BOOL, "0", "0", 1}}, {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0", "0", 1}}, {"PauseLateralSpeed", {PERSISTENT, FLOAT, "0.0", "0.0", 1}}, + {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0", "0.0", 1}}, {"PedalsOnUI", {PERSISTENT, BOOL, "0", "0", 1}}, {"PondPaired", {PERSISTENT, BOOL, "0", "0", 0}}, {"PondUploadPending", {PERSISTENT, BOOL, "0", "0", 0}}, diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 03ff9f975..3b7add4fd 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1089,6 +1089,7 @@ class StarPilotVariables: quality_of_life_lateral = self.get_value("QOLLateral") toggle.pause_lateral_below_speed = self.get_value("PauseLateralSpeed", cast=float, condition=quality_of_life_lateral, conversion=speed_conversion) toggle.pause_lateral_below_signal = self.get_value("PauseLateralOnSignal", condition=toggle.pause_lateral_below_speed != 0) + toggle.pause_lateral_signal_delay = self.get_value("LateralResumeDelay", cast=float, condition=toggle.pause_lateral_below_signal, default=0.0, min=0.0, max=5.0) quality_of_life_longitudinal = toggle.openpilot_longitudinal and self.get_value("QOLLongitudinal") toggle.cruise_increase = self.get_value("CustomCruise", cast=float, condition=(quality_of_life_longitudinal and not pcm_cruise)) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 5aa84a029..f81f3ce5d 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -61,6 +61,13 @@ class StarPilotPlanner: self.tracking_lead = False self._prev_gps_bearing = 0 + # Blinker-based lateral resume delay state + self.blinker_min_speed = float('inf') + self.blinker_delay_active = False + self.blinker_delay_frame = 0 + self.CS_prev_left_blinker = False + self.CS_prev_right_blinker = False + self.lane_width_left = 0 self.lane_width_right = 0 self._lane_width_counter = 0 @@ -128,6 +135,37 @@ class StarPilotPlanner: self.lateral_check |= sm["carState"].standstill self.lateral_check &= not sm["starpilotCarState"].pauseLateral + # Blinker-based lateral resume delay: after blinker turns off, delay lateral + # resumption if the vehicle went below half the pause speed during the blinker. + # This lets the driver manually straighten the wheel after a turn without + # openpilot fighting them. + CS = sm["carState"] + blinker_on = CS.leftBlinker or CS.rightBlinker + prev_blinker_on = self.CS_prev_left_blinker or self.CS_prev_right_blinker + + if blinker_on: + # Track minimum speed while blinker is active + if not prev_blinker_on: + self.blinker_min_speed = CS.vEgo + else: + self.blinker_min_speed = min(self.blinker_min_speed, CS.vEgo) + self.blinker_delay_active = False + elif prev_blinker_on and starpilot_toggles.pause_lateral_below_signal and starpilot_toggles.pause_lateral_signal_delay > 0: + # Blinker just turned off — start the delay timer + self.blinker_delay_active = True + self.blinker_delay_frame = sm.frame + + if self.blinker_delay_active: + time_since_blinker = (sm.frame - self.blinker_delay_frame) * DT_MDL + if time_since_blinker < starpilot_toggles.pause_lateral_signal_delay and \ + self.blinker_min_speed < starpilot_toggles.pause_lateral_below_speed / 2: + self.lateral_check = False + else: + self.blinker_delay_active = False + + self.CS_prev_left_blinker = CS.leftBlinker + self.CS_prev_right_blinker = CS.rightBlinker + self.model_length = sm["modelV2"].position.x[-1] self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME diff --git a/starpilot/ui/qt/offroad/lateral_settings.cc b/starpilot/ui/qt/offroad/lateral_settings.cc index 40e673143..10dbd392a 100644 --- a/starpilot/ui/qt/offroad/lateral_settings.cc +++ b/starpilot/ui/qt/offroad/lateral_settings.cc @@ -59,7 +59,8 @@ StarPilotLateralPanel::StarPilotLateralPanel(StarPilotSettingsWindow *parent, bo {"NNFFLite", tr("Neural Network Feedforward (NNFF) Lite"), tr("A lightweight version of Twilsonco's \"Neural Network FeedForward\" controller. Uses the \"look-ahead\" planned lateral jerk logic from the full model to help smoothen steering adjustments in curves, but does not use the full neural network for torque calculation."), ""}, {"QOLLateral", tr("Quality of Life"), tr("Steering control changes to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_quality_of_life.png"}, - {"PauseLateralSpeed", tr("Pause Steering Below"), tr("Pause steering below the set speed."), ""} + {"PauseLateralSpeed", tr("Pause Steering Below"), tr("Pause steering below the set speed."), ""}, + {"LateralResumeDelay", tr("Lateral Resume Delay"), tr("Delay before lateral control resumes after the turn signal is turned off. Only applies when the vehicle speed dropped below half the \"Pause Steering Below\" speed during the turn signal. Set to 0 to disable."), ""} }; for (const auto &[param, title, desc, icon] : lateralToggles) { @@ -136,6 +137,14 @@ StarPilotLateralPanel::StarPilotLateralPanel(StarPilotSettingsWindow *parent, bo std::vector pauseLateralToggleNames{tr("Turn Signal Only")}; lateralToggle = new StarPilotParamValueButtonControl(param, title, desc, icon, 0, 99, QString(), std::map(), 1, true, pauseLateralToggles, pauseLateralToggleNames, true); + } else if (param == "LateralResumeDelay") { + std::map delayLabels; + for (int i = 0; i <= 50; ++i) { + float key = i / 10.0f; + delayLabels[key] = key == 0.0f ? tr("Off") : QString::number(key, 'f', 1) + tr(" s"); + } + lateralToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 5, QString(), delayLabels, 0.1); + } else { lateralToggle = new ParamControl(param, title, desc, icon); } @@ -375,6 +384,10 @@ void StarPilotLateralPanel::updateToggles() { setVisible &= params.getBool("LaneChanges") && params.getBool("NudgelessLaneChange"); } + else if (key == "LateralResumeDelay") { + setVisible &= params.getBool("PauseLateralOnSignal"); + } + else if (key == "NNFF") { setVisible &= parent->hasNNFFLog; setVisible &= !parent->isAngleCar; diff --git a/starpilot/ui/qt/offroad/lateral_settings.h b/starpilot/ui/qt/offroad/lateral_settings.h index 9e4238672..a8eac5635 100644 --- a/starpilot/ui/qt/offroad/lateral_settings.h +++ b/starpilot/ui/qt/offroad/lateral_settings.h @@ -28,7 +28,7 @@ private: QSet aolKeys = {"AlwaysOnLateralLKAS", "PauseAOLOnBrake"}; QSet laneChangeKeys = {"LaneChangeSmoothing", "LaneChangeTime", "LaneDetectionWidth", "MinimumLaneChangeSpeed", "NudgelessLaneChange", "OneLaneChange"}; QSet lateralTuneKeys = {"NNFF", "NNFFLite", "TurnDesires"}; - QSet qolKeys = {"PauseLateralSpeed"}; + QSet qolKeys = {"PauseLateralSpeed", "LateralResumeDelay"}; QSet parentKeys;