Hodor

selfdrive/controls/controlsd.py

@@ -46,6 +46,23 @@ LANE_CHANGE_SMOOTH_RELEASE_T = 2.0
 # 0.6 (≈ pace-5 rate) fully tracks the arrest demand seen in logs while staying 40% below stock.
 LANE_CHANGE_ARREST_JERK_FLOOR = 0.6
 
+# Low-speed turn-intent curvature hold. Approaching a turn with the blinker on, the
+# model's time-based plan collapses as the car slows to a stop: desiredCurvature decays
+# to zero, the controller actively unwinds the wheel at the intersection, and on
+# pull-away it re-winds too late — the car goes wide (pauseturn rlog 2026-07-13).
+# The hold ratchets up on the blinker-matching model command below the release speed
+# and floors the command magnitude afterwards. Below the hard speed the floor is firm;
+# between hard and release speed it leaks away so a genuine end-of-turn unwind or an
+# aborted turn still completes in ~1-2 s. Retention deliberately does NOT depend on the
+# blinker (the stalk auto-cancels during the stop in the log), on latActive (lateral
+# goes inactive at standstill on torque cars; the wheel parks on rack friction), or on
+# 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
+CURVATURE_HOLD_LEAK_RATE = 0.04    # 1/m per s; drains a typical 0.04 intersection hold in ~1 s
+CURVATURE_HOLD_STANDSTILL_TIMEOUT = 30.0  # s stopped before the held turn intent is dropped
+
 
 def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   spoofed_speed = set_speed_ms
@@ -100,6 +117,8 @@ class Controls:
     self.curvature = 0.0
     self.desired_curvature = 0.0
     self.lc_smooth_release = 0.0
+    self.turn_hold_curvature = 0.0
+    self.turn_hold_standstill_t = 0.0
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -223,6 +242,36 @@ class Controls:
     else:
       new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
 
+    # Low-speed turn-intent hold (see CURVATURE_HOLD_* above). Curvature sign convention
+    # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
+    # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
+    blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
+    if CS.vEgo >= CURVATURE_HOLD_RELEASE_SPEED:
+      self.turn_hold_curvature = 0.0
+      self.turn_hold_standstill_t = 0.0
+    else:
+      if CS.vEgo > CURVATURE_HOLD_HARD_SPEED:
+        self.turn_hold_curvature = math.copysign(max(abs(self.turn_hold_curvature) - CURVATURE_HOLD_LEAK_RATE * DT_CTRL, 0.0),
+                                                 self.turn_hold_curvature)
+      if CS.vEgo < 0.5:
+        self.turn_hold_standstill_t += DT_CTRL
+        if self.turn_hold_standstill_t > CURVATURE_HOLD_STANDSTILL_TIMEOUT:
+          self.turn_hold_curvature = 0.0
+      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
+        elif self.turn_hold_curvature * blinker_dir < 0.0:
+          # blinker flipped to the other side: turn intent changed
+          self.turn_hold_curvature = 0.0
+      if CC.latActive and self.turn_hold_curvature != 0.0:
+        hold_dir = math.copysign(1.0, self.turn_hold_curvature)
+        if new_desired_curvature * hold_dir < abs(self.turn_hold_curvature):
+          new_desired_curvature = self.turn_hold_curvature
+
     jerk_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:
       set_jerk = self.starpilot_toggles.lane_change_jerk_factor