mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-23 20:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactor speed-based steering scaling logic.

Browse Source 
Updated the steering angle computation to use a clearer and more descriptive speed-scaling configuration. Replaced low-speed-specific logic with a generalized approach based on speed breakpoints and corresponding influence factors. This improves maintainability and ensures smoother steering adjustments at varying speeds.
This commit is contained in:
DevTekVE
2025-03-11 20:36:25 +01:00
parent 956d2c36d0
commit 10f345f956
1 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_angle.py
+12 -10
View File
@@ -6,9 +6,11 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
# Define a speed-based scaling factor similar to low_speed_factor
LOW_SPEED_X = [0, 3, 6, 10] # Speed breakpoints
LOW_SPEED_Y = [0, 0.2, 0.45, 1.0] # Factor reducing influence at low speeds
# Speed-based scaling configuration for steering responsiveness
# As speed increases, we apply more of the desired steering angle change
SPEED_BREAKPOINTS_MS = [0, 3, 6, 10] # Speed breakpoints in m/s
STEERING_FACTOR_AT_SPEED = [0, 0.2, 0.45, 1.0] # Corresponding steering influence factors
# 0 = ignore desired angle (keep current steering), 1.0 = fully apply desired angle change
class LatControlAngle(LatControl):
def __init__(self, CP, CP_SP, CI):
@@ -24,17 +26,17 @@ class LatControlAngle(LatControl):
else:
angle_log.active = True
# Compute the base desired steering angle
base_angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
base_angle_steers_des += params.angleOffsetDeg
# Compute the fully desired steering angle based on curvature
raw_angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
raw_angle_steers_des += params.angleOffsetDeg # Apply calibration offset
# Apply a low-speed factor to reduce aggressive changes at low speeds
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)
angle_steers_des = CS.steeringAngleDeg + low_speed_factor * (base_angle_steers_des - CS.steeringAngleDeg)
# Calculate the scaling factor. At lower speeds, we apply less of the desired steering change to avoid jerky movements
speed_scaling_factor = np.interp(CS.vEgo, SPEED_BREAKPOINTS_MS, STEERING_FACTOR_AT_SPEED)
angle_steers_des = CS.steeringAngleDeg + speed_scaling_factor * (raw_angle_steers_des - CS.steeringAngleDeg)
angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = float(angle_steers_des)
return 0, float(angle_steers_des), angle_log
return 0, float(angle_steers_des), angle_log