diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 19b2ee6da..8b149bdec 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -74,6 +74,17 @@ CURVATURE_HOLD_RELEASE_SPEED = 10.0 * CV.MPH_TO_MS CURVATURE_HOLD_PLAN_SOURCE_SPEED = 6.0 * CV.MPH_TO_MS CURVATURE_HOLD_RATCHET_RATE = 0.04 # 1/m per s, hold growth limit above the plan-source speed 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 +# the tau-2 decay melts the floor slower than the model's exit ramp — the car keeps +# arcing while the driver hauls the wheel back (tightright3/4 rlogs 2026-07-15, drv +# +500). Turn progress is the discriminator between a mid-turn dip (protect the floor; +# left-turn sags happened at ~20 deg of swept heading) and an exit (drop it; the +# sticks happened past ~80 deg): once the swept heading passes the threshold, the +# decay switches to the fast tau and runs at ANY speed, including below the hard-hold +# speed. Swept resets whenever the hold disengages. +CURVATURE_HOLD_SWEPT_EXIT = 0.9 # rad of heading actually turned (~52 deg) +CURVATURE_HOLD_EXIT_DECAY_TAU = 0.5 # s CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 # s stopped before the held turn intent is dropped # The model's time-domain action.desiredCurvature is blind below ~2.5 m/s (0.3 s ahead @@ -179,6 +190,7 @@ class Controls: self.lc_smooth_release = 0.0 self.turn_hold_curvature = 0.0 self.turn_hold_standstill_t = 0.0 + self.turn_hold_swept = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -309,8 +321,15 @@ class Controls: if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED: self.turn_hold_curvature = 0.0 self.turn_hold_standstill_t = 0.0 + self.turn_hold_swept = 0.0 else: - if CS.vEgo > CURVATURE_HOLD_HARD_SPEED and CC.latActive and self.turn_hold_curvature != 0.0: + if self.turn_hold_curvature == 0.0: + self.turn_hold_swept = 0.0 + else: + # heading actually swept in the hold's direction: the measure of turn progress + self.turn_hold_swept += max(CS.vEgo * self.curvature * math.copysign(1.0, self.turn_hold_curvature), 0.0) * DT_CTRL + turn_exiting = self.turn_hold_swept > CURVATURE_HOLD_SWEPT_EXIT + if (CS.vEgo > CURVATURE_HOLD_HARD_SPEED or turn_exiting) and CC.latActive and self.turn_hold_curvature != 0.0: # Decay toward the model's sustained same-direction demand instead of leaking on # wall-clock time: a wall-clock leak drained the floor mid-turn while the model # dipped transiently (turnn rlog 40.0-40.5s), while sustained low demand (end of @@ -318,7 +337,8 @@ class Controls: hold_dir = math.copysign(1.0, self.turn_hold_curvature) model_mag = max(new_desired_curvature * hold_dir, 0.0) if model_mag < abs(self.turn_hold_curvature): - decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / CURVATURE_HOLD_DECAY_TAU) + decay_tau = CURVATURE_HOLD_EXIT_DECAY_TAU if turn_exiting else CURVATURE_HOLD_DECAY_TAU + decayed = abs(self.turn_hold_curvature) + (model_mag - abs(self.turn_hold_curvature)) * (DT_CTRL / decay_tau) self.turn_hold_curvature = math.copysign(decayed, self.turn_hold_curvature) if CS.vEgo < 0.5: self.turn_hold_standstill_t += DT_CTRL