From d8587bba317139230679d4c307ed5c3ab6fddeed Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 22 Jun 2026 16:55:49 -0500 Subject: [PATCH] longy long long --- selfdrive/controls/lib/longcontrol.py | 5 ++ .../controls/lib/longitudinal_planner.py | 61 +++++++++++++++++-- selfdrive/controls/tests/test_longcontrol.py | 31 +++++++++- .../tests/test_longitudinal_planner.py | 51 ++++++++++++++++ .../controls/tests/test_starpilot_vcruise.py | 36 ++++++++++- starpilot/controls/lib/starpilot_vcruise.py | 23 +++++-- 6 files changed, 194 insertions(+), 13 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 54e707d63..c530789fe 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -13,6 +13,7 @@ clip = np.clip interp = np.interp STOPPING_RELEASE_HYSTERESIS = 0.35 STOPPING_RELEASE_MIN_ACCEL = 0.15 +STOPPING_RELEASE_STRONG_ACCEL = 0.45 MOVING_STOP_FOLLOW_MIN_GAP = 0.25 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 @@ -184,6 +185,10 @@ class LongControl: self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) return True + if a_target >= STOPPING_RELEASE_STRONG_ACCEL and not CS.cruiseState.standstill: + self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) + return True + if a_target > STOPPING_RELEASE_MIN_ACCEL: max_frames = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL)) self.stop_release_counter = min(self.stop_release_counter + 1, max_frames) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 72fd6abce..036b5ff25 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -46,6 +46,11 @@ STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5 STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6 STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8 STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL = 0.18 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.12 +STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 1.4 +STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30 LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75 LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3 @@ -562,6 +567,7 @@ class LongitudinalPlanner: self.output_a_target = 0.0 self.output_should_stop = False self.confident_lead_depart_elapsed = 0.0 + self.slow_creep_lead_depart_elapsed = 0.0 self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) @@ -1096,6 +1102,28 @@ class LongitudinalPlanner: lead_accel >= LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL ) + def is_slow_creep_lead_depart(self, lead, v_ego, standstill_nudge_gap): + if lead is None or not lead.status: + 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 < STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB: + return False + + if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET: + return False + + lead_gap = float(getattr(lead, "dRel", 0.0)) + lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0) + lead_accel = float(getattr(lead, "aLeadK", 0.0)) + return bool( + float(v_ego) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED and + lead_gap >= standstill_nudge_gap + STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN and + lead_speed >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED and + lead_accel >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL + ) + @staticmethod def get_centered_model_lead(model_data): try: @@ -2646,11 +2674,33 @@ class LongitudinalPlanner: confident_depart_detected and self.confident_lead_depart_elapsed >= LEAD_DEPART_CONFIDENT_CONFIRM_TIME ) + slow_creep_depart_detected = any( + self.is_slow_creep_lead_depart(lead, float(sm['carState'].vEgo), standstill_nudge_gap) + for lead in (self.lead_one, self.lead_two) + ) + if ( + lead_control_active and + sm['carState'].standstill and + not depart_safety_veto and + not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and + not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and + slow_creep_depart_detected + ): + self.slow_creep_lead_depart_elapsed = min( + STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME, + self.slow_creep_lead_depart_elapsed + self.dt, + ) + else: + self.slow_creep_lead_depart_elapsed = 0.0 + slow_creep_depart_ready = ( + slow_creep_depart_detected and + self.slow_creep_lead_depart_elapsed >= STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME + ) standstill_stopped_lead_guard_cap = None standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two)) if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED): - release_ready = bool(lead_depart_ready or confident_depart_ready) + release_ready = bool(lead_depart_ready or confident_depart_ready or slow_creep_depart_ready) standstill_stopped_lead_guard_caps = [ cap for cap in ( self.get_standstill_stopped_lead_guard_cap( @@ -2681,15 +2731,18 @@ class LongitudinalPlanner: if ( lead_control_active and sm['carState'].standstill and - (confident_depart_ready or lead_depart_ready) and + (confident_depart_ready or lead_depart_ready or slow_creep_depart_ready) and not depart_safety_veto and not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and - (confident_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL) + (confident_depart_ready or slow_creep_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL) ): vision_low_speed_stop_active = False output_should_stop = False - output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL) + depart_min_accel = STANDSTILL_LEAD_DEPART_MIN_ACCEL + if slow_creep_depart_ready and not (confident_depart_ready or lead_depart_ready): + depart_min_accel = STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL + output_a_target = max(output_a_target, 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: diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 95fb39c0e..c52277f98 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -151,7 +151,7 @@ def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled(): assert output_accel == 0.1 -def test_update_requires_sustained_positive_target_to_leave_stopping(): +def test_update_requires_sustained_moderate_positive_target_to_leave_stopping(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] CP.longitudinalTuning.kpV = [0.1] @@ -168,7 +168,7 @@ def test_update_requires_sustained_positive_target_to_leave_stopping(): output_accel = lc.update( active=True, CS=CS, - a_target=0.5, + a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01, should_stop=False, accel_limits=(-3.0, 2.0), starpilot_toggles=make_toggles(startAccel=1.5), @@ -179,7 +179,7 @@ def test_update_requires_sustained_positive_target_to_leave_stopping(): lc.update( active=True, CS=CS, - a_target=0.5, + a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01, should_stop=False, accel_limits=(-3.0, 2.0), starpilot_toggles=make_toggles(startAccel=1.5), @@ -188,6 +188,31 @@ def test_update_requires_sustained_positive_target_to_leave_stopping(): assert lc.long_control_state == LongCtrlState.starting +def test_update_releases_stopping_immediately_on_strong_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 + + output_accel = lc.update( + active=True, + CS=CS, + a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel > 0.0 + + 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] diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 38f440f7e..09902e30c 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1222,6 +1222,57 @@ def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(mod assert planner.output_a_target <= 0.0 +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_slow_creep_depart_releases_after_short_confirm(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.33, a_lead=0.28, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + for _ in range(max(frames, 1)): + planner.update(sm, make_toggles(model_version)) + + assert not planner.output_should_stop + assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL + + +@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) +def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_signal(model_version): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=0.0) + + sm = make_sm( + 0.0, + desired_accel=0.0, + min_accel=-0.5, + experimental_mode=False, + tracking_lead=True, + lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.20, a_lead=0.05, radar=True, model_prob=1.0), + ) + sm["carState"].standstill = True + sm["controlsState"].longControlState = LongCtrlState.stopping + sm["starpilotPlan"].vCruise = 10.0 + sm["modelV2"].action.shouldStop = False + + frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + 2 + for _ in range(max(frames, 1)): + planner.update(sm, make_toggles(model_version)) + + assert planner.output_should_stop + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version): v_ego = 0.0 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index cfd24c1d0..df366af2d 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -44,7 +44,15 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False def make_sm(*, standstill=True, min_steer_speed=0.0): return { "carControl": SimpleNamespace(longActive=True), - "carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0), + "carState": SimpleNamespace( + standstill=standstill, + gasPressed=False, + vCruiseCluster=0.0, + vEgoCluster=0.0, + leftBlinker=False, + rightBlinker=False, + steeringAngleDeg=0.0, + ), "carParams": SimpleNamespace(minSteerSpeed=min_steer_speed), "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), } @@ -155,6 +163,32 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens(): assert vcruise.forcing_stop +def test_force_stop_turn_scene_veto_blocks_new_activation(): + _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) + sm = make_sm(standstill=False) + sm["carState"].leftBlinker = True + sm["carState"].steeringAngleDeg = 15.0 + + result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=7.0) + + assert result == pytest.approx(20.0) + assert vcruise.force_stop_timer == pytest.approx(0.0) + assert not vcruise.forcing_stop + + +def test_force_stop_turn_scene_clears_moving_commitment(): + _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + sm = make_sm(standstill=False) + sm["carState"].rightBlinker = True + sm["carState"].steeringAngleDeg = -15.0 + + result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=8.0) + + assert result == pytest.approx(20.0) + assert vcruise.force_stop_timer == pytest.approx(0.0) + assert not vcruise.forcing_stop + + def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold(): _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index a1a6c0737..a344d29d1 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -25,15 +25,18 @@ NAV_TURN_TARGET_SPEEDS = { } # Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset, -# in feet); positive = stop later/longer, negative = stop sooner/shorter. All other -# shape parameters are fixed constants converged from FP-Testing Sessions A-O. -COMFORT_DECEL = 1.0 # m/s^2 — kinematic decel ceiling +# in feet); positive = stop later/longer, negative = stop sooner/shorter. +# Smaller values pull speed down earlier on approach. +FORCE_STOP_MODEL_APPROACH_DECEL = 0.8 +FORCE_STOP_DASH_APPROACH_DECEL = 1.0 ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap # tracked length closer when dashboard confirms a sign FT_TO_M = 0.3048 +FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS +FORCE_STOP_TURN_VETO_STEERING_ANGLE = 12.0 # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -190,6 +193,11 @@ class StarPilotVCruise: def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles): long_control_active = sm["carControl"].longActive + turn_scene_active = bool( + v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED and + (getattr(sm["carState"], "leftBlinker", False) or getattr(sm["carState"], "rightBlinker", False)) and + abs(float(getattr(sm["carState"], "steeringAngleDeg", 0.0))) >= FORCE_STOP_TURN_VETO_STEERING_ANGLE + ) # ----- Activation paths ----- # Raw lead check: block Force Stop as soon as a relevant lead is present, without @@ -209,6 +217,7 @@ class StarPilotVCruise: and self.starpilot_planner.model_length < ACTIVATION_M and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve + and not turn_scene_active and not self.starpilot_planner.tracking_lead and not lead_present) @@ -220,6 +229,7 @@ class StarPilotVCruise: and v_ego < ADAS_MAX_MS and self.override_force_stop_timer <= 0 and not self.starpilot_planner.driving_in_curve + and not turn_scene_active and not self.starpilot_planner.tracking_lead and not lead_present) @@ -275,6 +285,8 @@ class StarPilotVCruise: if force_stop_active and not sm["carState"].standstill: rate = DT_MDL * 2 if dash_active else DT_MDL self.force_stop_timer = min(self.force_stop_timer + rate, 2.0) + elif turn_scene_active and not sm["carState"].standstill: + self.force_stop_timer = 0.0 elif self.standstill_force_stop_hold: self.force_stop_timer = max(self.force_stop_timer, 0.5) elif (self.forcing_stop and sm["carState"].standstill and not dash_active and @@ -285,7 +297,7 @@ class StarPilotVCruise: force_stop_enabled = self.force_stop_timer >= 0.5 # Stay committed across model dropouts until standstill - force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill + force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill and not turn_scene_active force_stop_enabled |= self.standstill_force_stop_hold # Override: gas/accel pedal during an active force stop @@ -379,7 +391,8 @@ class StarPilotVCruise: if effective_d <= MPC_HANDOFF_M: v_target = 0.0 else: - v_target = math.sqrt(2.0 * COMFORT_DECEL * (effective_d - MPC_HANDOFF_M)) + approach_decel = FORCE_STOP_DASH_APPROACH_DECEL if dash_active else FORCE_STOP_MODEL_APPROACH_DECEL + v_target = math.sqrt(2.0 * approach_decel * (effective_d - MPC_HANDOFF_M)) v_cruise = min(v_target, v_cruise)