From ea3a9ae91111528f581d0f347c2a33faa283a2e3 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Thu, 3 Apr 2025 23:07:23 +0200 Subject: [PATCH] Improve angle smoothing by integrating dynamic parameter tuning Introduced a dynamic smoothing factor using the `HkgTuningAngleSmoothingFactor` parameter. This allows more granular control over curvature smoothing based on customizable user input, enhancing driving smoothness. Added necessary logic to process and apply this parameter efficiently. --- selfdrive/controls/lib/latcontrol_angle.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 8ee2b581bf..1a0083df6b 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,5 +1,6 @@ import math import numpy as np +from openpilot.common.params_pyx import Params from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl @@ -9,14 +10,22 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees class LatControlAngle(LatControl): def __init__(self, CP, CP_SP, CI): super().__init__(CP, CP_SP, CI) + self.params = Params() 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] + self.count = 0 + self.smoothing_factor_incr = 0. def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): + self.count += 1 + if self.count % 25 == 0 and (smoothingFactorParam := self._params.get("HkgTuningAngleSmoothingFactor")): + self.count = 0 + self.smoothing_factor_incr = float(smoothingFactorParam) / 10.0 + angle_log = log.ControlsState.LateralAngleState.new_message() if not active: @@ -25,7 +34,7 @@ class LatControlAngle(LatControl): else: angle_log.active = True # Apply exponential smoothing to the curvature - adjusted_alpha = float(np.interp(CS.vEgo, self.filter_speed_matrox, self.filter_alpha_matrix)) + adjusted_alpha = max(float(np.interp(CS.vEgo, self.filter_speed_matrox, self.filter_alpha_matrix)) + self.smoothing_factor_incr, 1) 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))