diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 736620d7e..c661172a8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -44,6 +44,11 @@ VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98 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_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75 +VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45 +VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20 +VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30 +VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75 VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9 VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97 VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0 @@ -396,6 +401,18 @@ class LongitudinalPlanner: deficit_decel = VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL * deficit_factor * prob_factor deficit_decel *= 0.5 + 0.5 * closing_factor approach_decel = max(approach_decel, deficit_decel) + + # If a tracked vision lead is already far inside the tight-follow window and + # it is actively braking, don't stay stuck at the softer comfort cap. + if deficit_factor >= VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN and lead_brake >= VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE: + braking_floor = float(np.interp( + lead_brake, + [VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE, VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE], + [VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL, VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL], + )) + braking_floor *= 0.85 + 0.15 * max(closing_factor, prob_factor) + approach_decel = max(approach_decel, braking_floor) + if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL: return None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3ecc4beff..2d3bffa1f 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -228,6 +228,21 @@ def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap(): assert approach_cap < -0.5 +def test_vision_lead_approach_cap_brakes_harder_for_braking_tracked_lead_inside_tight_gap(): + v_ego = 19.50 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98) + + hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -3.0) + approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -3.0, 1.45) + + assert hard_cap == pytest.approx(-1.01, abs=0.03) + assert approach_cap is not None + assert approach_cap < -1.35 + assert approach_cap < hard_cap + + def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap(): v_ego = 19.37 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)