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

Revert "Safer get accel (#37918)"

Browse Source 
This reverts commit 545ad018e0.
This commit is contained in:
Adeeb Shihadeh
2026-04-26 10:15:12 -07:00
parent 545ad018e0
commit 3af3c7e748
1 changed files with 2 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/drive_helpers.py
+2 -9
View File
@@ -12,7 +12,6 @@ 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):
@@ -44,10 +43,7 @@ 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]
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)
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
@@ -62,9 +58,6 @@ 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):
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_target = np.interp(action_t, t_idxs, yaws)
psi_rate = yaw_rates[0]
return curv_from_psis(psi_target, psi_rate, vego, action_t)