diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7296a28b11..31e193911e 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -102,11 +102,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. return np.interp(v_ego, x_vel, y_dist) +def get_stop_distance(v_ego): + v_ego = np.asarray(v_ego) + stop_distance = np.where(v_ego < 1.5, 8.0, STOP_DISTANCE) + return stop_distance + def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) def get_safe_obstacle_distance(v_ego, t_follow): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + stop_distance = get_stop_distance(v_ego) + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance def desired_follow_distance(v_ego, v_lead, t_follow=None): if t_follow is None: