From 10f345f9569939879a9e1bf6ea36711e9ef40751 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Tue, 11 Mar 2025 20:36:25 +0100 Subject: [PATCH] Refactor speed-based steering scaling logic. 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. --- selfdrive/controls/lib/latcontrol_angle.py | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 5eab619d85..459d233136 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -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 \ No newline at end of file