diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 91e63c202..2c64077fe 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -89,7 +89,9 @@ class FrogPilotPlanner: self.update_v_cruise(carState, controlsState, frogpilotCarState, frogpilotNavigation, modelData, v_cruise, v_ego, frogpilot_toggles) def set_acceleration(self, controlsState, frogpilotCarState, v_cruise, v_ego, frogpilot_toggles): - if frogpilot_toggles.acceleration_profile == 1: + if self.tracking_lead and frogpilot_toggles.aggressive_acceleration: + self.max_accel = clip(self.lead_one.aLeadK, get_max_accel_sport_plus(v_ego), 2.0 if v_ego >= 20 else 4.0) + elif frogpilot_toggles.acceleration_profile == 1: self.max_accel = get_max_accel_eco(v_ego) elif frogpilot_toggles.acceleration_profile == 2: self.max_accel = get_max_accel_sport(v_ego) @@ -142,6 +144,16 @@ class FrogPilotPlanner: self.tracking_lead = self.tracking_lead_mac.get_moving_average() >= PROBABILITY 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 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 = clip((v_lead - v_ego) + standstill_offset - COMFORT_BRAKE, 1, distance_factor) + self.acceleration_jerk = self.base_acceleration_jerk / acceleration_offset + self.danger_jerk = self.base_danger_jerk / acceleration_offset + self.speed_jerk = self.base_speed_jerk / acceleration_offset + self.t_follow /= acceleration_offset + # Offset by FrogAi for FrogPilot for a more natural approach to a slower lead if frogpilot_toggles.conditional_slower_lead and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)