First time in San Juan mi hijo

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

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
 

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)
 

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)

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: