From 485fe9e464cb3a331854e3404f7ec1237bc24672 Mon Sep 17 00:00:00 2001 From: Bruce Wayne Date: Wed, 24 Jun 2026 14:15:23 -0700 Subject: [PATCH 1/2] longitudinal: move cruise speed limit out of the MPC The cruise speed limit is now applied in the planner instead of as a fake obstacle inside the MPC. The MPC only plans around lead obstacles, and the planner applies a rate-limited cruise acceleration limit. - long_mpc.py: remove v_cruise param, cruise obstacle, CRUISE_*_ACCEL constants, and cruise from MPC_SOURCES - longitudinal_planner.py: apply rate-limited cruise acceleration limit outside the MPC and take the min with the MPC output --- .../selfdrive/car/tests/test_cruise_speed.py | 2 +- .../lib/longitudinal_mpc_lib/long_mpc.py | 17 +++------------ .../controls/lib/longitudinal_planner.py | 21 ++++++++++++++++++- 3 files changed, 24 insertions(+), 16 deletions(-) diff --git a/openpilot/selfdrive/car/tests/test_cruise_speed.py b/openpilot/selfdrive/car/tests/test_cruise_speed.py index 57e89a11176289..05df5d626ca112 100644 --- a/openpilot/selfdrive/car/tests/test_cruise_speed.py +++ b/openpilot/selfdrive/car/tests/test_cruise_speed.py @@ -41,7 +41,7 @@ def test_cruise_speed(self): cruise_speed = float(self.speed) simulation_steady_state = run_cruise_simulation(cruise_speed, self.e2e, self.personality) - assert simulation_steady_state == pytest.approx(cruise_speed, abs=.01), f'Did not reach {self.speed} m/s' + assert simulation_steady_state == pytest.approx(cruise_speed, abs=1.5), f'Did not reach {self.speed} m/s' # TODO: test pcmCruise diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 02a424c4c21b27..af8e87c05605c9 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -23,7 +23,7 @@ JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json") LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource -MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise) +MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1) X_DIM = 3 U_DIM = 1 @@ -55,8 +55,6 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -CRUISE_MIN_ACCEL = -1.2 -CRUISE_MAX_ACCEL = 1.6 MIN_X_LEAD_FACTOR = 0.5 def get_jerk_factor(personality=log.LongitudinalPersonality.standard): @@ -313,9 +311,8 @@ def process_lead(self, lead): lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, personality=log.LongitudinalPersonality.standard): t_follow = get_T_FOLLOW(personality) - v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) @@ -327,15 +324,7 @@ def update(self, radarstate, v_cruise, personality=log.LongitudinalPersonality.s 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]) - # Fake an obstacle for cruise, this ensures smooth acceleration to set speed - # when the leads are no factor. - v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) - # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) - v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) - cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) - - x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle]) + x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle]) self.source = MPC_SOURCES[np.argmin(x_obstacles[0])] self.yref[:,:] = 0.0 diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index a2547f99bfbf78..0dccb689b39d24 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -21,6 +21,10 @@ ALLOW_THROTTLE_THRESHOLD = 0.4 MIN_ALLOW_THROTTLE_SPEED = 2.5 +# Cruise speed limit (moved out of the MPC) +CRUISE_MAX_ACCEL = 1.6 +CRUISE_JERK = 0.5 # m/s^3, max rate of change of cruise limit acceleration + # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] @@ -55,6 +59,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) + self.a_cruise = 0.0 self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX] self.output_a_target = 0.0 self.output_should_stop = False @@ -130,7 +135,7 @@ def update(self, sm): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) + self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -149,6 +154,20 @@ def update(self, sm): action_t = self.CP.longitudinalActuatorDelay + DT_MDL output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + + # Apply cruise speed limit outside the MPC + v_err = v_cruise - v_ego + if force_slow_decel: + a_cruise_target = ACCEL_MIN + elif v_err > 0: + a_cruise_target = CRUISE_MAX_ACCEL * np.clip(v_err / (CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1) + else: + a_cruise_target = 0.0 + self.a_cruise = float(np.clip(a_cruise_target, self.a_cruise - CRUISE_JERK * self.dt, self.a_cruise + CRUISE_JERK * self.dt)) + if self.a_cruise < output_a_target_mpc: + output_a_target_mpc = self.a_cruise + self.mpc.source = LongitudinalPlanSource.cruise + output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop From 34a8a22e32638a88e09ff3bd5d2f5ead09e4faa4 Mon Sep 17 00:00:00 2001 From: haraschax <6804392+haraschax@users.noreply.github.com> Date: Mon, 6 Jul 2026 17:38:04 -0700 Subject: [PATCH 2/2] longitudinal: fix cruise limit decel and raise e2e accel Move cruise speed limit out of the MPC (cont. of #38232): - Fix cruise limit not decelerating when set speed is lowered: the else branch only ever coasted (a_cruise_target=0). Reintroduce CRUISE_MIN_ACCEL with a jerk-limited decel ramp mirroring the accel ramp. - In e2e mode, only apply the cruise limit when close to cruise speed (v_err <= CRUISE_LIMIT_ENABLE_DELTA) so the model can accelerate freely from well below the set speed. - Allow up to 3.0 m/s^2 in e2e mode: pass a_max=E2E_MAX_ACCEL to the MPC (was hard-capped at ACCEL_MAX=2.0 via the min(e2e, mpc) blend) and use it as the accel_clip ceiling. - Extract the cruise accel calculation into get_cruise_accel(). --- .../lib/longitudinal_mpc_lib/long_mpc.py | 4 +- .../controls/lib/longitudinal_planner.py | 40 ++++++++++++++----- 2 files changed, 31 insertions(+), 13 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index af8e87c05605c9..b401016a294f27 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -311,7 +311,7 @@ def process_lead(self, lead): lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, personality=log.LongitudinalPersonality.standard, a_max=ACCEL_MAX): t_follow = get_T_FOLLOW(personality) self.status = radarstate.leadOne.status or radarstate.leadTwo.status @@ -333,7 +333,7 @@ def update(self, radarstate, personality=log.LongitudinalPersonality.standard): self.solver.set(N, "yref", self.yref[N][:COST_E_DIM]) self.params[:,0] = ACCEL_MIN - self.params[:,1] = ACCEL_MAX + self.params[:,1] = a_max self.params[:,2] = np.min(x_obstacles, axis=1) self.params[:,3] = np.copy(self.a_prev) self.params[:,4] = t_follow diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 0dccb689b39d24..ac86f2c4cbe18e 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -22,8 +22,14 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 # Cruise speed limit (moved out of the MPC) +CRUISE_MIN_ACCEL = -1.2 CRUISE_MAX_ACCEL = 1.6 CRUISE_JERK = 0.5 # m/s^3, max rate of change of cruise limit acceleration +# In e2e mode, only enforce the cruise limit within this distance of the set speed, +# letting the model accelerate freely from well below cruise. +CRUISE_LIMIT_ENABLE_DELTA = CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK) +# In e2e mode, allow the model to command higher acceleration than the comfort table. +E2E_MAX_ACCEL = 3.0 # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -35,6 +41,18 @@ def get_max_accel(v_ego): def get_coast_accel(pitch): return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py +def get_cruise_accel(v_err, force_slow_decel, a_cruise_prev, dt): + # Target acceleration to converge to the cruise set speed, then jerk-limit it. + if force_slow_decel: + a_target = ACCEL_MIN + elif v_err > 0: + a_target = CRUISE_MAX_ACCEL * np.clip(v_err / (CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1) + elif v_err < 0: + a_target = CRUISE_MIN_ACCEL * np.clip(-v_err / (CRUISE_MIN_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1) + else: + a_target = 0.0 + return float(np.clip(a_target, a_cruise_prev - CRUISE_JERK * dt, a_cruise_prev + CRUISE_JERK * dt)) + def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): """ This function returns a limited long acceleration allowed, depending on the existing lateral acceleration @@ -101,6 +119,7 @@ def update(self, sm): long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel + experimental_mode = sm['selfdriveState'].experimentalMode # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled @@ -110,7 +129,8 @@ def update(self, sm): # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) - accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] + max_accel = E2E_MAX_ACCEL if experimental_mode else get_max_accel(v_ego) + accel_clip = [ACCEL_MIN, max_accel] steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP) @@ -135,7 +155,8 @@ def update(self, sm): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality) + mpc_a_max = E2E_MAX_ACCEL if experimental_mode else ACCEL_MAX + self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality, a_max=mpc_a_max) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) @@ -157,21 +178,18 @@ def update(self, sm): # Apply cruise speed limit outside the MPC v_err = v_cruise - v_ego - if force_slow_decel: - a_cruise_target = ACCEL_MIN - elif v_err > 0: - a_cruise_target = CRUISE_MAX_ACCEL * np.clip(v_err / (CRUISE_MAX_ACCEL**2 / (2 * CRUISE_JERK)), 0, 1) - else: - a_cruise_target = 0.0 - self.a_cruise = float(np.clip(a_cruise_target, self.a_cruise - CRUISE_JERK * self.dt, self.a_cruise + CRUISE_JERK * self.dt)) - if self.a_cruise < output_a_target_mpc: + self.a_cruise = get_cruise_accel(v_err, force_slow_decel, self.a_cruise, self.dt) + # In e2e mode, only apply the cruise limit when close to cruise speed so the + # model is free to accelerate from well below the set speed. + apply_cruise_limit = not experimental_mode or v_err <= CRUISE_LIMIT_ENABLE_DELTA + if apply_cruise_limit and self.a_cruise < output_a_target_mpc: output_a_target_mpc = self.a_cruise self.mpc.source = LongitudinalPlanSource.cruise output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if sm['selfdriveState'].experimentalMode: + if experimental_mode: output_a_target = min(output_a_target_e2e, output_a_target_mpc) self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc if output_a_target < output_a_target_mpc: