mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-27 09:53:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

add min_v_following look up table

Browse Source
This commit is contained in:
Rick Lan
2020-03-25 16:44:45 +10:00
parent d9ee484830
commit cbf3453028
1 changed files with 6 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/planner.py
+6 -9
View File
@@ -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])