Steam Your Water

2026-09-29 02:43:47 +08:00 · 2026-06-19 14:14:07 -05:00
parent 0e5d40fb1b
commit b47c18426d
2 changed files with 163 additions and 0 deletions
@@ -219,6 +219,14 @@ POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL = 0.25
 POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN = 0.10
+POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME = 75.0
+POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED = 8.0
+POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB = 0.9
+POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET = 1.15
+POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED = 0.8
+POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE = 0.10
+POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN = 0.10
+POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN = 0.05
 COMFORTABLE_PULLAWAY_FOLLOW_MIN_MODEL_PROB = 0.95
 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_DELTA = -0.05
 COMFORTABLE_PULLAWAY_FOLLOW_MIN_LEAD_ACCEL = 0.20
@@ -582,6 +590,7 @@ class LongitudinalPlanner:
    self.manual_stop_resume_override_until = 0.0
    self.lead_depart_accel_hold_until = 0.0
    self.spacious_follow_cap_bypass_until = 0.0
+    self.post_departure_follow_settle_until = 0.0

    if self.is_preap:
      try:
@@ -1320,6 +1329,41 @@ class LongitudinalPlanner:
    headway_margin = actual_headway - float(t_follow)
    return headway_margin >= COMFORTABLE_PULLAWAY_FOLLOW_MIN_HEADWAY_MARGIN

+  def post_departure_follow_settle_active(self, lead, v_ego, t_follow):
+    if lead is None or not lead.status:
+      return False
+    if time.monotonic() > self.post_departure_follow_settle_until:
+      self.post_departure_follow_settle_until = 0.0
+      return False
+    if float(v_ego) < POST_DEPARTURE_FOLLOW_SETTLE_MIN_SPEED:
+      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 < POST_DEPARTURE_FOLLOW_SETTLE_MIN_MODEL_PROB:
+      return False
+
+    if abs(float(getattr(lead, "yRel", 0.0))) > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LATERAL_OFFSET:
+      return False
+
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    headway_margin = actual_headway - float(t_follow)
+    if headway_margin <= POST_DEPARTURE_FOLLOW_SETTLE_COMPLETE_HEADWAY_MARGIN:
+      self.post_departure_follow_settle_until = 0.0
+      return False
+
+    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
+    if lead_brake > POST_DEPARTURE_FOLLOW_SETTLE_MAX_LEAD_BRAKE:
+      self.post_departure_follow_settle_until = 0.0
+      return False
+
+    closing_speed = max(float(v_ego) - float(lead.vLead), 0.0)
+    if closing_speed > POST_DEPARTURE_FOLLOW_SETTLE_MAX_CLOSING_SPEED:
+      self.post_departure_follow_settle_until = 0.0
+      return False
+
+    return headway_margin >= POST_DEPARTURE_FOLLOW_SETTLE_MIN_HEADWAY_MARGIN
+
  def is_spacious_low_closure_follow(self, lead, v_ego, t_follow):
    if lead is None or not lead.status or float(v_ego) < CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED:
      return False
@@ -1356,6 +1400,9 @@ class LongitudinalPlanner:
    if lead is None or not lead.status:
      return None

+    if self.post_departure_follow_settle_active(lead, v_ego, t_follow):
+      return None
+
    lead_radar = bool(getattr(lead, "radar", False))
    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
    lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
@@ -1457,6 +1504,8 @@ class LongitudinalPlanner:
  def get_cruise_tracking_lead_accel_cap(self, lead, v_ego, t_follow, current_source, tracking_lead_active):
    if lead is None or not lead.status or current_source != "cruise":
      return None
+    if self.post_departure_follow_settle_active(lead, v_ego, t_follow):
+      return None
    if not (CRUISE_TRACKED_LEAD_ACCEL_CAP_MIN_SPEED <= float(v_ego) <= CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_SPEED):
      return None

@@ -2641,9 +2690,11 @@ class LongitudinalPlanner:
      vision_low_speed_stop_active = False
      output_should_stop = False
      output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
+      self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME

    if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:
      output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
+      self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME

    if depart_safety_veto or output_should_stop or bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) or bool(getattr(sm['starpilotPlan'], 'redLight', False)):
      self.lead_depart_accel_hold_until = 0.0
@@ -2487,6 +2487,118 @@ def test_route_8bc6_radar_matched_follow_catchup_cap_holds_small_cap_for_slower_
  assert cap == pytest.approx(0.04, abs=1e-6)


+def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_catchup_cap():
+  v_ego = 16.4023914337
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=25.75, v_lead=15.9339208603,
+    a_lead=0.1164954603, radar=True, model_prob=1.0, y_rel=0.0,
+  )
+
+  cap_without_latch = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="lead0",
+    tracking_lead_active=True,
+  )
+
+  planner.post_departure_follow_settle_until = time.monotonic() + 5.0
+  cap_with_latch = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="lead0",
+    tracking_lead_active=True,
+  )
+
+  assert cap_without_latch == pytest.approx(0.03, abs=1e-6)
+  assert cap_with_latch is None
+
+
+def test_route_8bc6_post_departure_settle_latch_bypasses_mild_closure_cruise_cap():
+  v_ego = 19.2975330353
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(
+    status=True, d_rel=28.9, v_lead=19.6019687653,
+    a_lead=0.1241656989, radar=True, model_prob=1.0, y_rel=0.0,
+  )
+
+  cap_without_latch = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  planner.post_departure_follow_settle_until = time.monotonic() + 5.0
+  cap_with_latch = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap_without_latch == pytest.approx(0.10831585854957321, abs=1e-6)
+  assert cap_with_latch is None
+
+
+def test_post_departure_settle_latch_does_not_bypass_when_lead_brakes_again():
+  v_ego = 19.192998192
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  planner.post_departure_follow_settle_until = time.monotonic() + 5.0
+  lead = make_lead(
+    status=True, d_rel=30.5, v_lead=19.147550216,
+    a_lead=-0.16, radar=True, model_prob=1.0, y_rel=0.2,
+  )
+
+  catchup_cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+  cruise_cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert catchup_cap is not None
+  assert cruise_cap is not None
+  assert planner.post_departure_follow_settle_until == 0.0
+
+
+def test_post_departure_settle_latch_clears_once_follow_has_settled_near_target():
+  v_ego = 16.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  planner.post_departure_follow_settle_until = time.monotonic() + 5.0
+  lead = make_lead(
+    status=True, d_rel=20.48, v_lead=16.25,
+    a_lead=0.18, radar=True, model_prob=1.0, y_rel=0.0,
+  )
+
+  cap = planner.get_lead_catchup_accel_cap(
+    lead,
+    v_ego,
+    1.25,
+    current_source="lead0",
+    tracking_lead_active=True,
+  )
+
+  assert cap is not None
+  assert planner.post_departure_follow_settle_until == 0.0
+
+
 def test_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again():
  v_ego = 19.03
  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)