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

@@ -373,6 +373,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"ScreenTimeout", PERSISTENT},
     {"ScreenTimeoutOnroad", PERSISTENT},
     {"SearchInput", PERSISTENT},
+    {"SetSpeedOffset", PERSISTENT},
     {"ShowCPU", PERSISTENT},
     {"ShowGPU", PERSISTENT},
     {"ShowJerk", PERSISTENT},

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")

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)

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<QString> reverseCruiseToggles{"ReverseCruiseUI"};
       std::vector<QString> 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<int, QString>(), 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<FrogPilotParamValueControl*>(toggles["LaneDetectionWidth"]);
   FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseAOLOnBrake"]);
   FrogPilotParamValueControl *pauseLateralToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseLateralSpeed"]);
+  FrogPilotParamValueControl *setSpeedOffsetToggle = static_cast<FrogPilotParamValueControl*>(toggles["SetSpeedOffset"]);
   FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(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();
 }
 

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -45,7 +45,7 @@ private:
   std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite"};
   std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"};
   std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"};
-  std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise"};
+  std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};
   std::set<QString> speedLimitControllerKeys = {};
   std::set<QString> speedLimitControllerControlsKeys = {};
   std::set<QString> speedLimitControllerQOLKeys = {};