diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 54d4b0be31..f895f6b163 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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) diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 6661847b7f..a0110a4dab 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/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__": diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index bfef2971a7..38d5860398 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7ad16d3db047bbcbf91c704e8a52f91e5fc08690 \ No newline at end of file +7bcb9e414784d5ca8a4338680421023d07ef9e53 \ No newline at end of file