From 5d0edc8cbbac38a6f83bc5bc77bad157bce0e40f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 3 Jan 2026 01:41:54 -0600 Subject: [PATCH] Revert "Planner Pedal Calculation" This reverts commit 60ef10bd32300f96a89715b786aa1f1acc40036e. --- .../lib/longitudinal_mpc_lib/long_mpc.py | 23 ++++-------- .../controls/lib/longitudinal_planner.py | 35 ------------------- 2 files changed, 7 insertions(+), 51 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 680b7fb3c..a7ae314c8 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -310,9 +310,8 @@ class LongitudinalMpc: self.current_j_ego_cost = J_EGO_COSTS[0] self.current_a_change_cost = A_CHANGE_COSTS[0] self.current_dist_adapt = DIST_ADAPTS[0] + # Initialize acceleration limits to prevent AttributeError self.cruise_min_a = ACCEL_MIN - self.expected_min_a = ACCEL_MIN - self.expected_min_a_profile = np.full(N+1, ACCEL_MIN) self.max_a = 1.2 # Default max acceleration self.reset() @@ -333,7 +332,6 @@ class LongitudinalMpc: self.params = np.zeros((N+1, PARAM_DIM)) for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) - self.expected_min_a_profile = np.full(N+1, self.expected_min_a) self.last_cloudlog_t = 0 self.status = False self.crash_cnt = 0.0 @@ -497,9 +495,8 @@ class LongitudinalMpc: a_lead_tau = LEAD_ACCEL_TAU # MPC will not converge if immediate crash is expected - # Clip lead distance to what is still possible to brake for using the current decel limit - decel_capable = max(-self.expected_min_a, 0.1) - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (decel_capable * 2) + # Clip lead distance to what is still possible to brake for + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) x_lead = clip(x_lead, min_x_lead, 1e8) v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) @@ -511,13 +508,6 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau, v_ego) return lead_xv - def set_expected_min_accel(self, min_a, profile=None): - self.expected_min_a = min_a - if profile is not None and len(profile) == N+1: - self.expected_min_a_profile = np.asarray(profile, dtype=float) - else: - self.expected_min_a_profile = np.full(N+1, min_a) - def set_accel_limits(self, min_a, max_a): # TODO this sets a max accel limit, but the minimum limit is only for cruise decel # needs refactor @@ -537,7 +527,7 @@ class LongitudinalMpc: lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1]) lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) - self.params[:,0] = self.cruise_min_a + self.params[:,0] = ACCEL_MIN # negative accel constraint causes problems because negative speed is not allowed self.params[:,1] = max(0.0, self.max_a) @@ -545,8 +535,9 @@ class LongitudinalMpc: if self.mode == 'acc': self.params[:,5] = LEAD_DANGER_FACTOR - v_lower = v_ego + np.cumsum(T_DIFFS * self.expected_min_a_profile * 1.05) - v_lower = np.clip(v_lower, 0.0, 1e8) + # Fake an obstacle for cruise, this ensures smooth acceleration to set speed + # when the leads are no factor. + v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0f1aa175c..c9fc80407 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -18,18 +18,6 @@ from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.0 -PEDAL_DECEL_BP = [ - 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, - 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, - 30, 31, 32, 33, 34, -] -PEDAL_DECEL_V = [ - -1.379, -1.855, -2.167, -2.311, -2.411, -2.455, -2.493, -2.500, -2.500, - -2.530, -2.366, -2.151, -1.955, -1.839, -1.777, -1.741, - -1.705, -1.670, -1.652, -1.661, -1.679, -1.696, -1.696, -1.679, - -1.652, -1.634, -1.652, -1.670, -1.696, -1.696, -1.696, -1.598, - -1.518, -1.471, -1.4, -] A_CRUISE_MAX_BP = [0.0, 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS = [1.125, 1.125, 1.125, 1.125, 1.25, 1.25, 1.5] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -66,12 +54,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): return [a_target[0], min(a_target[1], a_x_allowed)] -def get_min_accel(CP, v_ego): - if CP.enableGasInterceptor: - return float(interp(v_ego, PEDAL_DECEL_BP, PEDAL_DECEL_V)) - return A_CRUISE_MIN - - def get_accel_from_plan_classic(CP, speeds, accels, vEgoStopping): if len(speeds) == CONTROL_N: v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds) @@ -196,20 +178,6 @@ class LongitudinalPlanner: throttle_prob = 1.0 return x, v, a, j, throttle_prob - def _expected_decel_profile(self, v_start): - t_diffs = np.diff(T_IDXS_MPC, prepend=[0.0]) - profile = np.zeros_like(T_IDXS_MPC) - v = float(v_start) - for i in range(len(T_IDXS_MPC)): - a_exp = get_min_accel(self.CP, v) - speed_mph = v * CV.MS_TO_MPH - factor = interp(speed_mph, [0.0, 30.0, 300.0], [0.75, 0.75, 0.9]) - a_scaled = a_exp * factor - profile[i] = a_scaled - if i < len(T_IDXS_MPC) - 1: - v = max(0.0, v + a_scaled * t_diffs[i + 1]) - return profile - def update(self, tinygrad_model, sm, frogpilot_toggles): self.generation = frogpilot_toggles.model_version if tinygrad_model: @@ -226,8 +194,6 @@ class LongitudinalPlanner: accel_coast = ACCEL_MAX v_ego = max(sm['carState'].vEgo, sm['carState'].vEgoCluster) - expected_min_accel_profile = self._expected_decel_profile(v_ego) - expected_min_accel = float(expected_min_accel_profile[0]) v_cruise = sm['frogpilotPlan'].vCruise v_cruise_initialized = sm['controlsState'].vCruise != V_CRUISE_UNSET @@ -431,7 +397,6 @@ class LongitudinalPlanner: uncertainty=uncertainty, accel_reengage=self.accel_gate, panic_bypass=panic_bypass) - self.mpc.set_expected_min_accel(expected_min_accel, expected_min_accel_profile) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) # After deciding the MPC mode via get_mpc_mode(), ensure MPC uses that mode when not mlsim