Mr. Freeze

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()

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