cleanup

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)

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)))

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
 

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)

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