longitudinal_planner: allow for zero actuator delay (#33855)

Allow zero actuator delay
1 year ago · 0ff498cc83
parent cf420ed001
commit 0ff498cc83
1 changed files with 4 additions and 1 deletions
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@ -56,7 +56,10 @@ def get_accel_from_plan(CP, speeds, accels):
    a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)

    v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
-    a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
+    if v_target != v_target_now:
+      a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
+    else:
+      a_target = a_target_now

    v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
  else: