diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2c7cf30f4..2215105e8 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -53,20 +53,7 @@ LANE_CHANGE_ARREST_JERK_FLOOR = 0.6 # and noise-scale lag inside the deadband gets no boost (kills the fast-down/slow-up sawtooth). LANE_CHANGE_ARREST_PURSUIT_TAU = 0.2 # s LANE_CHANGE_ARREST_GAP_DEADBAND = 5e-5 # 1/m -# The pursuit engage test compares the model's step against a FIXED entry direction -# (latched once per maneuver), not the command's live sign: on a curve the arrest has to -# swing back through zero and past it to reach the new lane's own steady curve-following -# curvature, so by the time the deep arrest is happening the command has already crossed -# zero onto the same side the model is diving toward — a live-sign reference reads that as -# "still winding up" and never engages, leaving the command stranded ~4x further behind the -# model than on a straight road (curvelanechange rlog 2026-07-21: lag grew to -0.0013 1/m -# and didn't close until 0.8s after the state machine had already exited). A fixed -# direction has no such blind spot. Raw per-frame engagement then flickers on 20 Hz model -# noise once the command is moving fast enough to overtake-and-oscillate around the model -# (jerk_factor sawtoothing 0.07-0.59 in the same rlog's tail) — LANE_CHANGE_ARREST_RISE_TAU -# smooths only the RISE of the applied jerk factor (never the fall, so genuine disengagement -# is instant) to absorb that flicker without dulling the pursuit's response to a real gap. -LANE_CHANGE_ARREST_RISE_TAU = 0.2 # s +LANE_CHANGE_ARREST_RISE_TAU = 0.2 # Low-speed turn-intent curvature hold. Approaching a turn with the blinker on, the # model's time-based plan collapses as the car slows to a stop: desiredCurvature decays @@ -610,9 +597,6 @@ class Controls: # lane center before it can build enough counter-curvature. if in_lane_change: self.lc_smooth_release = LANE_CHANGE_SMOOTH_RELEASE_T - # Latch the entry's direction once, on the first meaningful model step, and hold it - # for the whole maneuver — this is what "unwinding" is measured against, not the - # command's live sign (see LANE_CHANGE_ARREST_PURSUIT_TAU comment above). if self.lc_entry_sign == 0.0 and abs(new_desired_curvature - self.desired_curvature) > 2e-4: self.lc_entry_sign = math.copysign(1.0, new_desired_curvature - self.desired_curvature) else: @@ -622,12 +606,6 @@ class Controls: if self.lc_smooth_release > 0.0: release = 1.0 - self.lc_smooth_release / LANE_CHANGE_SMOOTH_RELEASE_T # 0 in maneuver → 1 after jerk_factor = set_jerk + (1.0 - set_jerk) * release - # When the model is unwinding curvature (its step opposes the latched entry - # direction) and the entry cap would make the command lag it, grant extra rate - # proportional to the lag so the car can stop on the new lane center — including a - # curve's own steady curvature, which the arrest must swing through zero to reach. - # Applies only to the unwind direction; the entry ramp keeps the full pace - # smoothness. Robust to double lane changes (entry sign re-latches per maneuver). step = new_desired_curvature - self.desired_curvature model_unwinding = self.lc_entry_sign != 0.0 and abs(step) > LANE_CHANGE_ARREST_GAP_DEADBAND and \ math.copysign(1.0, step) == -self.lc_entry_sign @@ -637,9 +615,6 @@ class Controls: jf_gap = (gap / LANE_CHANGE_ARREST_PURSUIT_TAU) * v_lim ** 2 / MAX_LATERAL_JERK arrest_cap = LANE_CHANGE_ARREST_JERK_FLOOR + (1.0 - LANE_CHANGE_ARREST_JERK_FLOOR) * release jerk_factor = max(jerk_factor, min(arrest_cap, jerk_factor + jf_gap)) - # Smooth only the RISE of the applied factor so pursuit re-engaging on 20 Hz model - # noise near the deadband can't flicker the command; a genuine drop (pursuit no - # longer needed) still takes effect immediately. if jerk_factor > self.lc_arrest_jerk_factor: rise_alpha = 1.0 - math.exp(-DT_CTRL / LANE_CHANGE_ARREST_RISE_TAU) jerk_factor = self.lc_arrest_jerk_factor + rise_alpha * (jerk_factor - self.lc_arrest_jerk_factor) diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 339374faa..4740fd35f 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -61,9 +61,6 @@ class DesireHelper: self.prev_one_blinker = False self.desire = log.Desire.none - # Suppress the turn desire while stopping for a light/sign so the model keeps its - # stop plan (a turn desire extends model_length and rolls the car past the stop - # line). Released once the car has actually stopped: "stop first, then turn." self.turn_stop_hold = False self.lane_change_completed = False @@ -243,9 +240,6 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < starpilot_toggles.minimum_lane_change_speed - # Hold the turn desire while a stop is in progress, release it once stopped. The - # plan message is a cycle behind the model, so its stop flags reflect the model's - # stop intent from before this frame's desire could inflate model_length. stop_imminent = (bool(getattr(starpilotPlan, "redLight", False)) or bool(getattr(starpilotPlan, "forcingStop", False)) or bool(getattr(starpilotPlan, "stopSignConfirmed", False))) diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index 39383d17c..e2101c4bf 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -473,7 +473,6 @@ def test_turn_desire_held_while_stopping_for_red_light(): make_toggles(use_turn_desires=True, minimum_lane_change_speed=10.0), ) - # A turn desire here would extend the model past the stop line, so it is withheld. assert helper.turn_stop_hold assert helper.desire == log.Desire.none @@ -482,15 +481,12 @@ def test_turn_desire_released_after_stop_completes(): helper = DesireHelper() toggles = make_toggles(use_turn_desires=True, minimum_lane_change_speed=10.0) - # Approaching the stop with the blinker on -> desire held. helper.update(make_car_state(vEgo=5.0, rightBlinker=True), True, 0.0, make_plan(redLight=True), toggles) assert helper.desire == log.Desire.none - # Car reaches the stop line -> hold clears (still standstill, so no desire yet). helper.update(make_car_state(vEgo=0.0, rightBlinker=True, standstill=True), True, 0.0, make_plan(redLight=True), toggles) assert not helper.turn_stop_hold - # Pulling away through the turn, blinker still on, stop cleared -> turn desire resumes. helper.update(make_car_state(vEgo=2.0, rightBlinker=True), True, 0.0, make_plan(), toggles) assert helper.desire == log.Desire.turnRight diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index b5ff8fd0e..78670af9f 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -369,7 +369,6 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens(): def test_force_stop_turn_scene_veto_blocks_new_activation(): - # No stop seen: a wound wheel is a turn instead of a stop -> veto still blocks. _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False) sm = make_sm(standstill=False) sm["carState"].leftBlinker = True @@ -383,7 +382,6 @@ def test_force_stop_turn_scene_veto_blocks_new_activation(): def test_force_stop_curve_veto_blocks_new_activation(): - # No stop seen: road curvature is a genuine curve -> curve veto still blocks. _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False, road_curvature=0.005) sm = make_sm(standstill=False) toggles = make_toggles() @@ -397,8 +395,6 @@ def test_force_stop_curve_veto_blocks_new_activation(): def test_force_stop_turn_scene_veto_yields_to_stop_then_turn(): - # Low Speed Turn Assist winds the wheel into the turn while approaching a red light. - # The wound wheel must NOT block Force Stop when the model saw a stop -> stop-then-turn. _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False) sm = make_sm(standstill=False) sm["carState"].leftBlinker = True @@ -414,7 +410,6 @@ def test_force_stop_turn_scene_veto_yields_to_stop_then_turn(): def test_force_stop_curve_veto_yields_to_stop_then_turn(): - # Path curvature bends into the turn on a stop-then-turn approach -> curve veto must yield. _, vcruise = make_vcruise(red_light=True, raw_model_stopped=True, forcing_stop=False, road_curvature=0.05) sm = make_sm(standstill=False) sm["carState"].rightBlinker = True @@ -430,8 +425,6 @@ def test_force_stop_curve_veto_yields_to_stop_then_turn(): def test_stop_then_turn_override_releases_after_stop_seen_window_expires(): - # Once the model stops seeing a stop and the hold window lapses, the veto resumes so a - # real mid-intersection turn isn't force-stopped. _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=False) planner = vcruise.starpilot_planner sm = make_sm(standstill=False) @@ -439,12 +432,10 @@ def test_stop_then_turn_override_releases_after_stop_seen_window_expires(): sm["carState"].steeringAngleDeg = 30.0 toggles = make_toggles() - # Stop seen briefly on approach (seeds the stop_then_turn latch), then it disappears. planner.starpilot_cem.stop_light_detected = True update_vcruise(vcruise, sm, toggles, now=0.0, v_ego=7.0) planner.starpilot_cem.stop_light_detected = False - # Past the hold window with no stop -> veto active again, no new activation. now = FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME + 0.5 for frame in range(12): result = update_vcruise(vcruise, sm, toggles, now=now + frame * 0.05, v_ego=7.0) diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 56a6f45e2..de73c8eb6 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -50,12 +50,6 @@ FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS # for *new* activation — an in-progress stop is carried through (see force_stop_timer logic). FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003 -# Low Speed Turn Assist pre-winds the wheel into an upcoming turn while the car is still -# creeping to a stop, which crosses the veto angle before Force Stop can commit — so the -# veto blocks the stop it was meant to carry through. When the model recently saw a stop -# on approach, treat the wound wheel as a stop-then-turn (not turn-instead-of-stop) and -# suppress the veto for this long after the stop was last seen, letting Force Stop arm and -# latch; its committed hold + tracked_model_length then bridge the model dropout to the line. FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0 # Knob bounds (mirror of UI slider; defense in depth) @@ -152,8 +146,6 @@ class StarPilotVCruise: self.tracked_model_length = 0.0 self.stop_sign_confirmed = False - # Time a model/dash stop was last seen while approaching (moving). Suppresses the - # turn veto so LSTA's pre-wound wheel doesn't block a stop-then-turn. self.stop_seen_on_approach_at = None self.nav_turn_target = 0.0 self._nav_instruction_state_raw = None @@ -277,9 +269,6 @@ class StarPilotVCruise: long_control_active = sm["carControl"].longActive - # Track a model/dash stop seen while still moving (the approach). LSTA winds the wheel - # past the veto angle during this window; without this the veto zeroes the force-stop - # timer before it can commit, so the "carry an in-progress stop through" latch never arms. raw_stop_seen = bool( self.starpilot_planner.starpilot_cem.stop_light_detected or getattr(self.starpilot_planner, "raw_model_stopped", False) @@ -288,7 +277,6 @@ class StarPilotVCruise: if raw_stop_seen and not sm["carState"].standstill: self.stop_seen_on_approach_at = now elif sm["carState"].standstill: - # Stop reached: any wound wheel now is the turn itself, so let the veto resume. self.stop_seen_on_approach_at = None stop_then_turn = ( self.stop_seen_on_approach_at is not None @@ -310,10 +298,6 @@ class StarPilotVCruise: lead_present = (bool(getattr(lead, "status", False)) and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0) - # The road/path curvature bends toward the turn during a stop-then-turn approach, which - # trips this curve veto (meant for genuinely curvy roads) — same false block as the turn - # veto. When the model recently saw a stop here, the curvature is the turn we're stopping - # before, so let Force Stop arm through it. Left blinker-scoped via stop_then_turn. curved_approach_scene = ( abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE and not stop_then_turn