From f37f6ecd4e07cc42a03b58d3804276d72d6f03d7 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Fri, 24 May 2024 02:50:28 -0700 Subject: [PATCH] Controls - Longitudinal Tuning - Increase Acceleration Behind Lead Increase aggressiveness when following a faster lead. --- selfdrive/frogpilot/controls/frogpilot_planner.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 4f15371..a998fa0 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -117,6 +117,20 @@ class FrogPilotPlanner: self.frame += 1 def update_follow_values(self, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles): + # Offset by FrogAi for FrogPilot for a more natural approach to a faster lead + if frogpilot_toggles.aggressive_acceleration_experimental and v_lead > v_ego: + distance_factor = max(lead_distance - (v_ego * self.t_follow), 1) + standstill_offset = max(stopping_distance - v_ego, 0) * max(v_lead - v_ego, 0) + acceleration_offset = np.clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) + self.acceleration_jerk = self.base_acceleration_jerk / acceleration_offset + self.speed_jerk = self.base_speed_jerk / acceleration_offset + self.t_follow /= acceleration_offset + elif frogpilot_toggles.aggressive_acceleration and v_lead > v_ego: + distance_factor = np.maximum(lead_distance - (v_lead * self.t_follow), 1) + 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) + self.t_follow /= acceleration_offset + # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead if frogpilot_toggles.conditional_experimental_mode and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)