diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6b4f78dc1..cc009f1f8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -220,6 +220,9 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0 LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15 +RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA = 0.10 +RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL = 0.12 +RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN = 0.18 RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75 RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04 RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75 @@ -1505,9 +1508,20 @@ class LongitudinalPlanner: tracking_lead_active and self.lead_is_matched_follow_window(lead, v_ego, t_follow) ) + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + headway_margin = actual_headway - float(t_follow) if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN): return None + if ( + radar_matched_follow_active and + current_source in ("lead0", "lead1") and + lead_delta >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA and + float(getattr(lead, "aLeadK", 0.0)) >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL and + headway_margin >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN + ): + return None + if (radar_matched_follow_active and current_source == "cruise" and gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and lead_delta < min_lead_delta): diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9c766f677..2042d8500 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2604,6 +2604,46 @@ def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_ assert cap is None +def test_route_8bc6_radar_matched_follow_catchup_cap_skips_mild_pullaway_after_lead_lock(): + v_ego = 23.96 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=33.6, v_lead=24.23, a_lead=0.16, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.16, + current_source="lead0", + tracking_lead_active=True, + ) + + assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) + assert cap is None + + +def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed(): + v_ego = 23.96 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead( + status=True, d_rel=33.6, v_lead=24.23, a_lead=0.08, radar=True, model_prob=1.0, y_rel=0.0, + ) + + cap = planner.get_lead_catchup_accel_cap( + lead, + v_ego, + 1.16, + current_source="lead0", + tracking_lead_active=True, + ) + + assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16) + assert cap is not None + + def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave(): v_ego = 22.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)