mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-07-26 03:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Safer get accel (#37918)

Browse Source 
* more robust accel fun

* dead
This commit is contained in:
Harald Schäfer
2026-04-26 09:48:25 -07:00
committed by GitHub
parent e86d4e86a6
commit 545ad018e0
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/drive_helpers.py
+9 -2
View File
@@ -12,6 +12,7 @@ MAX_VEL_ERR = 5.0 # m/s
# EU guidelines
MAX_LATERAL_JERK = 5.0 # m/s^3
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
MIN_STABLE_DELAY = 0.3
def clamp(val, min_val, max_val):
@@ -43,7 +44,10 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
v_target = np.interp(action_t, t_idxs, speeds)
if action_t < MIN_STABLE_DELAY:
v_target = v_now + (action_t / MIN_STABLE_DELAY) * (np.interp(MIN_STABLE_DELAY, t_idxs, speeds) - v_now)
else:
v_target = np.interp(action_t, t_idxs, speeds)
a_target = 2 * (v_target - v_now) / (action_t) - a_now
else:
v_now = 0.0
@@ -58,6 +62,9 @@ def curv_from_psis(psi_target, psi_rate, vego, action_t):
return 2*curv_from_psi - psi_rate / vego
def get_curvature_from_plan(yaws, yaw_rates, t_idxs, vego, action_t):
psi_target = np.interp(action_t, t_idxs, yaws)
if action_t < MIN_STABLE_DELAY:
psi_target = (action_t / MIN_STABLE_DELAY) * np.interp(MIN_STABLE_DELAY, t_idxs, yaws)
else:
psi_target = np.interp(action_t, t_idxs, yaws)
psi_rate = yaw_rates[0]
return curv_from_psis(psi_target, psi_rate, vego, action_t)