From 8bae6a1063e5ef0679b5d6ba36b3c467a385f806 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 26 Apr 2026 23:50:48 -0500 Subject: [PATCH] Dom's Plan --- .../lib/longitudinal_mpc_lib/long_mpc.py | 38 +++++++++++-- .../controls/lib/longitudinal_planner.py | 4 ++ .../tests/test_longitudinal_planner.py | 53 +++++++++++++++++++ 3 files changed, 92 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c665ce7e3..743e0b65f 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -71,6 +71,13 @@ DIST_ADAPTS = [0.04, 0.06, 0.06, 0.05] # Balanced across speeds # ===== END TUNING PARAMETERS ===== +FAR_RADAR_LEAD_ACCEL_TAPER_MAX = 1.0 +FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING = 2.5 +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 + # Function to get parameter value based on current speed def get_speed_based_param(speed_mph, param_array): """Get parameter value based on current speed using smooth interpolation""" @@ -167,6 +174,28 @@ def desired_follow_distance(v_ego, v_lead, t_follow=None): return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead) +def soften_far_radar_lead_accel(d_rel, v_lead, a_lead, v_ego, t_follow, *, radar=True): + if not radar or a_lead >= 0.0: + return float(a_lead) + + desired_gap = float(desired_follow_distance(v_ego, v_lead, t_follow)) + closing_speed = max(0.0, float(v_ego) - float(v_lead)) + gap_excess = float(d_rel) - desired_gap + + taper_start = max(FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS, + FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN * float(v_ego)) + if gap_excess <= taper_start or closing_speed >= FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING: + return float(a_lead) + + taper_scale = max(FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS, + FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN * float(v_ego)) + distance_factor = float(np.clip((gap_excess - taper_start) / taper_scale, 0.0, 1.0)) + closing_factor = float(np.clip((FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING - closing_speed) / + FAR_RADAR_LEAD_ACCEL_TAPER_MAX_CLOSING, 0.0, 1.0)) + taper = FAR_RADAR_LEAD_ACCEL_TAPER_MAX * distance_factor * closing_factor + return float(a_lead * (1.0 - taper)) + + def gen_long_model(): model = AcadosModel() model.name = MODEL_NAME @@ -484,13 +513,16 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead, tracking_lead=True): + def process_lead(self, lead, tracking_lead=True, t_follow=None): v_ego = self.x0[1] if lead is not None and lead.status and tracking_lead: x_lead = lead.dRel v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau + a_lead = soften_far_radar_lead_accel(x_lead, v_lead, a_lead, v_ego, + get_T_FOLLOW() if t_follow is None else t_follow, + radar=bool(getattr(lead, "radar", False))) else: # Fake a fast lead car, so mpc can keep running in the same mode x_lead = 50.0 @@ -525,8 +557,8 @@ class LongitudinalMpc: lead_two = radarstate.leadTwo self.status = tracking_lead and (lead_one.status or lead_two.status) - lead_xv_0 = self.process_lead(lead_one, tracking_lead) - lead_xv_1 = self.process_lead(lead_two, tracking_lead) + lead_xv_0 = self.process_lead(lead_one, tracking_lead, t_follow=t_follow) + lead_xv_1 = self.process_lead(lead_two, tracking_lead, t_follow=t_follow) # To estimate a safe distance from a moving lead, we calculate how much stopping # distance that lead needs as a minimum. We can add that to the current distance diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 928cc5bbd..bb7b2f30f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -29,6 +29,7 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5 RAW_LEAD_SAFETY_TTC = 7.0 RAW_LEAD_SAFETY_DISTANCE = 40.0 +CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0 # Uncertainty-based filter disable thresholds UNCERT_SLOPE_TRIG = 0.12 # per second @@ -238,6 +239,9 @@ class LongitudinalPlanner: target_gap = float(np.clip(2.0 + 0.2 * v_ego, 2.0, 6.0)) delay_buffer = projected_closing_speed * reaction_t available_gap = max(float(lead.dRel) - target_gap - delay_buffer, 0.5) + projected_ttc = available_gap / max(projected_closing_speed, 0.1) + if projected_ttc > CLOSE_LEAD_BRAKE_CAP_MAX_TTC: + return None required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap) + 0.7 * lead_brake if required_decel < 0.2: return None diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 122dcfe3f..63498c3a0 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -10,6 +10,7 @@ from opendbc.car.honda.interface import CarInterface from opendbc.car.honda.values import CAR from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_vehicle_min_accel +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel from openpilot.selfdrive.modeld.constants import ModelConstants, Plan @@ -188,6 +189,58 @@ def test_acc_mode_matches_no_lead_baseline_for_far_vision_only_lead_without_trac np.testing.assert_allclose(far_vision_outputs, no_lead_outputs, atol=1e-6) +def test_soften_far_radar_lead_accel_reduces_gentle_far_brake(): + softened = soften_far_radar_lead_accel(114.8, 28.88, -0.75, 29.26, 1.45, radar=True) + assert softened > -0.35 + assert softened < 0.0 + + +def test_soften_far_radar_lead_accel_keeps_close_closing_brake(): + baseline = -0.76 + softened = soften_far_radar_lead_accel(68.0, 26.38, baseline, 29.38, 1.45, radar=True) + assert softened == pytest.approx(baseline) + + +@pytest.mark.parametrize("model_version", ["v11", "v12"]) +def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version): + far_v_ego = 29.26 + far_v_cruise = 32.22 + close_v_ego = 29.38 + close_v_cruise = 32.22 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_far = LongitudinalPlanner(CP, init_v=far_v_ego) + planner_close = LongitudinalPlanner(CP, init_v=close_v_ego) + + sm_far = make_sm( + far_v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9), + ) + sm_close = make_sm( + close_v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=68.0, v_lead=26.38, a_lead=-0.76, radar=True, model_prob=0.9), + ) + sm_far["starpilotPlan"].vCruise = far_v_cruise + sm_close["starpilotPlan"].vCruise = close_v_cruise + + for _ in range(80): + planner_far.update(sm_far, make_toggles(model_version)) + planner_close.update(sm_close, make_toggles(model_version)) + + assert planner_far.mode == "acc" + assert planner_close.mode == "acc" + assert planner_far.output_a_target > -0.4 + assert planner_close.output_a_target < planner_far.output_a_target - 0.1 + + def test_modeld_action_passes_tomb_raider_longitudinal_params(monkeypatch): monkeypatch.setenv("DEBUG", "0") fake_commonmodel = types.ModuleType("openpilot.selfdrive.modeld.models.commonmodel_pyx")