From 87b9fd5b574c00acd6fd047547be460ababe59c8 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 18 Aug 2026 15:11:37 -0500 Subject: [PATCH] test4 --- .../tests/test_curve_speed_controller.py | 38 ++++++++++++++-- .../controls/tests/test_starpilot_vcruise.py | 44 ++++++++++++++++--- .../controls/lib/curve_speed_controller.py | 19 +++++++- starpilot/controls/lib/starpilot_vcruise.py | 7 +++ 4 files changed, 97 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/tests/test_curve_speed_controller.py b/selfdrive/controls/tests/test_curve_speed_controller.py index 39ee0f9f9..cf1b0ab35 100644 --- a/selfdrive/controls/tests/test_curve_speed_controller.py +++ b/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(): diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 99f82a719..47092e935 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/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(): diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index 734e92933..eb31447ad 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/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 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 67fae8c59..9c102ffe5 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/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