From f9d6c0d648ff225ec734b6f9446948ee5cc40e9b Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 29 Apr 2026 15:38:45 -0500 Subject: [PATCH] Doms plan small update --- .../controls/lib/longitudinal_planner.py | 29 ++++++++++++------- .../tests/test_longitudinal_planner.py | 8 ++--- 2 files changed, 23 insertions(+), 14 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0e8e33158..1648aa491 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -35,11 +35,11 @@ VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0 VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5 VISION_LEAD_APPROACH_FULL_TIME = 1.0 VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0 -VISION_LEAD_APPROACH_MAX_DECEL = 0.65 +VISION_LEAD_APPROACH_MAX_DECEL = 0.80 VISION_LEAD_APPROACH_MIN_DECEL = 0.15 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 -VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.15 +VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30 VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0 VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20 VISION_SLOW_LEAD_MAX_SPEED = 5.0 @@ -68,12 +68,12 @@ VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5 VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75 VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9 VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35 -VISION_LOW_SPEED_STOP_BUFFER_BASE = 2.8 +VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.2 VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80 VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25 -VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.75 -VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.6 -VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 0.9 +VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9 +VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8 +VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.1 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25 # Uncertainty-based filter disable thresholds @@ -736,24 +736,30 @@ class LongitudinalPlanner: self.v_desired_trajectory, self.a_desired_trajectory, action_t=action_t, vEgoStopping=starpilot_toggles.vEgoStopping) - output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] + comfort_output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0] + vision_cap_accel_min = min(comfort_output_accel_min, get_vehicle_min_accel(self.CP, v_ego)) + output_accel_min = comfort_output_accel_min close_lead_caps = [] vision_low_speed_stop_active = False + vision_brake_cap_active = False if lead_control_active: for lead in (self.lead_one, self.lead_two): cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min) if cap is not None: close_lead_caps.append(cap) - slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, output_accel_min, effective_t_follow) + slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow) if slow_stop_cap is not None: close_lead_caps.append(slow_stop_cap) - approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow) + vision_brake_cap_active = True + approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow) if approach_cap is not None: close_lead_caps.append(approach_cap) - low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, output_accel_min) + vision_brake_cap_active = True + low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, vision_cap_accel_min) if low_speed_stop_cap is not None: close_lead_caps.append(low_speed_stop_cap) + vision_brake_cap_active = True vision_low_speed_stop_active |= low_speed_stop_active if close_lead_caps: close_lead_brake_cap = min(close_lead_caps) @@ -772,6 +778,9 @@ class LongitudinalPlanner: cruise_accel_cap = (v_cruise - v_ego + 0.01) / max(action_t, self.dt) output_a_target = min(output_a_target, cruise_accel_cap) + if vision_brake_cap_active: + output_accel_min = min(output_accel_min, vision_cap_accel_min) + output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1] output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max)) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9ef14777f..e5945f94f 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -345,7 +345,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers sm_approach = make_sm( approach_v_ego, desired_accel=0.2, - min_accel=-3.0, + min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984), @@ -353,7 +353,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers sm_close = make_sm( close_v_ego, desired_accel=0.2, - min_accel=-3.0, + min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=27.18, v_lead=15.76, a_lead=-0.824, radar=False, model_prob=0.988), @@ -366,7 +366,7 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers assert planner_approach.mode == "acc" assert planner_close.mode == "acc" - assert planner_approach.output_a_target < -0.5 + assert planner_approach.output_a_target < -0.6 assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 @@ -379,7 +379,7 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap( sm = make_sm( v_ego, desired_accel=0.1, - min_accel=-3.0, + min_accel=-0.5, experimental_mode=False, tracking_lead=True, lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99),