hoooold

selfdrive/controls/controlsd.py

@@ -64,6 +64,33 @@ CURVATURE_HOLD_RELEASE_SPEED = 6.0 * CV.MPH_TO_MS
 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
 
+# The model's time-domain action.desiredCurvature is blind below ~2.5 m/s (0.3 s ahead
+# at creep speed is centimeters of road), but the plan's spatial geometry already shows
+# the turn at standstill: turn3/turn4 rlogs 2026-07-14 read plan curvature 0.13-0.16
+# while the action output sat at 0.005, and the plan value matched the demand the
+# action produced once rolling. Feeding it into the turn-hold ratchet lets the wheel
+# pre-wind toward the real turn before the car moves. Scaled and capped conservatively:
+# a too-high floor turns in tighter than the path (mild at creep lat accel), while a
+# too-low one just reduces the head start. The plan flickers straight for ~1-2 s right
+# at the standstill->motion transition; the ratchet holds through it by design.
+CURVATURE_HOLD_PLAN_LOOKAHEAD = 7.0  # m along the plan for the spatial curvature probe
+CURVATURE_HOLD_PLAN_SCALE = 0.85
+CURVATURE_HOLD_PLAN_CAP = 0.12       # 1/m
+
+
+def get_plan_spatial_curvature(model_v2, lookahead: float = CURVATURE_HOLD_PLAN_LOOKAHEAD) -> float:
+  # curvature of the circle through the origin, tangent to the car's heading, passing
+  # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
+  px, py = 0.0, 0.0
+  for x, y in zip(model_v2.position.x, model_v2.position.y):
+    px, py = x, y
+    if math.hypot(x, y) >= lookahead:
+      break
+  d2 = px * px + py * py
+  if d2 < 1.0:
+    return 0.0
+  return 2.0 * py / d2
+
 
 def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   spoofed_speed = set_speed_ms
@@ -268,10 +295,18 @@ class Controls:
       else:
         self.turn_hold_standstill_t = 0.0
       if blinker_dir != 0.0:
-        if CC.latActive and new_desired_curvature * blinker_dir > abs(self.turn_hold_curvature):
-          # ratchet up on the raw model command, never on the floored/measured value,
-          # so the hold can't feed itself and defeat the leak
-          self.turn_hold_curvature = new_desired_curvature
+        # 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,
+        # 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 turn_candidate * blinker_dir > abs(self.turn_hold_curvature):
+          self.turn_hold_curvature = 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