fixy

selfdrive/controls/lib/longitudinal_planner.py

@@ -102,8 +102,10 @@ VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0
 VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35
 VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 6.5
 VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 3.25
+VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED = 4.0
 VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9
 VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35
+VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED = -0.2
 VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.8
 VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80
 VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25
@@ -646,23 +648,32 @@ class LongitudinalPlanner:
       return None, False
 
     lead_speed = max(float(lead.vLead), 0.0)
+    relative_speed = float(v_ego) - lead_speed
     closing_speed = max(0.0, v_ego - lead_speed)
-    valid_context = (
+    entry_context = (
       v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and
       lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and
       closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED
     )
+    hold_context = (
+      v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and
+      lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED and
+      relative_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED
+    )
 
     now_t = time.monotonic()
     entry_buffer = max(3.2, VISION_LOW_SPEED_STOP_BUFFER_BASE +
                        VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN * float(v_ego) +
                        VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN * lead_speed)
     release_buffer = entry_buffer + VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN
-    if valid_context and float(lead.dRel) <= entry_buffer:
+    if entry_context and float(lead.dRel) <= entry_buffer:
       self.vision_low_speed_stop_hold_until = now_t + VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME
 
     latched = now_t < self.vision_low_speed_stop_hold_until
-    active = valid_context and (float(lead.dRel) <= entry_buffer or (latched and float(lead.dRel) <= release_buffer))
+    active = bool(
+      (entry_context and float(lead.dRel) <= entry_buffer) or
+      (latched and hold_context and float(lead.dRel) <= release_buffer)
+    )
     if not active:
       return None, False
 

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -8,6 +8,7 @@ import pytest
 from cereal import log
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
+import openpilot.selfdrive.controls.lib.longitudinal_planner as longitudinal_planner_module
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, get_coast_accel, get_vehicle_min_accel
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import soften_far_radar_lead_accel, should_trigger_planner_fcw
@@ -781,6 +782,25 @@ def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_visi
   assert planner.output_a_target <= -2.7
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_softens_near_stop(model_version, monkeypatch):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.55)
+  lead = make_lead(status=True, d_rel=2.1, v_lead=0.05, a_lead=-0.02, radar=False, model_prob=0.99)
+
+  monotonic_values = iter([10.0, 10.1])
+  monkeypatch.setattr(longitudinal_planner_module.time, "monotonic", lambda: next(monotonic_values))
+
+  cap_armed, active_armed = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.55, -2.0)
+  cap_held, active_held = planner.get_vision_low_speed_stop_buffer_cap(lead, 0.34, -2.0)
+
+  assert active_armed
+  assert cap_armed is not None
+  assert active_held
+  assert cap_held is not None
+  assert cap_held <= -1.25
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
 def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version):
   v_ego = 0.0