Reaction buffer on follow distance (#23112)

* Reaction time buffer * new ref
4 years ago · 5b0e10ea33
parent 37e2de2244
commit 5b0e10ea33
2 changed files with 6 additions and 4 deletions
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@ -49,14 +49,16 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N
 T_IDXS = np.array(T_IDXS_LST)
 T_DIFFS = np.diff(T_IDXS, prepend=[0.])
 MIN_ACCEL = -3.5
-T_REACT = 1.45
+T_REACT = 0.5
+T_FOLLOW = 1.45
 COMFORT_BRAKE = 2.0
+STOP_DISTANCE = 6.0

 def get_stopped_equivalence_factor(v_lead):
-  return v_lead**2 / (2 * COMFORT_BRAKE) - T_REACT * v_lead
+  return v_lead**2 / (2 * COMFORT_BRAKE) - (T_FOLLOW - T_REACT) * v_lead

 def get_safe_obstacle_distance(v_ego):
-  return (v_ego*v_ego) / (2 * COMFORT_BRAKE) + 6.0
+  return (v_ego*v_ego) / (2 * COMFORT_BRAKE) + T_REACT * v_ego + STOP_DISTANCE

 def desired_follow_distance(v_ego, v_lead):
  return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@ -1 +1 @@
-fb4318e66ce98c9bc267b0778d9238265c1c5b17
+6ab94cfcfe183a09e75e9f8b67a7ee2a2d3630df