From 9172945005cea2037f4b9b9f1eba09ba212d5449 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Wed, 27 Mar 2024 05:20:20 -0700 Subject: [PATCH] Set speed offset Added toggle to apply an offset to the set speed. Co-Authored-By: Tim Wilson <7284371+twilsonco@users.noreply.github.com> --- common/params.cc | 1 + selfdrive/controls/controlsd.py | 1 + selfdrive/controls/lib/drive_helpers.py | 5 +++++ selfdrive/frogpilot/ui/qt/offroad/control_settings.cc | 9 +++++++++ selfdrive/frogpilot/ui/qt/offroad/control_settings.h | 2 +- 5 files changed, 17 insertions(+), 1 deletion(-) diff --git a/common/params.cc b/common/params.cc index c3b9a7887..0d4e3f7be 100644 --- a/common/params.cc +++ b/common/params.cc @@ -373,6 +373,7 @@ std::unordered_map keys = { {"ScreenTimeout", PERSISTENT}, {"ScreenTimeoutOnroad", PERSISTENT}, {"SearchInput", PERSISTENT}, + {"SetSpeedOffset", PERSISTENT}, {"ShowCPU", PERSISTENT}, {"ShowGPU", PERSISTENT}, {"ShowJerk", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 880ecc73e..9c7307882 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1032,6 +1032,7 @@ class Controls: self.pause_lateral_below_speed = self.params.get_int("PauseLateralSpeed") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) if quality_of_life else 0 self.pause_lateral_below_signal = quality_of_life and self.params.get_bool("PauseLateralOnSignal") self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") + self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0 toyota_doors = self.params.get_bool("ToyotaDoors") self.frogpilot_variables.lock_doors = toyota_doors and self.params.get_bool("LockDoors") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 770ba32dd..da7f0d2be 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -112,6 +112,11 @@ class VCruiseHelper: else: self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type] + v_cruise_offset = (frogpilot_variables.set_speed_offset * CRUISE_INTERVAL_SIGN[button_type]) if long_press else 0 + if v_cruise_offset < 0: + v_cruise_offset = frogpilot_variables.set_speed_offset - v_cruise_delta + self.v_cruise_kph += v_cruise_offset + # If set is pressed while overriding, clip cruise speed to minimum of vEgo if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise): self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH) diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 142f80d2c..701b0e701 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -106,6 +106,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"OnroadDistanceButton", tr("Onroad Distance Button"), tr("Simulate a distance button via the onroad UI to control personalities, 'Experimental Mode', and 'Traffic Mode'."), ""}, {"PauseLateralSpeed", tr("Pause Lateral Below"), tr("Pause lateral control on all speeds below the set speed."), ""}, {"ReverseCruise", tr("Reverse Cruise Increase"), tr("Reverses the 'long press' functionality logic to increase the max set speed by 5 instead of 1. Useful to increase the max speed quickly."), ""}, + {"SetSpeedOffset", tr("Set Speed Offset"), tr("Set an offset for your desired set speed."), ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -514,6 +515,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } else { modifiedQolKeys.erase("CustomCruise"); modifiedQolKeys.erase("CustomCruiseLong"); + modifiedQolKeys.erase("SetSpeedOffset"); } toggle->setVisible(modifiedQolKeys.find(key.c_str()) != modifiedQolKeys.end()); @@ -538,6 +540,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector reverseCruiseToggles{"ReverseCruiseUI"}; std::vector reverseCruiseNames{tr("Control Via UI")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, reverseCruiseToggles, reverseCruiseNames); + } else if (param == "SetSpeedOffset") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map(), this, false, tr(" mph")); } else if (param == "NudgelessLaneChange") { FrogPilotParamManageControl *laneChangeToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); @@ -705,6 +709,7 @@ void FrogPilotControlsPanel::updateMetric() { params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion)); params.putIntNonBlocking("PauseLateralOnSignal", std::nearbyint(params.getInt("PauseLateralOnSignal") * speedConversion)); params.putIntNonBlocking("PauseLateralSpeed", std::nearbyint(params.getInt("PauseLateralSpeed") * speedConversion)); + params.putIntNonBlocking("SetSpeedOffset", std::nearbyint(params.getInt("SetSpeedOffset") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } @@ -713,6 +718,7 @@ void FrogPilotControlsPanel::updateMetric() { FrogPilotParamValueControl *laneWidthToggle = static_cast(toggles["LaneDetectionWidth"]); FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast(toggles["PauseAOLOnBrake"]); FrogPilotParamValueControl *pauseLateralToggle = static_cast(toggles["PauseLateralSpeed"]); + FrogPilotParamValueControl *setSpeedOffsetToggle = static_cast(toggles["SetSpeedOffset"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { @@ -721,6 +727,7 @@ void FrogPilotControlsPanel::updateMetric() { laneWidthToggle->updateControl(0, 30, tr(" meters"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph")); pauseLateralToggle->updateControl(0, 99, tr(" kph")); + setSpeedOffsetToggle->updateControl(0, 150, tr(" kph")); stoppingDistanceToggle->updateControl(0, 5, tr(" meters")); } else { customCruiseToggle->updateControl(1, 99, tr(" mph")); @@ -728,6 +735,7 @@ void FrogPilotControlsPanel::updateMetric() { laneWidthToggle->updateControl(0, 100, tr(" feet"), 10); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph")); pauseLateralToggle->updateControl(0, 99, tr(" mph")); + setSpeedOffsetToggle->updateControl(0, 99, tr(" mph")); stoppingDistanceToggle->updateControl(0, 10, tr(" feet")); } @@ -736,6 +744,7 @@ void FrogPilotControlsPanel::updateMetric() { laneWidthToggle->refresh(); pauseAOLOnBrakeToggle->refresh(); pauseLateralToggle->refresh(); + setSpeedOffsetToggle->refresh(); stoppingDistanceToggle->refresh(); } diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 8b63c76f2..2ead4d567 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -45,7 +45,7 @@ private: std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; - std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise"}; + std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {}; std::set speedLimitControllerControlsKeys = {}; std::set speedLimitControllerQOLKeys = {};