mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-16 14:52:15 +08:00
Code Issues Packages Projects Releases Wiki Activity

Update frogpilot_acceleration.py

Browse Source
This commit is contained in:
firestar5683
2025-10-03 22:36:15 -05:00
parent c97208c1dd
commit 315fd1c3b9
1 changed files with 13 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
frogpilot/controls/lib/frogpilot_acceleration.py
+13 -19
View File
@@ -8,36 +8,36 @@ def cubic_interp(x, xp, fp):
return fp[0]
elif x >= xp[-1]:
return fp[-1]
# Find interval
i = np.searchsorted(xp, x) - 1
i = max(0, min(i, len(xp)-2)) # clamp the index
# Normalized position
t = (x - xp[i]) / float(xp[i+1] - xp[i])
# Hermite cubic formula
return fp[i]*(1 - 3*t**2 + 2*t**3) + fp[i+1]*(3*t**2 - 2*t**3)
def akima_interp(x, xp, fp):
"""Akima-inspired interpolation with reduced overshoot characteristics."""
if x <= xp[0]:
return fp[0]
elif x >= xp[-1]:
return fp[-1]
i = np.searchsorted(xp, x) - 1
i = max(0, min(i, len(xp)-2)) # clamp the index
t = (x - xp[i]) / float(xp[i+1] - xp[i])
# Quintic polynomial to reduce overshoot
t2 = t*t
t4 = t2*t2
t3 = t2*t
return (fp[i]*(1 - 10*t3 + 15*t4 - 6*t3*t2)
+ fp[i+1]*(10*t3 - 15*t4 + 6*t3*t2))
from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
from openpilot.frogpilot.common.frogpilot_variables import CITY_SPEED_LIMIT
@@ -49,7 +49,6 @@ A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
A_CRUISE_MAX_BP_CUSTOM = [0.0, 5., 10., 15., 20., 25., 40.]
A_CRUISE_MAX_VALS_ECO = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
A_CRUISE_MAX_VALS_SPORT = [1.5, 1.5, 1.25, 1.5, 1.5, 1.5, 2.0]
A_CRUISE_MAX_VALS_SPORT_PLUS = [2.5, 2.5, 3.0, 2.5, 2.5, 2.5, 2.5]
def get_max_accel_eco(v_ego):
return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO))
@@ -57,9 +56,6 @@ def get_max_accel_eco(v_ego):
def get_max_accel_sport(v_ego):
return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT))
def get_max_accel_sport_plus(v_ego):
return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_SPORT_PLUS))
def get_max_accel_low_speeds(max_accel, v_cruise):
return float(akima_interp(v_cruise, [0., CITY_SPEED_LIMIT / 2, CITY_SPEED_LIMIT], [max_accel / 4, max_accel / 2, max_accel]))
@@ -86,19 +82,17 @@ class FrogPilotAcceleration:
if eco_gear:
self.max_accel = get_max_accel_eco(v_ego)
else:
if frogpilot_toggles.sport_plus:
self.max_accel = get_max_accel_sport_plus(v_ego)
else:
if frogpilot_toggles.acceleration_profile == 2:
self.max_accel = get_max_accel_sport(v_ego)
else:
self.max_accel = get_max_allowed_accel(v_ego)
else:
if frogpilot_toggles.acceleration_profile == 1:
self.max_accel = get_max_accel_eco(v_ego)
elif frogpilot_toggles.acceleration_profile == 2:
self.max_accel = get_max_accel_sport(v_ego)
elif frogpilot_toggles.sport_plus:
self.max_accel = get_max_accel_sport_plus(v_ego)
else:
self.max_accel = get_max_accel(v_ego)
elif frogpilot_toggles.acceleration_profile == 3:
self.max_accel = get_max_allowed_accel(v_ego)
if frogpilot_toggles.human_acceleration:
self.max_accel = min(get_max_accel_low_speeds(self.max_accel, self.frogpilot_planner.v_cruise), self.max_accel)