Aggressive acceleration when following a faster lead

Added toggle to make the acceleration more aggressive when following a faster lead.

common/params.cc

@@ -211,6 +211,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     // FrogPilot parameters
     {"AccelerationPath", PERSISTENT},
     {"AccelerationProfile", PERSISTENT},
+    {"AggressiveAcceleration", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"CustomAlerts", PERSISTENT},
     {"CustomPaths", PERSISTENT},

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -89,6 +89,19 @@ class FrogPilotPlanner:
   def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead):
     lead_distance = radarState.leadOne.dRel
 
+    # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
+    if self.aggressive_acceleration and not self.release:
+      distance_factor = np.maximum(1, lead_distance - (v_ego * t_follow))
+      standstill_offset = max(stopping_distance - v_ego, 0)
+      acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor)
+      jerk /= acceleration_offset
+      t_follow /= acceleration_offset
+    elif self.aggressive_acceleration:
+      distance_factor = np.maximum(1, lead_distance - (v_lead * t_follow))
+      standstill_offset = max(STOP_DISTANCE - (v_ego**COMFORT_BRAKE), 0)
+      acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor)
+      t_follow /= acceleration_offset
+
     return jerk, t_follow
 
   def update_v_cruise(self, carState, controlsState, enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego):
@@ -128,3 +141,4 @@ class FrogPilotPlanner:
     longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune")
     self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0
     self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0
+    self.aggressive_acceleration = longitudinal_tune and self.params.get_bool("AggressiveAcceleration")

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

@@ -10,6 +10,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
     {"LongitudinalTune", tr("Longitudinal Tuning"), tr("Modify openpilot's acceleration and braking behavior."), "../frogpilot/assets/toggle_icons/icon_longitudinal_tune.png"},
     {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""},
     {"DecelerationProfile", tr("Deceleration Profile"), tr("Change the deceleration rate to be either sporty or eco-friendly."), ""},
+    {"AggressiveAcceleration", tr("Increase Acceleration Behind Faster Lead"), tr("Increase aggressiveness when following a faster lead."), ""},
 
     {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"},
   };

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

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