mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-29 10:32:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Lane Smoothing Tune

Browse Source 
Tune to prevent overshoot
This commit is contained in:
whoisdomi
2026-04-24 22:11:43 -05:00
committed by firestar5683
parent 2e4c3f43ee
commit 2eb4dfbdc9
2 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+16 -4
View File
@@ -63,6 +63,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.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
@@ -168,10 +169,21 @@ class Controls:
jerk_factor = 1.0
lat_accel_factor = 1.0
if model_v2.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing) \
and CS.vEgo >= self.starpilot_toggles.minimum_lane_change_speed:
jerk_factor = self.starpilot_toggles.lane_change_jerk_factor
lat_accel_factor = self.starpilot_toggles.lane_change_lat_accel_factor
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
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
jerk_factor, lat_accel_factor)
starpilot/common/starpilot_variables.py
+1
View File
@@ -828,6 +828,7 @@ class StarPilotVariables:
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)