diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index bf6dd04f60..d9edeec6f3 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -39,19 +39,17 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, b return float(new_curvature), limited_accel or limited_max_curv -def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05): +def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.1): if len(speeds) == len(t_idxs): v_now = speeds[0] a_now = accels[0] v_target = np.interp(action_t, t_idxs, speeds) a_target = 2 * (v_target - v_now) / (action_t) - a_now - v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds) else: v_target = 0.0 - v_target_1sec = 0.0 a_target = 0.0 should_stop = (v_target < vEgoStopping and - v_target_1sec < vEgoStopping) + a_target < 0.2) # even teslas cannot a<0.2 when starting from stop return a_target, should_stop def curv_from_psis(psi_target, psi_rate, vego, action_t):