diff --git a/selfdrive/controls/tests/test_conditional_chill_mode.py b/selfdrive/controls/tests/test_conditional_chill_mode.py index 3388e870b..720309a48 100644 --- a/selfdrive/controls/tests/test_conditional_chill_mode.py +++ b/selfdrive/controls/tests/test_conditional_chill_mode.py @@ -211,6 +211,40 @@ def test_ccm_hard_vetoes_force_experimental(monkeypatch): assert ccm.status_value == CCStatus["OFF"] +def test_ccm_far_lateral_adjacent_lead_does_not_block_open_road_chill(monkeypatch): + planner, _detector, ccm = make_ccm() + sm = make_sm() + toggles = make_toggles() + monotonic_values = iter([10.0, 10.5]) + monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: next(monotonic_values)) + + sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=6.5) + + v_ego = 55 * CV.MPH_TO_MS + v_cruise = v_ego + 5 * CV.MPH_TO_MS + ccm.update(v_ego, v_cruise, sm, toggles) + ccm.update(v_ego, v_cruise, sm, toggles) + + assert not ccm.experimental_mode + assert ccm.status_value == CCStatus["SPEED"] + + +def test_ccm_immediate_adjacent_lead_still_blocks_open_road_chill(monkeypatch): + planner, _detector, ccm = make_ccm() + sm = make_sm() + toggles = make_toggles() + + monkeypatch.setattr("openpilot.starpilot.controls.lib.conditional_chill_mode.time.monotonic", lambda: 10.0) + sm["starpilotRadarState"].leadLeft = SimpleNamespace(status=True, dRel=18.0, vLead=12.0, yRel=4.5) + + v_ego = 55 * CV.MPH_TO_MS + v_cruise = v_ego + 5 * CV.MPH_TO_MS + ccm.update(v_ego, v_cruise, sm, toggles) + + assert ccm.experimental_mode + assert ccm.status_value == CCStatus["OFF"] + + def test_ccm_immediately_exits_chill_when_scene_turns_into_slow_lead(monkeypatch): planner, detector, ccm = make_ccm() sm = make_sm() diff --git a/starpilot/controls/lib/conditional_chill_mode.py b/starpilot/controls/lib/conditional_chill_mode.py index 9d507ad10..7f8144201 100644 --- a/starpilot/controls/lib/conditional_chill_mode.py +++ b/starpilot/controls/lib/conditional_chill_mode.py @@ -38,6 +38,7 @@ class ConditionalChillMode: ADJACENT_LEAD_VETO_MIN_SPEED = 1.0 ADJACENT_LEAD_VETO_MAX_DISTANCE = 65.0 ADJACENT_LEAD_VETO_MAX_DISTANCE_TIME = 3.5 + ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET = 5.5 LOW_SPEED_STOP_SCENE_MAX_SPEED = 18 * CV.MPH_TO_MS @@ -310,6 +311,10 @@ class ConditionalChillMode: if lead is None or not getattr(lead, "status", False): continue + lateral_offset = abs(float(getattr(lead, "yRel", 0.0))) + if lateral_offset > self.ADJACENT_LEAD_VETO_MAX_LATERAL_OFFSET: + continue + if float(getattr(lead, "dRel", float("inf"))) < max_distance and float(getattr(lead, "vLead", 0.0)) > self.ADJACENT_LEAD_VETO_MIN_SPEED: return True