Aggressive acceleration with a lead

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

common/params.cc

@@ -213,6 +213,7 @@ std::unordered_map<std::string, uint32_t> keys = {
 
     // FrogPilot parameters
     {"AccelerationProfile", PERSISTENT},
+    {"AggressiveAcceleration", PERSISTENT},
     {"ApiCache_DriveStats", PERSISTENT},
     {"FrogPilotTogglesUpdated", PERSISTENT},
     {"GasRegenCmd", PERSISTENT},

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -330,7 +330,7 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, personality=log.LongitudinalPersonality.standard):
     t_follow = get_T_FOLLOW(personality)
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
@@ -338,6 +338,12 @@ class LongitudinalMpc:
     lead_xv_0 = self.process_lead(radarstate.leadOne)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
+    # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
+    if aggressive_acceleration:
+      distance_factor = np.maximum(1, lead_xv_0[:,0] - (lead_xv_0[:,1] * t_follow))
+      t_follow_offset = np.clip((lead_xv_0[:,1] - v_ego) + (STOP_DISTANCE - v_ego), 1, distance_factor)
+      t_follow = t_follow / t_follow_offset
+
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.

selfdrive/controls/lib/longitudinal_planner.py

@@ -139,7 +139,7 @@ class LongitudinalPlanner:
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=self.personality)
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, personality=self.personality)
 
     self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)

selfdrive/frogpilot/functions/frogpilot_planner.py

@@ -80,3 +80,4 @@ class FrogPilotPlanner:
 
     longitudinal_tune = params.get_bool("LongitudinalTune")
     self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0
+    self.aggressive_acceleration = params.get_bool("AggressiveAcceleration") and longitudinal_tune

selfdrive/frogpilot/ui/control_settings.cc

@@ -7,6 +7,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
 
     {"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.", ""},
+    {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""},
   };
 
   for (const auto &[param, title, desc, icon] : controlToggles) {
@@ -80,7 +81,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
   fireTheBabysitterKeys = {};
   laneChangeKeys = {};
   lateralTuneKeys = {};
-  longitudinalTuneKeys = {"AccelerationProfile"};
+  longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration"};
   speedLimitControllerKeys = {};
   visionTurnControlKeys = {};