From 7e1b3769d7929883f93f627ce28edf6b5cf2c88f Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 7 Jul 2026 10:33:40 -0500 Subject: [PATCH] Lane Change Smoothing 2.0 1. Reduced jerk 2. Smoother 3. Improved arrest, reducing chances of overshooting if setting is too low --- selfdrive/controls/controlsd.py | 48 +++++++++++++++++-------- selfdrive/controls/lib/drive_helpers.py | 17 +++++++-- starpilot/common/starpilot_variables.py | 8 ++--- 3 files changed, 51 insertions(+), 22 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ce9cf23e5..5a18174a8 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -36,6 +36,16 @@ LaneChangeDirection = log.LaneChangeDirection ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys()) +# After a smoothed lane change ends, ramp the curvature limits back to stock over this +# time so the final recenter correction is shaped instead of stepping through unclamped. +LANE_CHANGE_SMOOTH_RELEASE_T = 2.0 + +# Floor on the jerk factor when the model is unwinding lane-change curvature (the arrest and +# any correction back toward center). Entry gentleness is comfort, but arrest speed is a +# correctness constraint: a slow symmetric cap lets the car glide past the new lane center. +# 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 + def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: spoofed_speed = set_speed_ms @@ -89,7 +99,7 @@ class Controls: self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 - self.lc_smooth_elapsed = 0.0 + self.lc_smooth_release = 0.0 self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -212,25 +222,35 @@ class Controls: new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature jerk_factor = 1.0 - lat_accel_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: - t_target = self.starpilot_toggles.lane_change_t_target set_jerk = self.starpilot_toggles.lane_change_jerk_factor - set_accel = self.starpilot_toggles.lane_change_lat_accel_factor in_lane_change = model_v2.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing) \ and CS.vEgo >= self.starpilot_toggles.minimum_lane_change_speed - if in_lane_change or 0.0 < self.lc_smooth_elapsed < t_target: - self.lc_smooth_elapsed = min(self.lc_smooth_elapsed + DT_CTRL, t_target) - progress = self.lc_smooth_elapsed / t_target # 0 → 1 over T seconds - jerk_factor = set_jerk + (1.0 - set_jerk) * progress - lat_accel_factor = set_accel + (1.0 - set_accel) * progress - if not in_lane_change and self.lc_smooth_elapsed >= t_target: - self.lc_smooth_elapsed = 0.0 - elif not in_lane_change: - self.lc_smooth_elapsed = 0.0 + # Hold the tight jerk limit for the whole maneuver, then taper back to stock so the + # model's recenter step and mid-change corrections stay shaped instead of passing + # through a mostly-relaxed clamp. Only the jerk (curvature rate) is tightened: capping + # lat accel strangles the end-of-maneuver arrest and lets the car glide past the new + # lane center before it can build enough counter-curvature. + if in_lane_change: + self.lc_smooth_release = LANE_CHANGE_SMOOTH_RELEASE_T + else: + self.lc_smooth_release = max(self.lc_smooth_release - DT_CTRL, 0.0) + if self.lc_smooth_release > 0.0: + release = 1.0 - self.lc_smooth_release / LANE_CHANGE_SMOOTH_RELEASE_T # 0 in maneuver → 1 after + jerk_factor = set_jerk + (1.0 - set_jerk) * release + # When the model is unwinding curvature (reducing the lane-change curvature magnitude) + # and the entry cap would make the command lag it, apply the arrest floor so the car + # can stop on the new lane center. Applies only to the unwind direction; the entry + # ramp keeps the full pace smoothness. Robust to double lane changes (no baseline). + model_unwinding = abs(new_desired_curvature) < abs(self.desired_curvature) and \ + math.copysign(1.0, new_desired_curvature - self.desired_curvature) == -math.copysign(1.0, self.desired_curvature) and \ + abs(self.desired_curvature) > 1e-4 + if model_unwinding: + arrest_floor = LANE_CHANGE_ARREST_JERK_FLOOR + (1.0 - LANE_CHANGE_ARREST_JERK_FLOOR) * release + jerk_factor = max(jerk_factor, arrest_floor) self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, - jerk_factor, lat_accel_factor) + jerk_factor) lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS actuators.curvature = self.desired_curvature diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 16fb2d2dd..b6e711a95 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -32,9 +32,20 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll, jerk_factor=1.0, effective_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL * lat_accel_factor roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY - max_lat_accel = effective_lat_accel + roll_compensation - min_lat_accel = -effective_lat_accel + roll_compensation - new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2) + min_curvature = (-effective_lat_accel + roll_compensation) / v_ego ** 2 + max_curvature = (effective_lat_accel + roll_compensation) / v_ego ** 2 + if lat_accel_factor < 1.0: + # A tightened maneuver clamp must not clip curvature already being commanded + # (e.g. lane change on a curve); it only limits further growth. + min_curvature = min(min_curvature, prev_curvature) + max_curvature = max(max_curvature, prev_curvature) + # Saturation is reported against the stock envelope only: riding an intentionally + # tightened lane-change ceiling is comfort shaping, not steering saturation, and + # must not trip the "Turn Exceeds Steering Limit" alert. + stock_min_curvature = (-MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2 + stock_max_curvature = (MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2 + limited_accel = bool(new_curvature < stock_min_curvature or new_curvature > stock_max_curvature) + new_curvature, _ = clamp(new_curvature, min_curvature, max_curvature) new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE) return float(new_curvature), limited_accel or limited_max_curv diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 19259c3a5..02b61d2bc 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1039,19 +1039,17 @@ class StarPilotVariables: toggle.one_lane_change = self.get_value("OneLaneChange", condition=toggle.lane_changes) # Lane change pace: 1 = smoothest (~8 s target), 10 = stock (no clamp applied) - # Factors are derived from a sinusoidal lane-change profile: a = pi^2 * W / T^2, j = pi^3 * W / T^3. - # 1.3x headroom keeps the controller off the ceiling mid-maneuver. + # The jerk factor is derived from a sinusoidal lane-change profile: j = pi^3 * W / T^3, + # with 1.3x headroom. Only jerk (curvature rate) is shaped; lateral accel stays at the + # stock envelope so the end-of-maneuver arrest is never starved of authority. pace = self.get_value("LaneChangeSmoothing", cast=int, condition=toggle.lane_changes) or 10 pace = max(1, min(10, pace)) lane_w = 3.5 t_target = 3.0 + (10 - pace) * 5.0 / 9.0 - a_req = (math.pi ** 2) * lane_w / (t_target ** 2) j_req = (math.pi ** 3) * lane_w / (t_target ** 3) toggle.lane_change_pace = pace - toggle.lane_change_lat_accel_factor = min(1.0, a_req * 1.3 / 3.0) toggle.lane_change_jerk_factor = min(1.0, j_req * 1.3 / 5.0) toggle.lane_change_time_max = 10.0 + (10 - pace) * 2.0 / 9.0 - toggle.lane_change_t_target = t_target lateral_tuning = self.get_value("LateralTune") toggle.force_torque_controller = self.get_value("ForceTorqueController", condition=lateral_tuning and not is_torque_car and not is_angle_car)