Hold the line!

selfdrive/controls/controlsd.py

@@ -52,15 +52,16 @@ LANE_CHANGE_ARREST_JERK_FLOOR = 0.6
 # 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
+# between hard and release speed it decays toward the model's sustained demand, so a
+# transient model dip barely sags it while a genuine end-of-turn unwind or an aborted
+# turn still drains it in a few seconds. 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_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
 
 
@@ -250,9 +251,16 @@ class Controls:
       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 > CURVATURE_HOLD_HARD_SPEED 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
+        # turn, abort) still drains the hold within a couple of time constants.
+        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)
+          self.turn_hold_curvature = math.copysign(decayed, 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:

selfdrive/controls/lib/latcontrol_torque.py

@@ -162,27 +162,34 @@ class LatControlTorque(LatControl):
                               getattr(starpilot_toggles, "flm_active_profile_id", ""))
     set_flm_runtime_overrides(getattr(starpilot_toggles, "flm_active_overrides", None) if flm_profile_active else None)
     flm_surface_active = flm_profile_active and flm_runtime_overrides_active()
+    measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
+    measurement = measured_curvature * CS.vEgo ** 2
+    future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
     if not active:
       output_torque = 0.0
       pid_log.active = False
       self.pid.reset()
-      self.previous_measurement = 0.0
+      # Keep the request buffer and rate state primed with the live command (which tracks
+      # the measured curvature while inactive) instead of zeroing them. Re-engaging with a
+      # wound wheel against a zeroed buffer puts the setpoint ~lat_delay behind the
+      # measurement, and the low-speed gains turn that lag into a hard unwind shove
+      # (turnn rlog 38.75s: +0.8 torque against a held right turn on pull-away).
+      self.lat_accel_request_buffer.append(future_desired_lateral_accel)
+      self.previous_measurement = measurement
       self.measurement_rate_filter.x = 0.0
-      self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len)
-      self.prev_desired_lateral_accel = 0.0
+      self.jerk_filter.x = 0.0
+      self.prev_desired_lateral_accel = future_desired_lateral_accel
       self.ioniq_6_directional_taper_filter.x = 1.0
     else:
       if self.prev_steering_pressed and not CS.steeringPressed:
         self.pid.i *= self.steer_release_i_decay
 
-      measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
       roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
       curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
       delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
       expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
-      future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
       self.lat_accel_request_buffer.append(future_desired_lateral_accel)
       raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
       raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
@@ -194,7 +201,6 @@ class LatControlTorque(LatControl):
                          abs(setpoint) < UNWIND_LAT_ACCEL_NEAR_ZERO)
       self.prev_desired_lateral_accel = setpoint
 
-      measurement = measured_curvature * CS.vEgo ** 2
       measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
       measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
       self.previous_measurement = measurement