diff --git a/frogpilot/controls/lib/frogpilot_acceleration.py b/frogpilot/controls/lib/frogpilot_acceleration.py index 0e2168183..28ba6a7f5 100644 --- a/frogpilot/controls/lib/frogpilot_acceleration.py +++ b/frogpilot/controls/lib/frogpilot_acceleration.py @@ -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)