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

Revert "Significant improvement on the jerkiness"

Browse Source 
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.
This commit is contained in:
DevTekVE
2025-03-13 09:41:25 +01:00
parent 80f895c661
commit 85ce84e7b7
1 changed files with 4 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_angle.py
+4 -17
View File
@@ -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
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log