Acceleration/deceleration profiles

Added toggle to use DragonPilot's acceleration/deceleration 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_functions.py

@@ -12,6 +12,34 @@ CRUISING_SPEED = 5  # Roughly the speed cars go when not touching the gas while
 PROBABILITY = 0.6   # 60% chance of condition being true
 THRESHOLD = 5       # Time threshold (0.25s)
 
+# Acceleration profiles - Credit goes to the DragonPilot team!
+                 # MPH = [0., 18,  36,  63,  94]
+A_CRUISE_MIN_BP_CUSTOM = [0., 8., 16., 28., 42.]
+                 # 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 = [-0.001, -0.010, -0.28, -0.56, -0.56]
+A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30, .091]
+
+A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60]
+A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2]
+
 class FrogPilotFunctions:
   def __init__(self) -> None:
     self.params = Params()
+
+  @staticmethod
+  def get_min_accel_eco(v_ego):
+    return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO)
+
+  @staticmethod
+  def get_max_accel_eco(v_ego):
+    return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
+
+  @staticmethod
+  def get_min_accel_sport(v_ego):
+    return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_SPORT)
+
+  @staticmethod
+  def get_max_accel_sport(v_ego):
+    return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT)

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -1,6 +1,8 @@
 import cereal.messaging as messaging
 
 from openpilot.common.conversions import Conversions as CV
+from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
 
 from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions
 
@@ -13,11 +15,35 @@ class FrogPilotPlanner:
 
     self.v_cruise = 0
 
+    self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)]
+
     self.update_frogpilot_params(params)
 
   def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego):
     enabled = controlsState.enabled
 
+    # Configure the deceleration profile
+    if self.deceleration_profile == 1:
+      min_accel = self.fpf.get_min_accel_eco(v_ego)
+    elif self.deceleration_profile == 2:
+      min_accel = self.fpf.get_min_accel_sport(v_ego)
+    elif mpc.mode == 'acc':
+      min_accel = A_CRUISE_MIN
+    else:
+      min_accel = ACCEL_MIN
+
+    # Configure the acceleration profile
+    if self.acceleration_profile == 1:
+      max_accel = self.fpf.get_max_accel_eco(v_ego)
+    elif self.acceleration_profile in (2, 3):
+      max_accel = self.fpf.get_max_accel_sport(v_ego)
+    elif mpc.mode == 'acc':
+      max_accel = get_max_accel(v_ego)
+    else:
+      max_accel = ACCEL_MAX
+
+    self.accel_limits = [min_accel, max_accel]
+
     # Update the max allowed speed
     self.v_cruise = self.update_v_cruise(carState, controlsState, enabled, modelData, v_cruise, v_ego)
 
@@ -47,3 +73,5 @@ class FrogPilotPlanner:
     custom_ui = params.get_bool("CustomUI")
 
     longitudinal_tune = params.get_bool("LongitudinalTune")
+    self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0
+    self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0

selfdrive/frogpilot/ui/control_settings.cc

@@ -5,6 +5,8 @@ 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.", ""},
+    {"DecelerationProfile", "Deceleration Profile", "Change the deceleration rate to be either sporty or eco-friendly.", ""},
 
     {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"},
   };
@@ -31,6 +33,21 @@ 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 if (param == "DecelerationProfile") {
+      std::vector<QString> profileOptions{tr("Standard"), tr("Eco"), tr("Sport")};
+      FrogPilotButtonParamControl *profileSelection = new FrogPilotButtonParamControl(param, title, desc, icon, profileOptions);
+      toggle = profileSelection;
 
     } else if (param == "QOLControls") {
       FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this);

selfdrive/frogpilot/ui/control_settings.h

@@ -33,7 +33,7 @@ private:
   std::set<QString> fireTheBabysitterKeys = {};
   std::set<QString> laneChangeKeys = {};
   std::set<QString> lateralTuneKeys = {};
-  std::set<QString> longitudinalTuneKeys = {};
+  std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile"};
   std::set<QString> mtscKeys = {};
   std::set<QString> qolKeys = {};
   std::set<QString> speedLimitControllerKeys = {};