diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d46431a3e..0ecda0f2e 100644 --- a/selfdrive/controls/controlsd.py +++ b/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: diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index aab258885..154de28c7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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