long bugs

selfdrive/controls/lib/longcontrol.py

@@ -12,7 +12,7 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 clip = np.clip
 interp = np.interp
 STOPPING_RELEASE_HYSTERESIS = 0.35
-STOPPING_RELEASE_MIN_ACCEL = 0.2
+STOPPING_RELEASE_MIN_ACCEL = 0.15
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -181,6 +181,7 @@ def test_stop_light_hold_bridges_short_no_lead_model_flicker(monkeypatch):
 
   run_stop_light_detector(cem, v_ego, steps=20)
   assert cem.stop_light_detected
+  cem.stop_light_detected_hold_until = 11.75
 
   monotonic_values = iter([10.0, 11.0, 14.5, 16.5, 18.5])
   monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
@@ -235,6 +236,73 @@ def test_stop_light_hold_refreshes_through_stopped_approach_lead(monkeypatch):
   assert not cem.stop_light_detected
 
 
+def test_stop_light_approach_latch_clears_once_tracked_lead_takes_over(monkeypatch):
+  v_ego = 20 * CV.MPH_TO_MS
+  model_length = v_ego * 4.0
+  cem = make_cem(
+    model_length=model_length,
+    lead_status=True,
+    lead_d_rel=model_length - 5.0,
+    lead_v_lead=0.5,
+    lead_model_prob=0.98,
+  )
+
+  run_stop_light_detector(cem, v_ego, steps=20)
+  assert cem.stop_light_detected
+
+  monotonic_values = iter([20.0, 20.2])
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
+
+  cem.starpilot_planner.model_length = v_ego * 9.0
+  cem.stop_light_detected = False
+  cem.stop_light_model_detected = False
+  cem.stop_light_filter.x = 0.0
+
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+  assert cem.stop_light_detected
+
+  cem.starpilot_planner.tracking_lead = True
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+  assert not cem.stop_light_detected
+
+
+def test_stopped_lead_handoff_does_not_hold_cem_on_empty_road(monkeypatch):
+  v_ego = 40 * CV.MPH_TO_MS
+  cem = make_cem(
+    model_length=v_ego * 9.0,
+    tracking_lead=False,
+    lead_status=False,
+  )
+  cem.stop_light_detected = True
+  cem.stop_light_model_detected = False
+  cem.stop_light_filter.x = 0.0
+  cem.stop_approach_hold_until = 11.0
+  cem.stop_light_detected_hold_until = 0.0
+
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: 10.2)
+
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+
+  assert not cem.stop_light_detected
+
+
+def test_borderline_empty_road_model_dip_does_not_refresh_long_hold(monkeypatch):
+  v_ego = 20 * CV.MPH_TO_MS
+  stop_threshold = v_ego * 7.0
+  cem = make_cem(model_length=stop_threshold - 4.0)
+  cem.stop_light_filter.x = conditional_experimental_mode_module.THRESHOLD ** 2
+
+  monotonic_values = iter([10.0, 10.2])
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
+
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+  assert cem.stop_light_detected_hold_until == 0.0
+
+  cem.starpilot_planner.model_length = stop_threshold + 20.0
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+  assert not cem.stop_light_detected
+
+
 def test_standstill_red_light_keeps_exp_on_even_when_model_stopped_clears(monkeypatch):
   cem = make_cem(model_length=80.0, model_stopped=False)
   toggles = make_update_toggles()

selfdrive/controls/tests/test_longcontrol.py

@@ -188,6 +188,43 @@ def test_update_requires_sustained_positive_target_to_leave_stopping():
   assert lc.long_control_state == LongCtrlState.starting
 
 
+def test_update_releases_stopping_on_small_sustained_positive_target():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
+  for _ in range(release_frames - 1):
+    output_accel = lc.update(
+      active=True,
+      CS=CS,
+      a_target=0.16,
+      should_stop=False,
+      accel_limits=(-3.0, 2.0),
+      starpilot_toggles=make_toggles(startAccel=1.5),
+    )
+    assert lc.long_control_state == LongCtrlState.stopping
+    assert output_accel <= 0.0
+
+  lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.16,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(startAccel=1.5),
+  )
+
+  assert lc.long_control_state == LongCtrlState.starting
+
+
 def test_update_releases_stopping_with_cruise_standstill_latched():
   CP = car.CarParams.new_message(vEgoStarting=0.5)
   CP.longitudinalTuning.kpBP = [0.0]

starpilot/controls/lib/conditional_experimental_mode.py

@@ -43,6 +43,7 @@ class ConditionalExperimentalMode:
   LEAD_CLEAR_FILTER_TIME_HIGH = 0.35
   STOP_LIGHT_ON_MARGIN = 2.5
   STOP_LIGHT_OFF_MARGIN = 4.0
+  STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN = 10.0
   STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0
   STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0
   STOP_LIGHT_DETECTED_HOLD_TIME = 1.75
@@ -365,6 +366,7 @@ class ConditionalExperimentalMode:
       lead_speed = float(getattr(lead, "vLead", float("inf")))
       lead_radar = bool(getattr(lead, "radar", False))
       lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+      tracking_lead = bool(self.starpilot_planner.tracking_lead)
       lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN
       vision_stop_approach = (
         lead_relevant and
@@ -373,12 +375,13 @@ class ConditionalExperimentalMode:
         lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED
       )
       stop_approach_hold_active = now < self.stop_approach_hold_until
-      if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and vision_stop_approach:
+      trackable_stop_approach = vision_stop_approach and not tracking_lead
+      if (self.stop_light_detected or self.stop_light_model_detected or stop_approach_hold_active) and trackable_stop_approach:
         self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME
-      stop_approach_latched = now < self.stop_approach_hold_until and vision_stop_approach
+      stop_approach_latched = now < self.stop_approach_hold_until and trackable_stop_approach
       handoff_to_stopped_lead = (
         lead_relevant and
-        not self.starpilot_planner.tracking_lead and
+        not tracking_lead and
         (
           (self.stop_light_detected and lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED) or
           stop_approach_latched
@@ -390,13 +393,16 @@ class ConditionalExperimentalMode:
         self.lead_clear_filter.update(not lead_relevant)
         lead_cleared = self.lead_clear_filter.x >= THRESHOLD
       self.stop_light_filter.update(model_stopping and lead_cleared)
-      detector_active = bool(
-        (self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead or stop_approach_latched
+      model_detector_active = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared)
+      detector_active = bool(model_detector_active or handoff_to_stopped_lead or stop_approach_latched)
+      model_hold_qualifies = bool(
+        self.starpilot_planner.model_stopped or
+        self.starpilot_planner.model_length < max(stop_threshold - self.STOP_LIGHT_MODEL_HOLD_STRONG_MARGIN, 0.0)
       )
-      if detector_active:
+      if model_detector_active and model_hold_qualifies:
         self.stop_light_detected_hold_until = now + self.STOP_LIGHT_DETECTED_HOLD_TIME
 
-      hold_context_ok = bool(not lead_relevant or vision_stop_approach)
+      hold_context_ok = bool((not lead_relevant) or trackable_stop_approach)
       self.stop_light_detected = bool(
         detector_active or
         (hold_context_ok and now < self.stop_light_detected_hold_until)