diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 0ef4712..3d0d66e 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -134,10 +134,16 @@ class FrogPilotPlanner: 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: + if (frogpilot_toggles.conditional_experimental_mode or frogpilot_toggles.smoother_braking) and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) - braking_offset = np.clip((v_ego - v_lead) - COMFORT_BRAKE, 1, distance_factor) - self.slower_lead = max(braking_offset, 1) > 1 + far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - stopping_distance + (v_lead - CITY_SPEED_LIMIT), 0) if frogpilot_toggles.smoother_braking_far_lead else 0 + braking_offset = np.clip((v_ego - v_lead) + far_lead_offset - COMFORT_BRAKE, 1, distance_factor) + if frogpilot_toggles.smoother_braking: + if frogpilot_toggles.smoother_braking_jerk: + self.acceleration_jerk = self.base_acceleration_jerk * min(braking_offset, COMFORT_BRAKE / 2) + self.speed_jerk = self.base_speed_jerk * min(braking_offset, COMFORT_BRAKE * 2) + self.t_follow /= braking_offset + self.slower_lead = max(braking_offset - far_lead_offset, 1) > 1 def update_v_cruise(self, carState, controlsState, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, v_cruise, v_ego, frogpilot_toggles): v_cruise_cluster = max(controlsState.vCruiseCluster, controlsState.vCruise) * CV.KPH_TO_MS