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

refactor

Browse Source
This commit is contained in:
rav4kumar
2025-05-24 16:18:07 -07:00
parent 1725f2500b
commit c10956b546
2 changed files with 83 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+13 -23
View File
@@ -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
sunnypilot/selfdrive/controls/lib/accel_personality/accel_profiles.py
+70 -12
View File
@@ -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))