diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ebabcfebd..19b2ee6da 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -60,7 +60,19 @@ LANE_CHANGE_ARREST_JERK_FLOOR = 0.6 # steeringPressed (the driver's instinctive grip during the unwind is what let the # collapse through, and the driver physically overpowers a torque command regardless). CURVATURE_HOLD_HARD_SPEED = 4.5 * CV.MPH_TO_MS -CURVATURE_HOLD_RELEASE_SPEED = 6.0 * CV.MPH_TO_MS +# Release must sit ABOVE the speed where the model's action wakes up mid-turn. Left +# turns cross the intersection before arcing, so the car reaches ~3.5-4 m/s while the +# action still reads ~0 (left1/left2 rlogs 2026-07-15: a 6 mph release dropped the +# floor mid-turn and visibly unwound the wheel 50 deg before the action woke; rights +# 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 +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 CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0 # s stopped before the held turn intent is dropped @@ -325,12 +337,16 @@ class Controls: # earlier-seeing source: it shows the turn at standstill while the action is # still blind, letting the pre-wind start before the car moves. turn_candidate = new_desired_curvature if CC.latActive else 0.0 - plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE - plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) - if CC.latActive and plan_curvature * blinker_dir > turn_candidate * blinker_dir: - turn_candidate = plan_curvature + if CC.latActive and CS.vEgo < CURVATURE_HOLD_PLAN_SOURCE_SPEED: + plan_curvature = get_plan_spatial_curvature(model_v2) * CURVATURE_HOLD_PLAN_SCALE + plan_curvature = max(min(plan_curvature, CURVATURE_HOLD_PLAN_CAP), -CURVATURE_HOLD_PLAN_CAP) + if plan_curvature * blinker_dir > turn_candidate * blinker_dir: + turn_candidate = plan_curvature if turn_candidate * blinker_dir > abs(self.turn_hold_curvature): - self.turn_hold_curvature = turn_candidate + new_mag = turn_candidate * blinker_dir + if CS.vEgo > CURVATURE_HOLD_PLAN_SOURCE_SPEED: + new_mag = min(new_mag, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL) + self.turn_hold_curvature = math.copysign(new_mag, turn_candidate) elif self.turn_hold_curvature * blinker_dir < 0.0: # blinker flipped to the other side: turn intent changed self.turn_hold_curvature = 0.0