From 49aad9cce3fd50ed29e4d6bc8c08de12987b5f53 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 6 May 2026 10:38:47 -0500 Subject: [PATCH] =?UTF-8?q?t=C5=AFne?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- selfdrive/controls/lib/latcontrol_torque.py | 20 +++--- .../controls/lib/longitudinal_planner.py | 20 +++++- .../tests/test_longitudinal_planner.py | 72 +++++++++++++++++++ 3 files changed, 100 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4d56be495c..f9038791d8 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -70,20 +70,20 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.06 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.14 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.10 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.04 -CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.10 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07 CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.17 CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.25 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.06 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.05 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.08 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.09 CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.68 CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.86 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.03 -CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.03 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.045 CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.46 CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.70 @@ -267,8 +267,8 @@ IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.30 IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.50 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.55 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.10 -IONIQ_6_CENTER_TAPER_MAX = 0.056 -IONIQ_6_CENTER_TAPER_LAT = 0.22 +IONIQ_6_CENTER_TAPER_MAX = 0.060 +IONIQ_6_CENTER_TAPER_LAT = 0.21 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 74c4eaa411..4565d86a7f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -62,6 +62,8 @@ VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_LEAD_SPEED = 8.0 VISION_UNTRACKED_SLOW_LEAD_RELAXED_MAX_TTC = 10.0 VISION_UNTRACKED_SLOW_LEAD_RELAXED_MIN_CLOSING_SPEED = 10.0 VISION_UNTRACKED_SLOW_LEAD_RELAXED_FULL_CLOSING_SPEED = 16.0 +VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME = 0.30 +VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL = 0.55 VISION_SLOW_LEAD_MAX_SPEED = 5.0 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5 @@ -273,6 +275,7 @@ class LongitudinalPlanner: self._uncert_last_t = None self.effective_t_follow = None self.vision_low_speed_stop_hold_until = 0.0 + self.untracked_slow_lead_confirm_t = 0.0 if self.is_preap: try: @@ -909,8 +912,21 @@ class LongitudinalPlanner: if pretracking_vision_caps: pretracking_vision_cap = min(pretracking_vision_caps) - self.a_desired = min(self.a_desired, pretracking_vision_cap) - output_a_target = min(output_a_target, pretracking_vision_cap) + if pretracking_vision_cap <= -VISION_UNTRACKED_SLOW_LEAD_IMMEDIATE_DECEL: + self.untracked_slow_lead_confirm_t = VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME + else: + self.untracked_slow_lead_confirm_t = min( + self.untracked_slow_lead_confirm_t + self.dt, + VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME, + ) + + if self.untracked_slow_lead_confirm_t >= VISION_UNTRACKED_SLOW_LEAD_CONFIRM_TIME: + self.a_desired = min(self.a_desired, pretracking_vision_cap) + output_a_target = min(output_a_target, pretracking_vision_cap) + else: + self.untracked_slow_lead_confirm_t = 0.0 + else: + self.untracked_slow_lead_confirm_t = 0.0 close_lead_caps = [] vision_low_speed_stop_active = False diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 185e11b2aa..58bd0b18ce 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -500,6 +500,78 @@ def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_track assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_pretracking_vision_far_slower_lead_can_still_brake_immediately(model_version): + v_ego = 21.48 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + sm = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935), + ) + sm["starpilotPlan"].vCruise = v_ego + 6.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.mode == "acc" + assert planner.output_a_target < -0.45 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_pretracking_flappy_far_lead_requires_persistence(model_version): + v_ego = 26.09 + + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego) + planner_flappy = LongitudinalPlanner(CP, init_v=v_ego) + sm_no_lead = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + ) + sm_flappy = make_sm( + v_ego, + desired_accel=0.2, + min_accel=-1.0, + experimental_mode=False, + tracking_lead=False, + lead_one=make_lead(status=True, d_rel=74.75, v_lead=26.63, a_lead=0.01, radar=False, model_prob=0.989), + ) + sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0 + sm_flappy["starpilotPlan"].vCruise = v_ego + 6.0 + + flappy_sequence = [ + (74.75, 26.63, 0.01, 0.989), + (68.17, 20.81, 0.094, 0.971), + (69.73, 24.12, 0.057, 0.981), + (62.15, 21.38, 0.064, 0.983), + (66.29, 23.19, 0.069, 0.985), + (70.58, 27.51, 0.036, 0.988), + ] + + no_lead_outputs = [] + flappy_outputs = [] + for d_rel, v_lead, a_lead, model_prob in flappy_sequence: + planner_no_lead.update(sm_no_lead, make_toggles(model_version)) + sm_flappy["radarState"].leadOne = make_lead( + status=True, d_rel=d_rel, v_lead=v_lead, a_lead=a_lead, radar=False, model_prob=model_prob, + ) + planner_flappy.update(sm_flappy, make_toggles(model_version)) + no_lead_outputs.append(planner_no_lead.output_a_target) + flappy_outputs.append(planner_flappy.output_a_target) + + assert planner_flappy.mode == "acc" + assert min(flappy_outputs) > -0.05 + assert min(flappy_outputs) > min(no_lead_outputs) - 0.12 + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) def test_acc_mode_pretracking_near_stopped_vision_lead_does_not_relax_when_confidence_is_midrange(model_version): v_ego = 20.35