From 85ce84e7b7ad3bbbc3826aa7684302a7ea232f87 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Thu, 13 Mar 2025 09:41:25 +0100 Subject: [PATCH] Revert "Significant improvement on the jerkiness" This reverts commit ea1af879ba2905b076ccfe65993a9db701d689dd. Revert "More improvement but still not quite" This reverts commit ad95493c5c61b2ace7c459d2ebc151ddaa80040f. Revert "Adjust low-speed scaling for lateral control angle" This reverts commit 6f789ac1ebb66b0239b4028303573c2d7d386b39. Revert "Refactor speed-based steering scaling logic." This reverts commit 1d40735ab8db8d470ff3b287a6b42847beffff7d. --- selfdrive/controls/lib/latcontrol_angle.py | 21 ++++----------------- 1 file changed, 4 insertions(+), 17 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 459d233136..7bd0abc115 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,16 +1,10 @@ import math -import numpy as np from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees -# 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): @@ -25,18 +19,11 @@ class LatControlAngle(LatControl): angle_steers_des = float(CS.steeringAngleDeg) else: angle_log.active = True - - # 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 - - # 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_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) + angle_steers_des += params.angleOffsetDeg 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 \ No newline at end of file + angle_log.steeringAngleDesiredDeg = angle_steers_des + return 0, float(angle_steers_des), angle_log