diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 3d323e6c8..30fd8e583 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -23,10 +23,10 @@ AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distract # lookup tables VS speed to determine min and max accels in cruise # make sure these accelerations are smaller than mpc limits +_A_CRUISE_MIN_V = [-2.0, -1.5, -1.0, -0.7, -0.5] _A_CRUISE_MIN_V_ECO = [-1.0, -0.7, -0.6, -0.5, -0.3] _A_CRUISE_MIN_V_SPORT = [-3.0, -2.6, -2.3, -2.0, -1.0] - -_A_CRUISE_MIN_V = [-2.0, -1.5, -1.0, -0.7, -0.5] +_A_CRUISE_MIN_V_FOLLOWING = [-4.0, -4.0, -3.5, -2.5, -2.0] _A_CRUISE_MIN_BP = [0.0, 5.0, 10.0, 20.0, 55.0] # need fast accel at very low speed for stop and go @@ -50,21 +50,18 @@ ACCEL_NORMAL_MODE = 0 ACCEL_SPORT_MODE = 1 def calc_cruise_accel_limits(v_ego, following, accel_profile): - if accel_profile == ACCEL_ECO_MODE: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO) - elif accel_profile == ACCEL_SPORT_MODE: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) - else: - a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) - if following: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_FOLLOWING) a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_FOLLOWING) else: if accel_profile == ACCEL_ECO_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_ECO) a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_ECO) elif accel_profile == ACCEL_SPORT_MODE: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V_SPORT) a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V_SPORT) else: + a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V) a_cruise_max = interp(v_ego, _A_CRUISE_MAX_BP, _A_CRUISE_MAX_V) return np.vstack([a_cruise_min, a_cruise_max])