Analytical Techniques

scripts/speed_limit_vision/build_review_classifier_dataset.py

@@ -20,6 +20,19 @@ def parse_args() -> argparse.Namespace:
   parser.add_argument("--output", type=Path, required=True, help="New isolated dataset root.")
   parser.add_argument("--positive-manifest", type=Path, action="append", default=[], help="Reviewed positive crop manifest. Repeat as needed.")
   parser.add_argument("--reject-manifest", type=Path, action="append", default=[], help="Reviewed classifier reject manifest. Repeat as needed.")
+  parser.add_argument(
+    "--exclude-base-record-key",
+    action="append",
+    default=[],
+    help="Remove inherited samples whose staged filename contains this corrected record key. Repeat as needed.",
+  )
+  parser.add_argument(
+    "--repeat-reject-record",
+    action="append",
+    default=[],
+    metavar="RECORD_KEY=COUNT",
+    help="Stage a reviewed reject COUNT times to give a corrected hard negative more training weight.",
+  )
   parser.add_argument(
     "--advisory-as-reject",
     action="store_true",
@@ -76,12 +89,45 @@ def keep_advisory_reject(row: dict[str, str], fraction: float) -> bool:
   return int.from_bytes(digest[:8], "big") / 2**64 < fraction
 
 
+def safe_record_key(record_key: str) -> str:
+  return "".join(char if char.isalnum() or char in "._-" else "_" for char in record_key)[:100]
+
+
+def remove_inherited_records(root: Path, record_keys: list[str]) -> int:
+  safe_keys = tuple(filter(None, (safe_record_key(record_key) for record_key in record_keys)))
+  if not safe_keys:
+    return 0
+  removed = 0
+  for split in ("train", "val"):
+    for path in (root / split).rglob("*"):
+      if path.is_file() and any(record_key in path.name for record_key in safe_keys):
+        path.unlink()
+        removed += 1
+  return removed
+
+
+def parse_reject_repeat_counts(specs: list[str]) -> dict[str, int]:
+  repeat_counts: dict[str, int] = {}
+  for spec in specs:
+    record_key, separator, count_text = spec.rpartition("=")
+    if not separator or not record_key:
+      raise ValueError(f"Invalid --repeat-reject-record value: {spec!r}")
+    try:
+      count = int(count_text)
+    except ValueError as exc:
+      raise ValueError(f"Invalid reject repeat count: {spec!r}") from exc
+    if count < 1:
+      raise ValueError(f"Reject repeat count must be at least 1: {spec!r}")
+    repeat_counts[record_key] = count
+  return repeat_counts
+
+
 def stage_crop(source: Path, destination_dir: Path, record_key: str) -> bool:
   if not source.is_file():
     return False
   digest = hashlib.sha256(source.read_bytes()).hexdigest()[:16]
   suffix = source.suffix.lower() if source.suffix.lower() in (".jpg", ".jpeg", ".png") else ".jpg"
-  safe_key = "".join(char if char.isalnum() or char in "._-" else "_" for char in record_key)[:100]
+  safe_key = safe_record_key(record_key)
   destination_dir.mkdir(parents=True, exist_ok=True)
   destination = destination_dir / f"review_{safe_key}_{digest}{suffix}"
   if not destination.exists():
@@ -103,6 +149,8 @@ def main() -> int:
     raise FileExistsError(f"Output dataset already exists: {output}")
   shutil.copytree(base, output, copy_function=shutil.copyfile)
   appledouble_removed = remove_appledouble_files(output)
+  inherited_records_removed = remove_inherited_records(output, args.exclude_base_record_key)
+  reject_repeat_counts = parse_reject_repeat_counts(args.repeat_reject_record)
 
   positive_counts: Counter[str] = Counter()
   reject_counts: Counter[str] = Counter()
@@ -132,10 +180,16 @@ def main() -> int:
   for row in read_rows(args.reject_manifest):
     split = row.get("split", "")
     source = Path(row.get("crop_path", "")).expanduser()
-    if split not in ("train", "val") or not stage_crop(source, output / split / "reject", row.get("record_key", "reject")):
+    record_key = row.get("record_key", "reject")
+    repeat_count = reject_repeat_counts.get(record_key, 1) if split == "train" else 1
+    staged = split in ("train", "val")
+    for repeat_index in range(repeat_count):
+      staged_key = record_key if repeat_index == 0 else f"{record_key}_repeat_{repeat_index:03d}"
+      staged = staged and stage_crop(source, output / split / "reject", staged_key)
+    if not staged:
       skipped += 1
       continue
-    reject_counts[split] += 1
+    reject_counts[split] += repeat_count
 
   appledouble_removed += remove_appledouble_files(output)
   for split in ("train", "val"):
@@ -150,6 +204,7 @@ def main() -> int:
     "reject_counts": dict(sorted(reject_counts.items())),
     "skipped": skipped,
     "appledouble_removed": appledouble_removed,
+    "inherited_records_removed": inherited_records_removed,
   }
   summary_path = output / "review_dataset_summary.json"
   summary_path.write_text(json.dumps(summary, indent=2, sort_keys=True) + "\n", encoding="utf-8")

scripts/speed_limit_vision/import_manual_review_queue.py

@@ -305,6 +305,58 @@ def classifier_reject_row(row: dict[str, str], split: str) -> dict[str, object]:
   }
 
 
+RUNTIME_REJECT_CROP_EXPANSIONS = (
+  (0.00, 0.00, 0.00, 0.00),
+  (0.10, 0.06, 0.10, 0.12),
+  (0.00, 0.00, 0.18, 0.18),
+)
+
+
+def classifier_reject_variant_rows(
+  row: dict[str, str],
+  split: str,
+  output_dir: Path,
+  overwrite: bool,
+) -> list[dict[str, object]]:
+  rows = [classifier_reject_row(row, split)]
+  if row.get("review_ignore_reason") != "conditional_restriction":
+    return rows
+
+  frame_path = Path(row.get("frame_path", "")).expanduser()
+  frame = cv2.imread(str(frame_path))
+  bbox = parse_bbox(row.get("review_bbox") or row.get("bbox", ""))
+  if frame is None or bbox is None:
+    raise RuntimeError(f"Cannot generate conditional reject crops for {row['record_key']}: unreadable frame or bbox")
+
+  image_h, image_w = frame.shape[:2]
+  x1, y1, x2, y2 = bbox
+  box_width = x2 - x1
+  box_height = y2 - y1
+  reject_dir = output_dir / "corrected_classifier_reject_crops"
+  ensure_dir(reject_dir)
+  for index, (expand_left, expand_top, expand_right, expand_bottom) in enumerate(RUNTIME_REJECT_CROP_EXPANSIONS):
+    crop_bbox = (
+      max(int(x1 - box_width * expand_left), 0),
+      max(int(y1 - box_height * expand_top), 0),
+      min(int(x2 + box_width * expand_right), image_w),
+      min(int(y2 + box_height * expand_bottom), image_h),
+    )
+    crop_x1, crop_y1, crop_x2, crop_y2 = crop_bbox
+    crop = frame[crop_y1:crop_y2, crop_x1:crop_x2]
+    crop_path = reject_dir / f"{safe_stem(row['record_key'])}_runtime_expansion_{index}.jpg"
+    if crop.size == 0:
+      raise RuntimeError(f"Cannot generate conditional reject crop for {row['record_key']}: empty bbox {crop_bbox}")
+    if overwrite or not crop_path.is_file():
+      if not cv2.imwrite(str(crop_path), crop, [cv2.IMWRITE_JPEG_QUALITY, 94]):
+        raise RuntimeError(f"Cannot write conditional reject crop for {row['record_key']}: {crop_path}")
+    variant = classifier_reject_row(row, split)
+    variant["record_key"] = f"{row['record_key']}_runtime_expansion_{index}"
+    variant["crop_path"] = str(crop_path)
+    variant["crop_bbox"] = ",".join(str(value) for value in crop_bbox)
+    rows.append(variant)
+  return rows
+
+
 def corrected_classifier_crop(
   row: dict[str, str],
   output_dir: Path,
@@ -508,7 +560,7 @@ def main() -> int:
 
   for row in classifier_reject_rows:
     split = split_for_key(split_group_key(row), args.val_modulo, args.val_remainder)
-    reject_rows.append(classifier_reject_row(row, split))
+    reject_rows.extend(classifier_reject_variant_rows(row, split, output_dir, args.overwrite))
 
   write_csv(classifier_manifest, CLASSIFIER_FIELDNAMES, classifier_rows)
   write_csv(runtime_manifest, RUNTIME_FIELDNAMES, runtime_rows)

scripts/speed_limit_vision/test_review_pipeline.py

@@ -235,3 +235,53 @@ def test_corrected_bbox_requires_readable_source_frame(tmp_path):
 
   with pytest.raises(RuntimeError, match="unreadable frame"):
     import_queue.corrected_classifier_crop(row, tmp_path, overwrite=False)
+
+
+def test_corrected_record_removes_inherited_classifier_sample(tmp_path):
+  stale = tmp_path / "train" / "55" / "base_review_bad_record_key_hash.jpg"
+  retained = tmp_path / "train" / "55" / "base_review_other_record_hash.jpg"
+  stale.parent.mkdir(parents=True)
+  stale.write_bytes(b"stale")
+  retained.write_bytes(b"retained")
+
+  removed = build_review_classifier.remove_inherited_records(tmp_path, ["bad:record/key"])
+
+  assert removed == 1
+  assert not stale.exists()
+  assert retained.exists()
+
+
+def test_reject_repeat_spec_preserves_record_key_punctuation():
+  counts = build_review_classifier.parse_reject_repeat_counts(["route/sign=track:55=32"])
+
+  assert counts == {"route/sign=track:55": 32}
+
+  with pytest.raises(ValueError, match="at least 1"):
+    build_review_classifier.parse_reject_repeat_counts(["bad-record=0"])
+
+
+def test_conditional_reject_generates_runtime_crop_expansions(tmp_path):
+  import cv2
+  import numpy as np
+
+  frame_path = tmp_path / "frame.jpg"
+  crop_path = tmp_path / "crop.jpg"
+  frame = np.zeros((100, 200, 3), dtype=np.uint8)
+  cv2.imwrite(str(frame_path), frame)
+  cv2.imwrite(str(crop_path), frame[20:80, 60:140])
+  row = {
+    "record_key": "conditional-sign",
+    "frame_path": str(frame_path),
+    "crop_path": str(crop_path),
+    "bbox": "60,20,140,80",
+    "review_bbox": "60,20,140,80",
+    "review_ignore_reason": "conditional_restriction",
+  }
+
+  rows = import_queue.classifier_reject_variant_rows(row, "train", tmp_path, overwrite=False)
+
+  assert len(rows) == 4
+  assert rows[1]["crop_bbox"] == "60,20,140,80"
+  assert rows[2]["crop_bbox"] == "52,16,148,87"
+  assert rows[3]["crop_bbox"] == "60,20,154,90"
+  assert all(Path(variant["crop_path"]).is_file() for variant in rows)

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -89,6 +89,8 @@ STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_REL_SPEED = 2.0
 STABLE_FOLLOW_CRUISE_PULLAWAY_MAX_HEADWAY_MARGIN = 0.35
 STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN = -0.10
 STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX = 1.75
+VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB = 0.95
+VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 2.0
 NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0
 NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED = 10.0
 NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9
@@ -747,6 +749,27 @@ class LongitudinalMpc:
 
     return prev_source
 
+  def get_vision_follow_cruise_hold(self, prev_source, lead_one, lead_two,
+                                    lead_0_obstacle, lead_1_obstacle, cruise_obstacle,
+                                    v_ego, t_follow, tracking_lead):
+    if not tracking_lead or prev_source not in ("lead0", "lead1"):
+      return None
+
+    prev_lead = lead_one if prev_source == "lead0" else lead_two
+    if prev_lead is None or not prev_lead.status or bool(getattr(prev_lead, "radar", False)):
+      return None
+    if float(getattr(prev_lead, "modelProb", 0.0)) < VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB:
+      return None
+    if self.get_stable_follow_cruise_hysteresis(prev_lead, v_ego, t_follow) <= 0.0:
+      return None
+
+    prev_lead_obstacle = float(lead_0_obstacle if prev_source == "lead0" else lead_1_obstacle)
+    cruise_advantage = prev_lead_obstacle - float(cruise_obstacle)
+    if cruise_advantage > VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE:
+      return None
+
+    return prev_source
+
   def get_identical_radar_duplicate_cruise_bias(self, lead_one, lead_two, v_ego, t_follow):
     if float(v_ego) < IDENTICAL_RADAR_DUPLICATE_CRUISE_BIAS_MIN_SPEED:
       return 0.0
@@ -862,6 +885,18 @@ class LongitudinalMpc:
             v_ego,
             t_follow,
           )
+          if sticky_source is None:
+            sticky_source = self.get_vision_follow_cruise_hold(
+              prev_source,
+              lead_one,
+              lead_two,
+              lead_0_obstacle[0],
+              lead_1_obstacle[0],
+              cruise_obstacle[0],
+              v_ego,
+              t_follow,
+              tracking_lead,
+            )
       self.source = sticky_source or candidate_source
 
       # These are not used in ACC mode

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -504,6 +504,87 @@ def test_post_stop_slow_lead_trigger_is_suppressed_after_red_light_release(monke
   assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"]
 
 
+def test_slow_lead_mode_release_waits_for_stable_credible_lead(monkeypatch):
+  v_ego = 20.0
+  cem = make_cem(
+    model_length=100.0,
+    tracking_lead=True,
+    lead_status=True,
+    lead_d_rel=34.0,
+    lead_v_lead=18.5,
+    lead_model_prob=0.99,
+  )
+  toggles = make_update_toggles()
+  toggles.conditional_lead = True
+  toggles.conditional_slower_lead = True
+  sm = make_update_sm(standstill=False)
+  slow_lead_active = [True]
+  now = [100.0]
+
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
+
+  def update_conditions(*args, **kwargs):
+    cem.slow_lead_detected = slow_lead_active[0]
+
+  monkeypatch.setattr(cem, "update_conditions", update_conditions)
+
+  cem.update(v_ego, sm, toggles)
+  assert cem.experimental_mode
+  assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"]
+
+  slow_lead_active[0] = False
+  now[0] = 100.9
+  cem.update(v_ego, sm, toggles)
+  assert cem.experimental_mode
+  assert cem.status_value == conditional_experimental_mode_module.CEStatus["LEAD"]
+
+  now[0] = 101.6
+  cem.update(v_ego, sm, toggles)
+  assert not cem.experimental_mode
+  assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"]
+
+
+def test_slow_lead_mode_release_does_not_hold_missing_lead(monkeypatch):
+  v_ego = 20.0
+  cem = make_cem(
+    model_length=100.0,
+    tracking_lead=True,
+    lead_status=True,
+    lead_d_rel=34.0,
+    lead_v_lead=18.5,
+    lead_model_prob=0.99,
+  )
+  toggles = make_update_toggles()
+  toggles.conditional_lead = True
+  toggles.conditional_slower_lead = True
+  sm = make_update_sm(standstill=False)
+  slow_lead_active = [True]
+  now = [200.0]
+
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: now[0])
+
+  def update_conditions(*args, **kwargs):
+    cem.slow_lead_detected = slow_lead_active[0]
+
+  monkeypatch.setattr(cem, "update_conditions", update_conditions)
+
+  cem.update(v_ego, sm, toggles)
+  assert cem.experimental_mode
+
+  slow_lead_active[0] = False
+  cem.starpilot_planner.lead_one.status = False
+  cem.starpilot_planner.tracking_lead = False
+  now[0] = 200.6
+  cem.update(v_ego, sm, toggles)
+  assert cem.experimental_mode
+
+  now[0] = 200.9
+  cem.update(v_ego, sm, toggles)
+
+  assert not cem.experimental_mode
+  assert cem.params_memory.get_int("CEStatus") == conditional_experimental_mode_module.CEStatus["OFF"]
+
+
 def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch):
   cem = make_cem(model_length=80.0, model_stopped=False)
   toggles = make_update_toggles()

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -3092,6 +3092,87 @@ def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead():
   assert hysteresis == 0.0
 
 
+def test_vision_follow_cruise_hold_keeps_high_confidence_matched_lead_through_small_crossover():
+  v_ego = 22.5
+  t_follow = 1.20
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=False, model_prob=0.99)
+  lead_two = make_lead(status=False)
+
+  sticky = planner.mpc.get_vision_follow_cruise_hold(
+    "lead0",
+    lead_one,
+    lead_two,
+    101.0,
+    200.0,
+    100.0,
+    v_ego,
+    t_follow,
+    True,
+  )
+
+  assert sticky == "lead0"
+
+
+@pytest.mark.parametrize("tracking_lead, radar, model_prob, cruise_obstacle", [
+  (False, False, 0.99, 100.0),
+  (True, True, 1.0, 100.0),
+  (True, False, 0.80, 100.0),
+  (True, False, 0.99, 98.0),
+])
+def test_vision_follow_cruise_hold_skips_nonmatching_or_clear_cruise_cases(
+  tracking_lead, radar, model_prob, cruise_obstacle,
+):
+  v_ego = 22.5
+  t_follow = 1.20
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=-0.02, radar=radar, model_prob=model_prob)
+  lead_two = make_lead(status=False)
+
+  sticky = planner.mpc.get_vision_follow_cruise_hold(
+    "lead0",
+    lead_one,
+    lead_two,
+    101.0,
+    200.0,
+    cruise_obstacle,
+    v_ego,
+    t_follow,
+    tracking_lead,
+  )
+
+  assert sticky is None
+
+
+@pytest.mark.parametrize("prev_source, lead_accel", [
+  ("cruise", -0.02),
+  ("lead0", -0.60),
+])
+def test_vision_follow_cruise_hold_never_delays_restrictive_transition(prev_source, lead_accel):
+  v_ego = 22.5
+  t_follow = 1.20
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=32.0, v_lead=22.0, a_lead=lead_accel, radar=False, model_prob=0.99)
+  lead_two = make_lead(status=False)
+
+  sticky = planner.mpc.get_vision_follow_cruise_hold(
+    prev_source,
+    lead_one,
+    lead_two,
+    101.0,
+    200.0,
+    100.0,
+    v_ego,
+    t_follow,
+    True,
+  )
+
+  assert sticky is None
+
+
 def test_near_duplicate_lead_source_hysteresis_prefers_previous_source_for_identical_radar_track():
   v_ego = 27.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -100,6 +100,54 @@ def test_active_slc_control_target_does_not_require_set_speed_limit():
   assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4)
 
 
+def test_curve_speed_controller_holds_target_through_brief_detector_dropout():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  planner.road_curvature_detected = True
+  result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  planner.road_curvature_detected = False
+  result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  result = update_vcruise(vcruise, sm, toggles, now=10.8, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_releases_immediately_when_disabled():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  def set_curve_target(_v_ego):
+    vcruise.csc.target_set = True
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  planner.road_curvature_detected = True
+  update_vcruise(vcruise, sm, toggles, now=20.0, v_ego=20.0)
+  assert vcruise.csc_controlling_speed
+
+  planner.road_curvature_detected = False
+  toggles.curve_speed_controller = False
+  result = update_vcruise(vcruise, sm, toggles, now=20.1, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+
+
 def test_active_slc_control_target_applies_offset_and_cluster_diff():
   target = get_active_slc_control_target(
     speed_limit_controller=True,
@@ -286,7 +334,7 @@ def test_force_stop_turn_scene_veto_blocks_new_activation():
   _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
   sm = make_sm(standstill=False)
   sm["carState"].leftBlinker = True
-  sm["carState"].steeringAngleDeg = 15.0
+  sm["carState"].steeringAngleDeg = 30.0
 
   result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=7.0)
 
@@ -321,17 +369,17 @@ def test_force_stop_still_activates_for_straight_red_light_approach():
   assert vcruise.forcing_stop
 
 
-def test_force_stop_turn_scene_clears_moving_commitment():
+def test_force_stop_turn_scene_does_not_abandon_moving_commitment():
   _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
   sm = make_sm(standstill=False)
   sm["carState"].rightBlinker = True
-  sm["carState"].steeringAngleDeg = -15.0
+  sm["carState"].steeringAngleDeg = -30.0
 
   result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=8.0)
 
-  assert result == pytest.approx(20.0)
-  assert vcruise.force_stop_timer == pytest.approx(0.0)
-  assert not vcruise.forcing_stop
+  assert result == pytest.approx(0.0)
+  assert vcruise.force_stop_timer >= 0.5
+  assert vcruise.forcing_stop
 
 
 def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold():

starpilot/controls/lib/conditional_experimental_mode.py

@@ -55,6 +55,7 @@ class ConditionalExperimentalMode:
   SLOW_LEAD_CONTINUITY_MIN_EGO = 2.5
   SLOW_LEAD_CONTINUITY_HOLD_TIME = 1.25
   SLOW_LEAD_FORCE_CLEAR_TIME = 0.75
+  SLOW_LEAD_MODE_RELEASE_HOLD_TIME = 1.5
   SLOW_LEAD_MIN_CLOSING_SPEED = 0.75
   SLOW_LEAD_CLEAR_FASTER_FACTOR = 0.5
   POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME = 2.0
@@ -104,6 +105,7 @@ class ConditionalExperimentalMode:
     self.prev_experimental_mode = False  # For hysteresis
     self.mode_hold_until = 0.0
     self.mode_false_since = 0.0
+    self.slow_lead_mode_hold_until = 0.0
     self._prev_ce_status = None
     self.prev_standstill = False
     self.prev_standstill_stop_hold = False
@@ -119,6 +121,7 @@ class ConditionalExperimentalMode:
       self.post_stop_launch_trigger_suppress_until = now + self.POST_STOP_LAUNCH_TRIGGER_SUPPRESS_TIME
       self.mode_hold_until = 0.0
       self.mode_false_since = 0.0
+      self.slow_lead_mode_hold_until = 0.0
       self.prev_experimental_mode = False
 
     if not standstill:
@@ -133,6 +136,10 @@ class ConditionalExperimentalMode:
       if triggered:
         self.mode_hold_until = now + self.CEM_TRANSITION_GUARD_TIME
         self.mode_false_since = 0.0
+        if self.status_value == CEStatus["LEAD"]:
+          self.slow_lead_mode_hold_until = now + self.SLOW_LEAD_MODE_RELEASE_HOLD_TIME
+        else:
+          self.slow_lead_mode_hold_until = 0.0
       elif self.prev_experimental_mode and self.mode_false_since == 0.0:
         self.mode_false_since = now
       elif not self.prev_experimental_mode:
@@ -140,8 +147,17 @@ class ConditionalExperimentalMode:
 
       hold_active = now < self.mode_hold_until
       transition_buffer_active = self.mode_false_since != 0.0 and (now - self.mode_false_since) < self.CEM_TRANSITION_BUFFER_TIME
+      slow_lead_hold_active = bool(
+        starpilot_toggles.conditional_lead and
+        now < self.slow_lead_mode_hold_until and
+        self.has_credible_slow_lead_context(v_ego)
+      )
+      if slow_lead_hold_active and not triggered:
+        self.status_value = CEStatus["LEAD"]
+      elif not slow_lead_hold_active:
+        self.slow_lead_mode_hold_until = 0.0
 
-      self.experimental_mode = triggered or hold_active or transition_buffer_active
+      self.experimental_mode = triggered or slow_lead_hold_active or hold_active or transition_buffer_active
       self.prev_experimental_mode = self.experimental_mode
       ce_write_value = self.status_value if self.experimental_mode else CEStatus["OFF"]
       if ce_write_value != self._prev_ce_status:
@@ -150,6 +166,7 @@ class ConditionalExperimentalMode:
     elif not is_manual_ce_status(self.status_value):
       self.mode_hold_until = 0.0
       self.mode_false_since = 0.0
+      self.slow_lead_mode_hold_until = 0.0
 
       # Keep the stop-light path live at standstill so EXP stays pinned for a red
       # light / stop sign. Stop signs latch until pedal, while stop lights can
@@ -168,6 +185,7 @@ class ConditionalExperimentalMode:
     else:
       self.mode_hold_until = 0.0
       self.mode_false_since = 0.0
+      self.slow_lead_mode_hold_until = 0.0
       self._prev_ce_status = None
       self.experimental_mode = self.status_value == CEStatus["USER_OVERRIDDEN"]
       self.prev_experimental_mode = self.experimental_mode
@@ -177,6 +195,19 @@ class ConditionalExperimentalMode:
     self.prev_standstill = standstill
     self.prev_standstill_stop_hold = current_standstill_stop_hold
 
+  def has_credible_slow_lead_context(self, v_ego):
+    lead = self.starpilot_planner.lead_one
+    if lead is None or not bool(getattr(lead, "status", False)):
+      return False
+
+    lead_radar = bool(getattr(lead, "radar", False))
+    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+    if not lead_radar and lead_prob < self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB:
+      return False
+
+    lead_distance = float(getattr(lead, "dRel", float("inf")))
+    return lead_distance < max(40.0, float(v_ego) * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME)
+
   def get_standstill_stop_hold(self, sm):
     dash_stop_sign = (
       bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or

starpilot/controls/lib/starpilot_vcruise.py

@@ -10,6 +10,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedCo
 from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController
 
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
+CSC_CURVE_RELEASE_HOLD_TIME = 0.75
 OVERRIDE_FORCE_STOP_TIMER = 10
 STANDSTILL_FORCE_STOP_CLEAR_TIME = 0.75
 STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME = 5.0
@@ -148,6 +149,9 @@ class StarPilotVCruise:
     self._nav_instruction_state_raw = None
     self._nav_instruction_state = {}
     self._applied_slc_control_target = 0.0
+    self.csc_controlling_speed = False
+    self.csc_target = 0.0
+    self.csc_curve_last_seen_at = None
 
   def _update_nav_instruction_state(self):
     raw = self.starpilot_planner.params_memory.get("NavInstructionState") or {}
@@ -399,19 +403,29 @@ class StarPilotVCruise:
     v_ego_diff = v_ego_cluster - v_ego
 
     # FrogsGoMoo's Curve Speed Controller
-    if long_control_active and v_ego > CRUISING_SPEED and self.starpilot_planner.road_curvature_detected and starpilot_toggles.curve_speed_controller:
+    csc_available = long_control_active and v_ego > CRUISING_SPEED and starpilot_toggles.curve_speed_controller
+    csc_curve_detected = csc_available and self.starpilot_planner.road_curvature_detected
+    if csc_curve_detected:
       self.csc.update_target(v_ego)
 
       self.csc_controlling_speed = True
-
       self.csc_target = self.csc.target
+      self.csc_curve_last_seen_at = now
     else:
-      self.csc.log_data(v_ego, sm)
+      csc_release_hold = bool(
+        csc_available and
+        self.csc_controlling_speed and
+        self.csc_curve_last_seen_at is not None and
+        self._elapsed_seconds(now, self.csc_curve_last_seen_at) < CSC_CURVE_RELEASE_HOLD_TIME
+      )
+      if not csc_release_hold:
+        self.csc.log_data(v_ego, sm)
 
-      self.csc_controlling_speed = False
-      self.csc.target_set = False
+        self.csc_controlling_speed = False
+        self.csc.target_set = False
+        self.csc_curve_last_seen_at = None
 
-      self.csc_target = v_cruise
+        self.csc_target = v_cruise
 
     # Pfeiferj's Speed Limit Controller
     self.slc.starpilot_toggles = starpilot_toggles