From 7d939e1c763ea29727b2b93d2f1c1f272eccb338 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 14 Jun 2026 17:10:32 -0500 Subject: [PATCH] planner work --- .../lib/longitudinal_mpc_lib/long_mpc.py | 47 ++++++++++++++----- .../tests/test_longitudinal_planner.py | 22 +++++++++ 2 files changed, 57 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index f304513cb..761af133c 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -77,9 +77,14 @@ FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0 FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9 -RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED = 12.0 -RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0 -RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14 +STABLE_FOLLOW_CRUISE_MIN_SPEED = 12.0 +STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0 +STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14 +STABLE_FOLLOW_CRUISE_MAX_REL_SPEED = 2.5 +STABLE_FOLLOW_CRUISE_MIN_HEADWAY = 0.95 +STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET = 0.35 +STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET = 0.90 +STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE = 0.35 NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0 NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9 NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE = 0.35 @@ -573,24 +578,42 @@ class LongitudinalMpc: return lead_xv @staticmethod - def get_radarless_matched_follow_cruise_hysteresis(lead, v_ego, t_follow): + def get_stable_follow_cruise_hysteresis(lead, v_ego, t_follow): if lead is None or not lead.status: return 0.0 - if not is_radarless_matched_follow_window( + lead_radar = bool(getattr(lead, "radar", False)) + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + if lead_brake > STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE: + return 0.0 + + if lead_radar: + if float(t_follow) <= 0.0 or float(v_ego) < STABLE_FOLLOW_CRUISE_MIN_SPEED: + return 0.0 + relative_speed = float(v_ego) - float(lead.vLead) + if abs(relative_speed) > STABLE_FOLLOW_CRUISE_MAX_REL_SPEED: + return 0.0 + + actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3) + min_headway = max(STABLE_FOLLOW_CRUISE_MIN_HEADWAY, + float(t_follow) - STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET) + max_headway = float(t_follow) + STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET + if not (min_headway <= actual_headway <= max_headway): + return 0.0 + elif not is_radarless_matched_follow_window( v_ego, lead.dRel, lead.vLead, t_follow, - radar=bool(getattr(lead, "radar", False)), - lead_brake=max(0.0, -float(getattr(lead, "aLeadK", 0.0))), + radar=lead_radar, + lead_brake=lead_brake, lead_prob=float(getattr(lead, "modelProb", 0.0)), - min_speed=RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED, + min_speed=STABLE_FOLLOW_CRUISE_MIN_SPEED, ): return 0.0 - return max(RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN, - RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego)) + return max(STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN, + STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego)) @staticmethod def leads_are_near_duplicates(lead_one, lead_two, v_ego): @@ -670,9 +693,9 @@ class LongitudinalMpc: prev_source = self.source if optional_far_lead_comfort: if prev_source == 'lead0': - cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow) + cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_one, v_ego, t_follow) elif prev_source == 'lead1': - cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow) + cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_two, v_ego, t_follow) if optional_far_lead_comfort and tracking_lead and lead_one.status: desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) closing_speed = max(0.0, v_ego - lead_one.vLead) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3a46811dd..3c20c231c 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2368,6 +2368,28 @@ def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): assert lead_1_bias > 0.0 +def test_stable_follow_cruise_hysteresis_applies_for_radar_lead(): + v_ego = 27.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=40.5, v_lead=26.3, a_lead=-0.02, radar=True, model_prob=1.0) + + hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) + + assert hysteresis > 0.0 + + +def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead(): + v_ego = 27.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=40.5, v_lead=21.5, a_lead=-0.02, radar=True, model_prob=1.0) + + hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) + + assert hysteresis == 0.0 + + def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)