From 64bb95e3cff914fe6465dbeaeb80def1ffce03e1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 18 Jun 2026 12:20:17 -0500 Subject: [PATCH] Applebees --- selfdrive/controls/lib/latcontrol_torque.py | 6 ++--- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++--- .../tests/test_longitudinal_planner.py | 25 +++++++++++++++++++ 3 files changed, 44 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 69ac8986a..f1d69dbd6 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -493,18 +493,18 @@ IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 IONIQ_6_TURN_IN_BOOST_LEFT = 1.64 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.88 +IONIQ_6_TURN_IN_BOOST_RIGHT = 2.10 IONIQ_6_UNWIND_TAPER_LEFT = 3.18 IONIQ_6_UNWIND_TAPER_RIGHT = 8.20 IONIQ_6_FRICTION_MULT = 0.928 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.78 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.24 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.42 IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.90 IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 10.20 IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.44 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.78 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.94 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10 IONIQ_6_CENTER_TAPER_MAX = 0.082 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a291292ba..3b7fcb797 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1813,17 +1813,25 @@ class LongitudinalPlanner: current_source, tracking_lead_active): if lead is None or not lead.status: return None - if current_source not in ("lead0", "lead1") and not tracking_lead_active: + if current_source not in ("cruise", "lead0", "lead1") and not tracking_lead_active: return None if not (self.lead_one.status and self.lead_two.status): return None if not self.mpc.leads_are_near_duplicates(self.lead_one, self.lead_two, v_ego): return None - if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED: + low_speed_extension_active = bool( + tracking_lead_active and + current_source in ("cruise", "lead0", "lead1") and + LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED <= float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED + ) + if float(v_ego) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_SPEED: + return None + if float(v_ego) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_SPEED and not low_speed_extension_active: return None - lead_prob = float(getattr(lead, "modelProb", 0.0)) - if bool(getattr(lead, "radar", False)) or lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB: + lead_radar = bool(getattr(lead, "radar", False)) + lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0)) + if not lead_radar and lead_prob < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_MODEL_PROB: return None lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) @@ -1849,6 +1857,10 @@ class LongitudinalPlanner: if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A: return None + if low_speed_extension_active and (float(prev_output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET or + float(output_a_target) < LOW_SPEED_MATCHED_FOLLOW_TRANSITION_MIN_TARGET): + return None + positive_step = NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP negative_step = NEAR_DUPLICATE_LEAD_TRANSITION_NEGATIVE_STEP if float(prev_output_a_target) * float(output_a_target) < 0.0: diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index c91e81cdb..bb4010925 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -2730,6 +2730,31 @@ def test_near_duplicate_lead_transition_target_damps_tracking_cruise_sign_flip() assert smoothed == pytest.approx(-0.92, abs=1e-6) +def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_handoff(): + v_ego = 14.31 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=25.7, v_lead=14.61, a_lead=0.0, radar=True, model_prob=1.0) + lead_one.vRel = lead_one.vLead - v_ego + lead_two.vRel = lead_two.vLead - v_ego + planner.lead_one = lead_one + planner.lead_two = lead_two + + smoothed = planner.get_near_duplicate_lead_transition_target( + lead_one, + v_ego, + 1.45, + prev_output_a_target=0.68, + output_a_target=0.03, + current_source="lead0", + tracking_lead_active=True, + ) + + assert smoothed is not None + assert smoothed == pytest.approx(0.36, abs=1e-6) + + def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads(): v_ego = 24.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)