Hold the veggies

selfdrive/controls/controlsd.py

@@ -73,16 +73,22 @@ CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0  # s stopped before the held turn inten
 # 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_LOOKAHEAD_NEAR = 4.0  # m; reads whether the turn starts NOW
+CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR = 7.0   # m; reads the turn's curvature
 CURVATURE_HOLD_PLAN_SCALE = 0.85
 CURVATURE_HOLD_PLAN_CAP = 0.12       # 1/m
+# The model counter-steers at every turn exit; an opposite-direction command is the
+# "turn is over" signal at any speed. Without this the floor converted the exit unwind
+# (-0.076) into a stuck +0.012 for 1.4 s and the driver had to unwind by hand
+# (rightturnfail rlog 33.2-34.4s). Deadband rejects the ~0.002 pull-away flickers.
+CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01  # 1/m
 
 
-def get_plan_spatial_curvature(model_v2, lookahead: float = CURVATURE_HOLD_PLAN_LOOKAHEAD) -> float:
+def _plan_circle_curvature(xs, ys, lookahead: float) -> 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):
+  for x, y in zip(xs, ys):
     px, py = x, y
     if math.hypot(x, y) >= lookahead:
       break
@@ -92,6 +98,20 @@ def get_plan_spatial_curvature(model_v2, lookahead: float = CURVATURE_HOLD_PLAN_
   return 2.0 * py / d2
 
 
+def get_plan_spatial_curvature(model_v2) -> float:
+  # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
+  # starts immediately, which over-winds wide turns whose arc begins several meters out
+  # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
+  # car creeps up to the arc, so the pre-wind self-scales to the turn geometry. Sign
+  # disagreement means no coherent turn ahead: contribute nothing.
+  xs, ys = model_v2.position.x, model_v2.position.y
+  near = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_NEAR)
+  far = _plan_circle_curvature(xs, ys, CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR)
+  if near * far <= 0.0:
+    return 0.0
+  return near if abs(near) < abs(far) else far
+
+
 def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   spoofed_speed = set_speed_ms
 
@@ -294,6 +314,10 @@ class Controls:
           self.turn_hold_curvature = 0.0
       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:
         # 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