Acceleration profiles

Added toggle to use DragonPilot's acceleration profiles.

Credit goes to DragonPilot!

https: //github.com/dragonpilot-community/dragonpilot
Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -1,7 +1,33 @@
 import cereal.messaging as messaging
 
 from openpilot.common.conversions import Conversions as CV
+from openpilot.common.numpy_fast import interp
 from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, A_CRUISE_MAX_VALS, A_CRUISE_MAX_BP, get_max_accel
+
+# Acceleration profiles - Credit goes to the DragonPilot team!
+                 # MPH = [0.,  35,   35,  40,    40,  45,    45,  67,    67,   67, 123]
+A_CRUISE_MIN_BP_CUSTOM = [0., 2.0, 2.01, 11., 11.01, 18., 18.01, 28., 28.01,  33., 55.]
+                 # MPH = [0., 6.71, 13.4, 17.9, 24.6, 33.6, 44.7, 55.9, 67.1, 123]
+A_CRUISE_MAX_BP_CUSTOM = [0.,    3,   6.,   8.,  11.,  15.,  20.,  25.,  30., 55.]
+
+A_CRUISE_MIN_VALS_ECO_TUNE = [-0.480, -0.480, -0.40, -0.40, -0.40, -0.36, -0.32, -0.28, -0.28, -0.25, -0.25]
+A_CRUISE_MAX_VALS_ECO_TUNE = [3.5, 3.3, 1.7, 1.1, .76, .62, .47, .36, .28, .09]
+
+A_CRUISE_MIN_VALS_SPORT_TUNE = [-0.500, -0.500, -0.42, -0.42, -0.42, -0.42, -0.40, -0.35, -0.35, -0.30, -0.30]
+A_CRUISE_MAX_VALS_SPORT_TUNE = [3.5, 3.5, 3.0, 2.6, 1.4, 1.0, 0.7, 0.6, .38, .2]
+
+def get_min_accel_eco_tune(v_ego):
+  return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO_TUNE)
+
+def get_max_accel_eco_tune(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO_TUNE)
+
+def get_min_accel_sport_tune(v_ego):
+  return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT_TUNE)
+
+def get_max_accel_sport_tune(v_ego):
+  return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_TUNE)
 
 
 class FrogPilotPlanner:
@@ -19,6 +45,14 @@ class FrogPilotPlanner:
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
     v_ego = carState.vEgo
 
+    # Acceleration profiles
+    if self.acceleration_profile == 1:
+      self.accel_limits = [get_min_accel_eco_tune(v_ego), get_max_accel_eco_tune(v_ego)]
+    elif self.acceleration_profile in (2, 3):
+      self.accel_limits = [get_min_accel_sport_tune(v_ego), get_max_accel_sport_tune(v_ego)]
+    else:
+      self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+
     self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego)
 
   def update_v_cruise(self, carState, controlsState, modelData, enabled, v_cruise, v_ego):
@@ -45,3 +79,4 @@ class FrogPilotPlanner:
     lateral_tune = params.get_bool("LateralTune")
 
     longitudinal_tune = params.get_bool("LongitudinalTune")
+    self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0

selfdrive/frogpilot/ui/control_settings.cc

@@ -6,6 +6,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"},
 
     {"LongitudinalTune", "Longitudinal Tuning", "Modify openpilot's acceleration and braking behavior.", "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
+    {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -30,6 +31,17 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
         }
       });
       toggle = longitudinalTuneToggle;
+    } else if (param == "AccelerationProfile") {
+      std::vector<QString> profileOptions{tr("Standard"), tr("Eco"), tr("Sport"), tr("Sport+")};
+      FrogPilotButtonParamControl *profileSelection = new FrogPilotButtonParamControl(param, title, desc, icon, profileOptions);
+      toggle = profileSelection;
+
+      QObject::connect(static_cast<FrogPilotButtonParamControl*>(toggle), &FrogPilotButtonParamControl::buttonClicked, [this](int id) {
+        if (id == 3) {
+          FrogPilotConfirmationDialog::toggleAlert("WARNING: This maxes out openpilot's acceleration from 2.0 m/s to 4.0 m/s and may cause oscillations when accelerating!",
+          "I understand the risks.", this);
+        }
+      });
 
     } else {
       toggle = new ParamControl(param, title, desc, icon, this);
@@ -68,7 +80,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   fireTheBabysitterKeys = {};
   laneChangeKeys = {};
   lateralTuneKeys = {};
-  longitudinalTuneKeys = {};
+  longitudinalTuneKeys = {"AccelerationProfile"};
   speedLimitControllerKeys = {};
   visionTurnControlKeys = {};