diff --git a/frogpilot/controls/lib/frogpilot_following.py b/frogpilot/controls/lib/frogpilot_following.py index ad3318baf..9bb9ebb7e 100644 --- a/frogpilot/controls/lib/frogpilot_following.py +++ b/frogpilot/controls/lib/frogpilot_following.py @@ -7,8 +7,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFO from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] -# Delay blending to the high-speed following value until much higher speeds (was ~55 mph) -PERSONALITY_BP = [20. * CV.KPH_TO_MS, 100. * CV.MPH_TO_MS] +PERSONALITY_BP = [20. * CV.KPH_TO_MS, 90. * CV.KPH_TO_MS] class FrogPilotFollowing: def __init__(self, FrogPilotPlanner): diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index aa48421f0..a7ae314c8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -500,6 +500,11 @@ class LongitudinalMpc: x_lead = clip(x_lead, min_x_lead, 1e8) v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) + # Apply smoothing filters with interp scaling + self.lead_a_filter.update(a_lead) + self.lead_v_filter.update(v_lead) + a_lead = self.lead_a_filter.x + v_lead = self.lead_v_filter.x lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) return lead_xv