From 1714c516ab952bb1b6cdac848bd9493f043f3d4e Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 9 May 2026 08:54:04 -0500 Subject: [PATCH] all the small things --- selfdrive/controls/lib/latcontrol_torque.py | 28 +++++------ .../controls/lib/longitudinal_planner.py | 41 ++++++++++++++++- .../tests/test_longitudinal_planner.py | 46 +++++++++++++++++-- 3 files changed, 96 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index b818cfa25..40b891639 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -277,22 +277,22 @@ IONIQ_6_FF_CUTOFF = 0.48 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.44 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.56 -IONIQ_6_UNWIND_TAPER_LEFT = 2.46 -IONIQ_6_UNWIND_TAPER_RIGHT = 5.60 -IONIQ_6_FRICTION_MULT = 0.955 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.50 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.70 +IONIQ_6_UNWIND_TAPER_LEFT = 2.62 +IONIQ_6_UNWIND_TAPER_RIGHT = 6.05 +IONIQ_6_FRICTION_MULT = 0.948 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.60 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.90 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.90 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.95 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.32 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.55 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.65 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.35 -IONIQ_6_CENTER_TAPER_MAX = 0.066 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.66 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 1.05 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 3.10 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 7.40 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.36 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.64 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.85 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.90 +IONIQ_6_CENTER_TAPER_MAX = 0.070 IONIQ_6_CENTER_TAPER_LAT = 0.215 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 IONIQ_6_CENTER_TAPER_SPEED = 18.0 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c661172a8..1ebdc19cb 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -49,6 +49,12 @@ VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45 VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20 VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30 VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75 +VISION_LEAD_APPROACH_CONFIRM_TIME = 0.25 +VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL = 1.0 +VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED = 4.0 +VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE = 0.20 +VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN = 28.0 +VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME = 0.85 VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9 VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97 VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0 @@ -282,6 +288,7 @@ class LongitudinalPlanner: self._uncert_last_t = None self.effective_t_follow = None self.vision_low_speed_stop_hold_until = 0.0 + self.vision_lead_approach_confirm_t = 0.0 self.untracked_slow_lead_confirm_t = 0.0 if self.is_preap: @@ -519,6 +526,19 @@ class LongitudinalPlanner: return max(accel_min, -approach_decel) + def tracked_vision_lead_approach_needs_immediate_brake(self, lead, v_ego, approach_cap): + lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt) + projected_closing_speed = max(0.0, v_ego - float(lead.vLead)) + lead_brake * reaction_t + bypass_distance = max(VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_MIN, + VISION_LEAD_APPROACH_CONFIRM_BYPASS_DISTANCE_TIME * float(v_ego)) + return ( + approach_cap <= -VISION_LEAD_APPROACH_CONFIRM_BYPASS_DECEL or + projected_closing_speed >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_CLOSING_SPEED or + lead_brake >= VISION_LEAD_APPROACH_CONFIRM_BYPASS_LEAD_BRAKE or + float(lead.dRel) <= bypass_distance + ) + def get_dynamic_t_follow(self, base_t_follow, lead, v_ego): base_t_follow = float(base_t_follow) target_t_follow = base_t_follow @@ -956,6 +976,7 @@ class LongitudinalPlanner: self.untracked_slow_lead_confirm_t = 0.0 close_lead_caps = [] + tracked_vision_approach_caps = [] vision_low_speed_stop_active = False vision_brake_cap_active = False if lead_control_active: @@ -969,13 +990,29 @@ class LongitudinalPlanner: vision_brake_cap_active = True approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, vision_cap_accel_min, effective_t_follow) if approach_cap is not None: - close_lead_caps.append(approach_cap) - vision_brake_cap_active = True + tracked_vision_approach_caps.append(( + approach_cap, + self.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap), + )) low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, vision_cap_accel_min) if low_speed_stop_cap is not None: close_lead_caps.append(low_speed_stop_cap) vision_brake_cap_active = True vision_low_speed_stop_active |= low_speed_stop_active + if tracked_vision_approach_caps: + if any(immediate for _, immediate in tracked_vision_approach_caps): + self.vision_lead_approach_confirm_t = VISION_LEAD_APPROACH_CONFIRM_TIME + else: + self.vision_lead_approach_confirm_t = min( + self.vision_lead_approach_confirm_t + self.dt, + VISION_LEAD_APPROACH_CONFIRM_TIME, + ) + + if self.vision_lead_approach_confirm_t >= VISION_LEAD_APPROACH_CONFIRM_TIME: + close_lead_caps.append(min(cap for cap, _ in tracked_vision_approach_caps)) + vision_brake_cap_active = True + else: + self.vision_lead_approach_confirm_t = 0.0 if close_lead_caps: close_lead_brake_cap = min(close_lead_caps) self.a_desired = min(self.a_desired, close_lead_brake_cap) diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 2d3bffa1f..f1efe75bc 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -430,13 +430,53 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers sm_approach["starpilotPlan"].vCruise = approach_v_ego + 8.0 sm_close["starpilotPlan"].vCruise = close_v_ego + 8.0 - planner_approach.update(sm_approach, make_toggles(model_version)) + approach_outputs = [] + for _ in range(6): + planner_approach.update(sm_approach, make_toggles(model_version)) + approach_outputs.append(planner_approach.output_a_target) + planner_close.update(sm_close, make_toggles(model_version)) assert planner_approach.mode == "acc" assert planner_close.mode == "acc" - assert planner_approach.output_a_target < -0.6 - assert planner_close.output_a_target < planner_approach.output_a_target - 0.25 + assert min(approach_outputs[:2]) > -0.55 + assert approach_outputs[-1] < -1.3 + assert planner_close.output_a_target < approach_outputs[0] - 0.8 + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_tracked_vision_far_mild_closure_does_not_bypass_persistence(model_version): + v_ego = 37.45 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=42.8, v_lead=35.31, a_lead=0.18, radar=False, model_prob=0.98) + + approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) + + assert approach_cap is not None + assert approach_cap > -1.0 + assert not planner.tracked_vision_lead_approach_needs_immediate_brake(lead, v_ego, approach_cap) + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"]) +def test_acc_mode_tracked_vision_close_or_braking_lead_bypasses_persistence(model_version): + v_ego = 19.50 + + 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=True, + lead_one=make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98), + ) + sm["starpilotPlan"].vCruise = v_ego + 6.0 + + planner.update(sm, make_toggles(model_version)) + + assert planner.output_a_target < -1.3 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])