From 5d0ab0cfb07d69b1ed9ed904bda45a5a4487bb4d Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 7 Jun 2026 21:25:41 -0500 Subject: [PATCH] First time in San Juan mi hijo --- selfdrive/controls/lib/lead_behavior.py | 5 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 10 ++-- .../tests/test_conditional_chill_mode.py | 58 +++++++++++++++++++ .../controls/tests/test_lead_behavior.py | 8 +++ .../controls/lib/conditional_chill_mode.py | 29 +++++++++- 5 files changed, 103 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 391af6bd8..ed47945e3 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -35,8 +35,9 @@ def should_track_lead(lead_status: bool, lead_distance: float, model_length: flo def is_radarless_matched_follow_window(v_ego: float, lead_distance: float, v_lead: float, t_follow: float, *, radar: bool = False, lead_brake: float = 0.0, - lead_prob: float = 0.0) -> bool: - if radar or float(t_follow) <= 0.0 or float(v_ego) < RADARLESS_MATCHED_FOLLOW_MIN_SPEED: + lead_prob: float = 0.0, + min_speed: float = RADARLESS_MATCHED_FOLLOW_MIN_SPEED) -> bool: + if radar or float(t_follow) <= 0.0 or float(v_ego) < float(min_speed): return False if float(lead_prob) < RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB: return False diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8e6e02fbf..f304513cb 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -77,15 +77,16 @@ 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_HYSTERESIS_MIN = 2.5 -RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.10 +RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED = 12.0 +RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0 +RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14 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 NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF = 1.5 NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF = 0.35 -NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MIN = 0.75 -NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MAX = 1.5 +NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MIN = 1.25 +NEAR_DUPLICATE_LEAD_SOURCE_HYSTERESIS_MAX = 2.25 # Function to get parameter value based on current speed def get_speed_based_param(speed_mph, param_array): @@ -584,6 +585,7 @@ class LongitudinalMpc: radar=bool(getattr(lead, "radar", False)), lead_brake=max(0.0, -float(getattr(lead, "aLeadK", 0.0))), lead_prob=float(getattr(lead, "modelProb", 0.0)), + min_speed=RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED, ): return 0.0 diff --git a/selfdrive/controls/tests/test_conditional_chill_mode.py b/selfdrive/controls/tests/test_conditional_chill_mode.py index 5508ccbb1..3388e870b 100644 --- a/selfdrive/controls/tests/test_conditional_chill_mode.py +++ b/selfdrive/controls/tests/test_conditional_chill_mode.py @@ -54,6 +54,7 @@ def make_sm(): "selfdriveState": SimpleNamespace(enabled=True), "longitudinalPlan": SimpleNamespace(allowThrottle=True, shouldStop=False), "starpilotCarState": SimpleNamespace(trafficModeEnabled=False), + "starpilotPlan": SimpleNamespace(redLight=False, forcingStop=False), "starpilotRadarState": SimpleNamespace( leadLeft=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), leadRight=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), @@ -91,6 +92,7 @@ def make_ccm(): status=False, dRel=float("inf"), vLead=0.0, + vRel=0.0, aLeadK=0.0, modelProb=0.0, radar=False, @@ -269,6 +271,60 @@ def test_ccm_launch_assist_does_not_bypass_real_stop_scene(monkeypatch): assert ccm.status_value == CCStatus["OFF"] +def test_ccm_launch_assist_rejects_red_light_stationary_lead_even_if_planner_says_go(monkeypatch): + planner, _detector, ccm = make_ccm() + sm = make_sm() + sm["carState"].standstill = True + sm["starpilotPlan"].redLight = True + toggles = make_toggles() + toggles.conditional_chill_launch_assist = True + + planner.tracking_lead = True + planner.lead_one.status = True + planner.lead_one.dRel = 6.1 + planner.lead_one.vLead = 0.01 + planner.lead_one.vRel = -0.08 + planner.lead_one.aLeadK = 0.0 + planner.lead_one.radar = True + + monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 22.0) + ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) + + assert ccm.experimental_mode + assert ccm.status_value == CCStatus["OFF"] + + +def test_ccm_launch_assist_requires_real_lead_departure_signal(monkeypatch): + planner, _detector, ccm = make_ccm() + sm = make_sm() + sm["carState"].standstill = True + toggles = make_toggles() + toggles.conditional_chill_launch_assist = True + + planner.tracking_lead = True + planner.lead_one.status = True + planner.lead_one.dRel = 18.0 + planner.lead_one.vLead = 0.12 + planner.lead_one.vRel = 0.05 + planner.lead_one.aLeadK = 0.02 + planner.lead_one.radar = True + + monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.0) + ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) + + assert ccm.experimental_mode + assert ccm.status_value == CCStatus["OFF"] + + planner.lead_one.vLead = 0.8 + planner.lead_one.vRel = 0.5 + planner.lead_one.aLeadK = 0.2 + monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 23.2) + ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) + + assert not ccm.experimental_mode + assert ccm.status_value == CCStatus["LEAD"] + + def test_ccm_launch_assist_exits_once_launch_speed_is_reached(monkeypatch): planner, _detector, ccm = make_ccm() sm = make_sm() @@ -301,6 +357,7 @@ def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch): planner.lead_one.status = True planner.lead_one.dRel = 18.0 planner.lead_one.vLead = 2.0 + planner.lead_one.vRel = 2.0 planner.lead_one.radar = True ccm.update(0.0, 25 * CV.MPH_TO_MS, sm, toggles) @@ -309,6 +366,7 @@ def test_ccm_launch_assist_exits_immediately_if_lead_slows_again(monkeypatch): sm["carState"].standstill = False planner.lead_one.vLead = 0.2 + planner.lead_one.vRel = 0.0 planner.lead_one.aLeadK = -0.4 ccm.update(3 * CV.MPH_TO_MS, 25 * CV.MPH_TO_MS, sm, toggles) diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index e08cb046d..b4d9cfb97 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -86,3 +86,11 @@ def test_radarless_matched_follow_window_rejects_far_headway(): def test_radarless_matched_follow_window_rejects_low_confidence_lead(): assert not is_radarless_matched_follow_window(31.0, 48.0, 30.4, 1.45, radar=False, lead_brake=0.05, lead_prob=0.55) + + +def test_radarless_matched_follow_window_keeps_default_low_speed_guard(): + assert not is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0) + + +def test_radarless_matched_follow_window_accepts_lower_speed_when_requested(): + assert is_radarless_matched_follow_window(14.4, 25.4, 16.2, 1.25, radar=False, lead_brake=0.0, lead_prob=1.0, min_speed=12.0) diff --git a/starpilot/controls/lib/conditional_chill_mode.py b/starpilot/controls/lib/conditional_chill_mode.py index 19bf2436e..9d507ad10 100644 --- a/starpilot/controls/lib/conditional_chill_mode.py +++ b/starpilot/controls/lib/conditional_chill_mode.py @@ -22,6 +22,10 @@ class ConditionalChillMode: CHILL_LAUNCH_MAX_ENTRY_SPEED = 1.0 CHILL_LAUNCH_MAX_BRAKE = 0.2 CHILL_LAUNCH_MAX_CLOSING_SPEED = 0.75 + CHILL_LAUNCH_MIN_LEAD_SPEED = 0.35 + CHILL_LAUNCH_MIN_LEAD_DELTA = 0.2 + CHILL_LAUNCH_MIN_LEAD_VREL = 0.2 + CHILL_LAUNCH_MIN_LEAD_ACCEL = 0.08 STABLE_LEAD_MIN_MODEL_PROB = 0.9 STABLE_LEAD_MAX_BRAKE = 0.2 @@ -228,12 +232,25 @@ class ConditionalChillMode: selfdrive_state = self._get_sm_service(sm, "selfdriveState") longitudinal_plan = self._get_sm_service(sm, "longitudinalPlan") + starpilot_plan = self._get_sm_service(sm, "starpilotPlan") if selfdrive_state is None or longitudinal_plan is None: return False if not bool(getattr(selfdrive_state, "enabled", False)): return False + if ( + self.detector.stop_light_detected or + self.detector.stop_light_model_detected or + self.starpilot_planner.raw_model_stopped or + self.starpilot_planner.model_stopped or + self.starpilot_planner.starpilot_vcruise.stop_sign_confirmed or + self.starpilot_planner.starpilot_vcruise.forcing_stop or + bool(getattr(starpilot_plan, "redLight", False)) or + bool(getattr(starpilot_plan, "forcingStop", False)) + ): + return False + if bool(getattr(longitudinal_plan, "shouldStop", False)) or not bool(getattr(longitudinal_plan, "allowThrottle", False)): return False @@ -245,9 +262,19 @@ class ConditionalChillMode: lead_speed = float(getattr(lead, "vLead", 0.0)) lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0))) + lead_accel = float(getattr(lead, "aLeadK", 0.0)) + lead_delta = lead_speed - float(v_ego) + lead_vrel = float(getattr(lead, "vRel", lead_delta)) closing_speed = max(0.0, v_ego - lead_speed) - return lead_speed > self.STABLE_LEAD_MIN_SPEED and lead_brake <= self.CHILL_LAUNCH_MAX_BRAKE and closing_speed <= self.CHILL_LAUNCH_MAX_CLOSING_SPEED + return ( + lead_speed >= self.CHILL_LAUNCH_MIN_LEAD_SPEED and + lead_delta >= self.CHILL_LAUNCH_MIN_LEAD_DELTA and + lead_vrel >= self.CHILL_LAUNCH_MIN_LEAD_VREL and + lead_accel >= self.CHILL_LAUNCH_MIN_LEAD_ACCEL and + lead_brake <= self.CHILL_LAUNCH_MAX_BRAKE and + closing_speed <= self.CHILL_LAUNCH_MAX_CLOSING_SPEED + ) def _launch_exit_required(self, v_ego, sm): if v_ego >= self.CHILL_LAUNCH_EXIT_SPEED: