Long policy: strictly squared stopped equivalence function (#23118)

* new ref

* never let lead stopped equivalence go down

* no overbraking

* stricter again

* cleanup

* updated refs

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -49,16 +49,15 @@ 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 = 0.5
 T_FOLLOW = 1.45
-COMFORT_BRAKE = 2.0
+COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
 def get_stopped_equivalence_factor(v_lead):
-  return v_lead**2 / (2 * COMFORT_BRAKE) - (T_FOLLOW - T_REACT) * v_lead
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
 
 def get_safe_obstacle_distance(v_ego):
-  return (v_ego*v_ego) / (2 * COMFORT_BRAKE) + T_REACT * v_ego + STOP_DISTANCE
+  return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * 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)

selfdrive/controls/tests/test_following_distance.py

@@ -29,7 +29,7 @@ class TestFollowingDistance(unittest.TestCase):
       simulation_steady_state = run_following_distance_simulation(v_lead)
       correct_steady_state = desired_follow_distance(v_lead, v_lead)
 
-      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + 1.0))
+      self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .5))
 
 
 if __name__ == "__main__":

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-7ad16d3db047bbcbf91c704e8a52f91e5fc08690
+7bcb9e414784d5ca8a4338680421023d07ef9e53