diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8b149bdec..d1b1b5ee5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -67,12 +67,23 @@ CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS # wake at ~2.2 m/s and never showed it). Hold authority above creep speed is bounded # by the opposite-command release and the decay band, not by this ceiling. CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS -# Pre-wind stays a low-speed device: the plan-geometry source only feeds the ratchet -# below this speed; above it only the model's own action can raise the hold, and only -# at a limited rate so a single-frame action spike (left1 rlog 21.9s: -0.157 for one -# model frame) can't get captured and floored for seconds. -CURVATURE_HOLD_PLAN_SOURCE_SPEED = 6.0 * CV.MPH_TO_MS +# Pre-wind is a NEAR-STANDSTILL device: winding the wheel is only free when the car +# isn't moving. On rolling slow turns a plan-sourced floor applies the turn's final +# curvature at the entry, starting the arc 4-7 m early — the "turning too much" +# corrections in the stickyright1/2 and left-crossing rlogs (2026-07-16) all trace to +# plan capture while moving. Above this speed only the model's own action can raise +# the hold, rate-limited so a single-frame action spike (left1 rlog 21.9s: -0.157 for +# one model frame) can't get captured and floored for seconds. +CURVATURE_HOLD_PLAN_SOURCE_SPEED = 2.0 * CV.MPH_TO_MS CURVATURE_HOLD_RATCHET_RATE = 0.04 # 1/m per s, hold growth limit above the plan-source speed +# Once the model's action has sustainably taken over the turn, hand off COMPLETELY: +# clear the hold and don't re-engage until the blinker cycle ends. A floor that chases +# the awake action only distorts the model's entry spiral, mid-turn shape, and exit +# unwind — the sticky right-turn exits were the floor trailing the model's unwind by +# 0.03-0.05 even at the fast decay (stickyright1 41.15s: floor 0.064 vs action 0.014, +# driver correcting +394). The bridge job is done the moment the action is awake. +CURVATURE_HOLD_HANDOFF_FRAC = 0.75 +CURVATURE_HOLD_HANDOFF_TIME = 0.3 # s of sustained action >= frac*hold before handoff CURVATURE_HOLD_DECAY_TAU = 2.0 # s; hold tracks a sustained lower model demand with this time constant # At a turn exit the model unwinds through small SAME-sign commands (curvature only # flips negative for the final counter-steer), so the opposite-release fires late and @@ -191,6 +202,8 @@ class Controls: self.turn_hold_curvature = 0.0 self.turn_hold_standstill_t = 0.0 self.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_hold_done = False self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -322,6 +335,8 @@ class Controls: self.turn_hold_curvature = 0.0 self.turn_hold_standstill_t = 0.0 self.turn_hold_swept = 0.0 + self.turn_hold_handoff_t = 0.0 + self.turn_hold_done = False else: if self.turn_hold_curvature == 0.0: self.turn_hold_swept = 0.0 @@ -344,13 +359,29 @@ class Controls: self.turn_hold_standstill_t += DT_CTRL if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT: self.turn_hold_curvature = 0.0 + # a stop resets the turn cycle: the model goes blind again, so a prior handoff + # must not block the standstill pre-wind (turn4 regression in v9 replay) + self.turn_hold_done = False else: self.turn_hold_standstill_t = 0.0 if CC.latActive and self.turn_hold_curvature != 0.0 and \ new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) < -CURVATURE_HOLD_OPPOSITE_RELEASE: # model is actively counter-steering: the turn is over, release at any speed self.turn_hold_curvature = 0.0 - if blinker_dir != 0.0: + self.turn_hold_done = True + if CC.latActive and self.turn_hold_curvature != 0.0 and \ + new_desired_curvature * math.copysign(1.0, self.turn_hold_curvature) >= CURVATURE_HOLD_HANDOFF_FRAC * abs(self.turn_hold_curvature): + self.turn_hold_handoff_t += DT_CTRL + if self.turn_hold_handoff_t > CURVATURE_HOLD_HANDOFF_TIME: + # action has sustainably taken over: hand off completely (see HANDOFF consts) + self.turn_hold_curvature = 0.0 + self.turn_hold_done = True + else: + self.turn_hold_handoff_t = 0.0 + if blinker_dir == 0.0: + # blinker cycle over: a fresh turn may engage a fresh hold + self.turn_hold_done = False + if blinker_dir != 0.0 and not self.turn_hold_done: # Ratchet up on the raw model command, never on the floored/measured value, so # the hold can't feed itself and defeat the decay. Below the release speed the # plan's spatial curvature (see get_plan_spatial_curvature) is the second,