From c10956b546d56d34d06865d3f005b3145decddff Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 24 May 2025 16:18:07 -0700 Subject: [PATCH] refactor --- .../lib/accel_personality/accel_controller.py | 36 +++----- .../lib/accel_personality/accel_profiles.py | 82 ++++++++++++++++--- 2 files changed, 83 insertions(+), 35 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index a7cd556d68..13a2bc8e94 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -5,22 +5,17 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ from cereal import custom -from numpy import interp from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_profiles import ( - MAX_ACCEL_ECO, MAX_ACCEL_NORMAL, MAX_ACCEL_SPORT, - MIN_ACCEL_ECO, MIN_ACCEL_NORMAL, MIN_ACCEL_SPORT, MIN_ACCEL_STOCK, - MAX_ACCEL_BREAKPOINTS, MIN_ACCEL_BREAKPOINTS + get_max_accel_hermite, + get_min_accel_hermite ) AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality -def clamp(val: float, lower: float, upper: float) -> float: - return max(lower, min(val, upper)) - class AccelController: def __init__(self): self.params = Params() @@ -38,34 +33,28 @@ class AccelController: def _get_max_accel_for_speed(self, v_ego: float) -> float: self._update_personality_from_param() - # Clamp v_ego to valid interpolation range - v_ego = clamp(v_ego, MAX_ACCEL_BREAKPOINTS[0], MAX_ACCEL_BREAKPOINTS[-1]) - if self.personality == AccelPersonality.eco: - accel_profile = MAX_ACCEL_ECO + mode = "eco" elif self.personality == AccelPersonality.sport: - accel_profile = MAX_ACCEL_SPORT + mode = "sport" else: - accel_profile = MAX_ACCEL_NORMAL + mode = "normal" - return float(interp(v_ego, MAX_ACCEL_BREAKPOINTS, accel_profile)) + return get_max_accel_hermite(v_ego, mode) def _get_min_accel_for_speed(self, v_ego: float) -> float: self._update_personality_from_param() - # Clamp v_ego to valid interpolation range - v_ego = clamp(v_ego, MIN_ACCEL_BREAKPOINTS[0], MIN_ACCEL_BREAKPOINTS[-1]) - if self.personality == AccelPersonality.eco: - accel_profile = MIN_ACCEL_ECO + mode = "eco" elif self.personality == AccelPersonality.sport: - accel_profile = MIN_ACCEL_SPORT + mode = "sport" elif self.personality == AccelPersonality.normal: - accel_profile = MIN_ACCEL_NORMAL + mode = "normal" else: - accel_profile = MIN_ACCEL_STOCK + mode = "stock" - return float(interp(v_ego, MIN_ACCEL_BREAKPOINTS, accel_profile)) + return get_min_accel_hermite(v_ego, mode) def get_accel_limits(self, v_ego: float, accel_limits: list[float]) -> tuple[float, float]: self._update_personality_from_param() @@ -74,7 +63,8 @@ class AccelController: return (accel_limits[0], accel_limits[1]) else: max_accel = self._get_max_accel_for_speed(v_ego) - return (accel_limits[0], max_accel) + min_accel = self._get_min_accel_for_speed(v_ego) + return (min_accel, max_accel) def is_personality_enabled(self, accel_personality: int = AccelPersonality.stock) -> bool: self.personality = accel_personality diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py index 3e18f8de29..533acecff4 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py @@ -5,17 +5,75 @@ This file is part of sunnypilot and is licensed under the MIT License. See the LICENSE.md file in the root directory for more details. """ -# Acceleration profile for maximum allowed acceleration -MAX_ACCEL_ECO = [2.50, 1.80, 1.58, 1.45, 0.82, .532, .432, .32, .29, .085] -MAX_ACCEL_NORMAL = [2.50, 1.90, 1.72, 1.65, 1.00, .75, .61, .50, .38, .2] -MAX_ACCEL_SPORT = [2.00, 2.00, 1.98, 1.90, 1.30, 1.00, .72, .60, .48, .3] +import numpy as np -# Acceleration profile for minimum (braking) acceleration -MIN_ACCEL_ECO = [-0.0062, -0.02, -0.05, -0.4, -0.6] -MIN_ACCEL_NORMAL = [-0.0064, -0.02, -0.06, -0.5, -0.7] -MIN_ACCEL_SPORT = [-0.0062, -0.02, -0.05, -0.4, -0.6] -MIN_ACCEL_STOCK = [-1.2, -1.2, -1.2, -1.2, -1.2] +# Profiles +MAX_ACCEL_PROFILES = { + "eco": [2.00, 2.00, 1.98, 1.54, 0.83, .572, .455, .365, .32, .10], + "normal": [2.00, 2.00, 1.99, 1.66, 1.06, .66, .58, .49, .37, .15], + "sport": [2.00, 2.00, 2.00, 1.95, 1.25, .88, .70, .60, .45, .20], +} +MAX_ACCEL_BREAKPOINTS = [0., 1., 6., 8., 11., 16., 20., 25., 30., 55.] -# Speed breakpoints for interpolation -MAX_ACCEL_BREAKPOINTS = [0., 1., 6., 8., 11., 16, 20., 25., 30., 55.] -MIN_ACCEL_BREAKPOINTS = [0., 0.3, 1., 27, 40] +MIN_ACCEL_PROFILES = { + "eco": [-.06, -.06, -.11, -.11, -.071, -.071, -.072, -.65, -.65], + "normal": [-.07, -.07, -.12, -.12, -.072, -.072, -.073, -.70, -.70], + "sport": [-1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2], + "stock": [-1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2, -1.2], +} +MIN_ACCEL_BREAKPOINTS = [0., 0.5, 0.6, 1.2, 8., 12., 14., 25., 40.] + +# Precompute slopes for Cubic Bézier curves +def compute_symmetric_slopes(x, y): + n = len(x) + if n < 2: + raise ValueError("At least two points are required to compute slopes.") + m = np.zeros(n) + for i in range(n): + if i == 0: + m[i] = (y[i+1] - y[i]) / (x[i+1] - x[i]) + elif i == n-1: + m[i] = (y[i] - y[i-1]) / (x[i] - x[i-1]) + else: + m[i] = ((y[i+1] - y[i]) / (x[i+1] - x[i]) + (y[i] - y[i-1]) / (x[i] - x[i-1])) / 2 + return m + + +MAX_ACCEL_SLOPES = { + mode: compute_symmetric_slopes(MAX_ACCEL_BREAKPOINTS, values) + for mode, values in MAX_ACCEL_PROFILES.items() +} + +MIN_ACCEL_SLOPES = { + mode: compute_symmetric_slopes(MIN_ACCEL_BREAKPOINTS, values) + for mode, values in MIN_ACCEL_PROFILES.items() +} + +# Hermite interpolation function +def hermite_interpolate(x, xp, yp, slopes, mode): + # Clip x inside the domain + x = np.clip(x, xp[0], xp[-1]) + + # Find segment + idx = np.searchsorted(xp, x) - 1 + idx = np.clip(idx, 0, len(slopes[mode]) - 1) + + x0, x1 = xp[idx], xp[idx+1] + y0, y1 = yp[idx], yp[idx+1] + m0, m1 = slopes[mode][idx], slopes[mode][idx+1] + + t = (x - x0) / (x1 - x0) + h00 = 2*t**3 - 3*t**2 + 1 + h10 = t**3 - 2*t**2 + t + h01 = -2*t**3 + 3*t**2 + h11 = t**3 - t**2 + + interpolated = (h00 * y0) + (h10 * (x1 - x0) * m0) + (h01 * y1) + (h11 * (x1 - x0) * m1) + return interpolated + +# Final functions to call +def get_max_accel_hermite(v_ego: float, mode: str = "normal") -> float: + return float(hermite_interpolate(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[mode], MAX_ACCEL_SLOPES, mode)) + +def get_min_accel_hermite(v_ego: float, mode: str = "normal") -> float: + return float(hermite_interpolate(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[mode], MIN_ACCEL_SLOPES, mode))