test4

selfdrive/controls/tests/test_curve_speed_controller.py

@@ -13,6 +13,7 @@ from openpilot.starpilot.controls.lib.curve_speed_controller import (
   CSC_LAT_ACCEL_MAX,
   CSC_MIN_SPEED,
   CSC_NUDGE_WEIGHT,
+  CSC_TARGET_UP_RATE,
   CSC_TRAINING_SETTLE_TIME,
   CurveSpeedController,
   weighted_isotonic,
@@ -114,12 +115,36 @@ def test_exit_recovery_rises_immediately_without_freeze():
 
   planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
   controller.update_target(15.0, 30.0)
-  assert controller.target == pytest.approx(15.0 + CSC_EGO_HEADROOM)
+  assert controller.target > low_target          # rises on the very next frame, no freeze
+  assert controller.target - low_target == pytest.approx(CSC_TARGET_UP_RATE * DT_MDL)
+
+  # and it clears the car by the headroom within the time the up-rate needs
+  frames = int((15.0 + CSC_EGO_HEADROOM - controller.target) / (CSC_TARGET_UP_RATE * DT_MDL)) + 1
+  for _ in range(frames):
+    controller.update_target(15.0, 30.0)
+  assert controller.target >= 15.0 + CSC_EGO_HEADROOM
 
   recovered = converge(controller, 15.0, 30.0)
   assert recovered == pytest.approx(30.0)
 
 
+def test_upward_jitter_in_the_envelope_is_rate_limited():
+  # a sweeper the envelope only grazes: raw_target flicks between a mild cap and the
+  # set speed. The target must not chase the jumps, or the glow strobes.
+  planner, controller = make_controller(curve_profile=single_apex_profile(0.002, 40.0))
+  steady = converge(controller, 30.0, 32.0)
+  assert steady < 32.0
+
+  flat = (np.zeros(33), np.linspace(0.0, 300.0, 33))
+  grazing = planner.curve_profile
+  peak = steady
+  for i in range(40):
+    planner.curve_profile = flat if i % 2 else grazing
+    controller.update_target(30.0, 32.0)
+    assert controller.target - peak <= CSC_TARGET_UP_RATE * DT_MDL + 1e-6
+    peak = controller.target
+
+
 def test_fresh_activation_seeds_at_envelope_not_cruise():
   _, controller = make_controller(curve_profile=(np.full(33, 0.05), np.linspace(0.0, 60.0, 33)))
 
@@ -134,10 +159,17 @@ def test_target_never_trails_accelerating_car_when_unconstrained():
 
   planner.curve_profile = (np.zeros(33), np.linspace(0.0, 300.0, 33))
   v_ego = 15.0
-  for _ in range(100):
+  caught_up = None
+  for frame in range(200):
     v_ego = min(v_ego + 2.0 * DT_MDL, 30.0)
     controller.update_target(v_ego, 30.0)
-    assert controller.target >= min(30.0, v_ego + CSC_EGO_HEADROOM) - 1e-6
+    # the target climbs faster than the car can, so once it is ahead it stays ahead
+    if controller.target >= v_ego:
+      caught_up = caught_up if caught_up is not None else frame
+    assert caught_up is None or controller.target >= min(30.0, v_ego) - 1e-6
+
+  assert caught_up is not None and caught_up * DT_MDL < 2.0
+  assert controller.target == pytest.approx(30.0)
 
 
 def test_target_does_not_ratchet_down_with_ego_speed():

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -128,7 +128,39 @@ def test_camry_tss2_gets_forward_force_stop_bias_only():
   assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0)
 
 
-def test_curve_speed_controller_blinker_resets_target():
+def test_curve_speed_controller_blinker_releases_the_cap_but_keeps_the_plan():
+  planner, vcruise = make_vcruise()
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+  toggles.curve_speed_controller = True
+
+  calls = []
+
+  def set_curve_target(_v_ego, _v_cruise):
+    calls.append(_v_ego)
+    vcruise.csc.target = 14.0
+
+  vcruise.csc.update_target = set_curve_target
+  result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+  # the cap lifts so CSC can't fight the lane change, but the envelope keeps planning
+  # so the curve doesn't have to be re-discovered from the set speed afterwards
+  sm["carState"].leftBlinker = True
+  result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0)
+  assert result == pytest.approx(20.0)
+  assert not vcruise.csc_controlling_speed
+  assert len(calls) == 2  # still planning, so nothing has to be rediscovered
+
+  # blinker off: the plan is already current, so the cap comes straight back
+  sm["carState"].leftBlinker = False
+  result = update_vcruise(vcruise, sm, toggles, now=10.5, v_ego=20.0)
+  assert result == pytest.approx(14.0)
+  assert vcruise.csc_controlling_speed
+
+
+def test_curve_speed_controller_reseeds_after_a_real_dropout():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)
   toggles = make_toggles()
@@ -138,14 +170,14 @@ def test_curve_speed_controller_blinker_resets_target():
     vcruise.csc.target = 14.0
 
   vcruise.csc.update_target = set_curve_target
-  result = update_vcruise(vcruise, sm, toggles, now=10.0, v_ego=20.0)
-  assert result == pytest.approx(14.0)
+  update_vcruise(vcruise, sm, toggles, now=11.0, v_ego=20.0)
   assert vcruise.csc_controlling_speed
 
-  sm["carState"].leftBlinker = True
-  result = update_vcruise(vcruise, sm, toggles, now=10.25, v_ego=20.0)
-  assert result == pytest.approx(20.0)
+  # disengaging is a real dropout, not a momentary veto -- that still resets
+  sm["carControl"].longActive = False
+  update_vcruise(vcruise, sm, toggles, now=11.05, v_ego=20.0)
   assert not vcruise.csc_controlling_speed
+  assert vcruise.csc.seed_pending
 
 
 def test_curve_speed_controller_releases_immediately_when_disabled():

starpilot/controls/lib/curve_speed_controller.py

@@ -24,6 +24,7 @@ CSC_TARGET_UP_RATE = 3.0
 CSC_TARGET_DOWN_RATE = 2.5
 CSC_TARGET_FILTER_RC = 0.4
 CSC_EGO_HEADROOM = 2.0            # target never trails below v_ego, so CSC can't drag re-acceleration
+CSC_RELEASE_DEBOUNCE = 0.25       # s the envelope must stay clear before that floor applies
 CSC_ACTIVE_ON_DELTA = 0.5
 CSC_ACTIVE_OFF_DELTA = 0.25
 
@@ -119,6 +120,7 @@ class CurveSpeedController:
 
     self.target = 0.0
     self.binding_distance = 0.0
+    self.release_timer = 0.0
     self.target_filter = FirstOrderFilter(0.0, CSC_TARGET_FILTER_RC, DT_MDL, initialized=False)
     self.seed_pending = True
 
@@ -334,6 +336,7 @@ class CurveSpeedController:
 
   def reset(self, v_cruise):
     self.target = float(v_cruise)
+    self.release_timer = 0.0
     self.target_filter.x = float(v_cruise)
     self.target_filter.initialized = True
     self.seed_pending = True
@@ -363,11 +366,23 @@ class CurveSpeedController:
       self.target_filter.x = seed
       self.seed_pending = False
 
+    if raw_target >= v_ego:
+      self.release_timer += DT_MDL
+    else:
+      self.release_timer = 0.0
+
+    # The headroom aim goes through the rate limiter with everything else; applying it
+    # after the clamp let every upward jitter in raw_target reach the target unsmoothed.
     filtered = self.target_filter.update(raw_target)
-    self.target = float(np.clip(filtered,
+    self.target = float(np.clip(max(filtered, min(raw_target, v_ego + CSC_EGO_HEADROOM)),
                                 self.target - CSC_TARGET_DOWN_RATE * DT_MDL,
                                 self.target + CSC_TARGET_UP_RATE * DT_MDL))
-    self.target = max(self.target, min(raw_target, v_ego + CSC_EGO_HEADROOM))
+
+    # Once the envelope really has released, the target must not sit under the car or it
+    # drags re-acceleration. Debounced, because a single jittery frame doing this yanks a
+    # legitimate cut back up to v_ego and strobes the glow on sweepers.
+    if self.release_timer >= CSC_RELEASE_DEBOUNCE:
+      self.target = max(self.target, min(raw_target, v_ego))
 
     if self.target < v_cruise - CSC_ACTIVE_ON_DELTA:
       self.training_quiet_timer = CSC_TRAINING_QUIET_TIME

starpilot/controls/lib/starpilot_vcruise.py

@@ -575,6 +575,13 @@ class StarPilotVCruise:
           self.csc_controlling_speed = True
         elif self.csc_target > v_cruise - CSC_ACTIVE_OFF_DELTA:
           self.csc_controlling_speed = False
+    elif csc_available:
+      # Blinker: release the cap so CSC can't fight a lane change, but keep planning.
+      # Resetting here threw the braking plan away, so the restart re-planned from the
+      # set speed with the curve much closer -- which arrived as a panic stop.
+      self.csc.update_target(v_ego, v_cruise)
+      self.csc_controlling_speed = False
+      self.csc_target = v_cruise
     else:
       self.csc.reset(v_cruise)
       self.csc_controlling_speed = False