From f9be24b154489f4f5e21070285893955a065dba6 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 28 Jul 2024 22:32:44 -0400 Subject: [PATCH 1/2] Revert "try this to make car stop a bit further" This reverts commit 04872944955f8cc506cc8e6c7412c9ce94a42b33. --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a2df527a55..7296a28b11 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST) FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 6.0 +STOP_DISTANCE = 8.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 48874282e8..62ed68a77e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -83,7 +83,7 @@ class LongitudinalPlanner: self.dt = dt self.a_desired = init_a - self.v_desired_filter = FirstOrderFilter(init_v, 0.5, self.dt) + self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) self.v_model_error = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) From cb76fb797fa58e0a14017b49b1b4565ce899be18 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 28 Jul 2024 22:44:01 -0400 Subject: [PATCH 2/2] dynamic stopping distance --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7296a28b11..20838608be 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -55,7 +55,7 @@ T_IDXS = np.array(T_IDXS_LST) FCW_IDXS = T_IDXS < 5.0 T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 -STOP_DISTANCE = 8.0 +STOP_DISTANCE = 6.0 def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): if personality==custom.LongitudinalPersonalitySP.relaxed: @@ -102,11 +102,17 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. return np.interp(v_ego, x_vel, y_dist) +def get_stop_distance(v_ego): + v_ego = np.asarray(v_ego) + stop_distance = np.where(v_ego < 1.5, 8.0, STOP_DISTANCE) + return stop_distance + def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) def get_safe_obstacle_distance(v_ego, t_follow): - return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE + stop_distance = get_stop_distance(v_ego) + return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance def desired_follow_distance(v_ego, v_lead, t_follow=None): if t_follow is None: