mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-10-10 17:43:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

exponential smoothing

Browse Source
This commit is contained in:
rav4kumar
2025-10-28 22:26:08 -07:00
parent 22aff8d2eb
commit cdcc2392a8
1 changed files with 44 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+44 -12
View File
@@ -15,20 +15,23 @@ AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality
# Acceleration Profiles
MAX_ACCEL_PROFILES = {
AccelPersonality.eco: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], # eco
AccelPersonality.normal: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125], # normal
AccelPersonality.sport: [2.0, 2.00, 1.97, 1.00, .635, .48, .31, .165], # sport
#AccelPersonality.sport: [2.0, 2.00, 2.00, 1.90, 1.40, .70, .42, .251], # sport
AccelPersonality.eco: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125],
AccelPersonality.normal: [2.0, 1.99, 1.92, .850, .500, .33, .23, .125],
AccelPersonality.sport: [2.0, 2.00, 1.97, 1.00, .635, .48, .31, .165],
}
MAX_ACCEL_BREAKPOINTS = [0., 4., 6., 9., 16., 25., 30., 55.]
# Braking Profiles
MIN_ACCEL_PROFILES = {
AccelPersonality.eco: [-.0100, -.0100, -.060, -.60, -1.20], # gentler braking
AccelPersonality.normal: [-.0000015, -.0000015, -.060, -.60, -1.20], # normal braking
AccelPersonality.sport: [-.0000020, -.0000020, -.200, -.80, -1.20], # more aggressive braking
AccelPersonality.eco: [-0.0000002, -0.0000002, -0.2, -0.2, -1.20],
AccelPersonality.normal: [-0.0000002, -0.0000002, -0.3, -0.3, -1.20],
AccelPersonality.sport: [-0.0000003, -0.0000003, -0.4, -0.4, -1.20],
}
MIN_ACCEL_BREAKPOINTS = [0., 5.0, 11., 14, 25.]
MIN_ACCEL_BREAKPOINTS = [0., 5., 8., 14., 25.]
DECEL_SMOOTH_ALPHA = 0.08 # Very aggressive smoothing for decel (lower = smoother)
ACCEL_SMOOTH_ALPHA = 0.20 # Less aggressive for accel (higher = more responsive)
MAX_DECEL_RATE = 0.15 # Maximum change in decel per timestep (m/s²)
class AccelPersonalityController:
@@ -37,6 +40,9 @@ class AccelPersonalityController:
self.params = Params()
self.frame = 0
self.accel_personality = AccelPersonality.normal
self.last_max_accel = 2.0
self.last_min_accel = -0.01
self.first_run = True
self.param_keys = {
'personality': 'AccelPersonality',
'enabled': 'AccelPersonalityEnabled'
@@ -83,9 +89,32 @@ class AccelPersonalityController:
return int(next_personality)
def get_accel_limits(self, v_ego: float) -> tuple[float, float]:
max_a = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
min_a = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality])
return float(min_a), float(max_a)
v_ego = max(0.0, v_ego)
target_max_accel = np.interp(v_ego, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES[self.accel_personality])
target_min_accel = np.interp(v_ego, MIN_ACCEL_BREAKPOINTS, MIN_ACCEL_PROFILES[self.accel_personality])
if self.first_run:
self.last_max_accel = target_max_accel
self.last_min_accel = target_min_accel
self.first_run = False
return float(target_min_accel), float(target_max_accel)
# exponential smoothing to max accel
self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max_accel + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel)
# VERY aggressive smoothing to min accel for ultra-smooth braking
# Also add rate limiting as a safety net
smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min_accel + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel)
# Rate limit decel changes and/or prevent sudden jerks
max_change_per_step = MAX_DECEL_RATE * DT_MDL
decel_change = smoothed_decel - self.last_min_accel
decel_change = np.clip(decel_change, -max_change_per_step, max_change_per_step)
self.last_min_accel = self.last_min_accel + decel_change
if self.last_min_accel > self.last_max_accel:
self.last_min_accel = self.last_max_accel - 0.1
return float(self.last_min_accel), float(self.last_max_accel)
def get_min_accel(self, v_ego: float) -> float:
return self.get_accel_limits(v_ego)[0]
@@ -108,7 +137,10 @@ class AccelPersonalityController:
def reset(self):
self.accel_personality = AccelPersonality.normal
self.frame = 0
self.last_max_accel = 2.0
self.last_min_accel = -0.01
self.first_run = True
def update(self):
self.frame += 1
self._update_from_params()
self._update_from_params()