mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-06-09 21:14:44 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Revert the EMA calculation on the curvature to test another approach"

Browse Source 
This reverts commit bd471b3498.
This commit is contained in:
DevTekVE
2025-04-03 23:01:33 +02:00
parent f1e070281b
commit 58fcda8cb5
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
selfdrive/controls/lib/latcontrol_angle.py
View File

@@ -1,15 +1,20 @@
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
class LatControlAngle(LatControl):
def __init__(self, CP, CP_SP, CI):
super().__init__(CP, CP_SP, CI)
self.sat_check_min_speed = 5.
self.sat_check_min_speed = 5.0
# Initialize the filtered curvature to zero (or an appropriate initial value)
self.filtered_curvature = 0.0
# Filter coefficient: adjust between 0 (very smooth) and 1 (no filtering)
self.filter_speed_matrox = [0, 8.5, 11, 13.8, 22.22]
self.filter_alpha_matrix = [0.05, 0.1, 0.3, 0.6, 1]
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -19,11 +24,15 @@ class LatControlAngle(LatControl):
angle_steers_des = float(CS.steeringAngleDeg)
else:
angle_log.active = True
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
# Apply exponential smoothing to the curvature
adjusted_alpha = float(np.interp(CS.vEgo, self.filter_speed_matrox, self.filter_alpha_matrix))
self.filtered_curvature = (adjusted_alpha * desired_curvature + (1 - adjusted_alpha) * self.filtered_curvature)
# Convert the smoothed curvature to a steering angle
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-self.filtered_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 = angle_steers_des
angle_log.steeringAngleDesiredDeg = float(angle_steers_des)
return 0, float(angle_steers_des), angle_log