From 613ed4b64ce1bbddf8c2ed424bb8f8b528168d46 Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Mon, 25 Mar 2024 15:59:28 -0700
Subject: [PATCH] Aggressive acceleration when following a faster lead

Added toggle to make the acceleration more aggressive when following a faster lead.
---
 common/params.cc                                   |  1 +
 selfdrive/frogpilot/controls/frogpilot_planner.py  | 14 ++++++++++++++
 .../frogpilot/ui/qt/offroad/control_settings.cc    |  1 +
 .../frogpilot/ui/qt/offroad/control_settings.h     |  2 +-
 4 files changed, 17 insertions(+), 1 deletion(-)

diff --git a/common/params.cc b/common/params.cc
index b967a478e..4a8aa5578 100644
--- a/common/params.cc
+++ b/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},
diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py
index c1df870bf..0b2a41eab 100644
--- a/selfdrive/frogpilot/controls/frogpilot_planner.py
+++ b/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")
diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc
index 02ec158bb..15348e34f 100644
--- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc
+++ b/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"},
   };
diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h
index 8b7f1b9c8..97d11e3cb 100644
--- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h
+++ b/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 = {};