From a8d95255d70c392e988a3b55ca1385547f2bb16a Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 30 Dec 2025 19:13:33 -0600 Subject: [PATCH] Use pedal braking limits in m/s breakpoints --- .../lib/longitudinal_mpc_lib/long_mpc.py | 7 ++++--- .../controls/lib/longitudinal_planner.py | 20 +++++++++++++++++++ 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a7ae314c8e2a41..64658ed642d8d4 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -495,8 +495,9 @@ def process_lead(self, lead, tracking_lead=True): 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 - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + # Clip lead distance to what is still possible to brake for using the current decel limit + decel_capable = max(-self.cruise_min_a, 0.1) + min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (decel_capable * 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.) @@ -527,7 +528,7 @@ def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, tracking_le 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] = ACCEL_MIN + self.params[:,0] = self.cruise_min_a # negative accel constraint causes problems because negative speed is not allowed self.params[:,1] = max(0.0, self.max_a) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c9fc80407dde38..ef582a9bb286c4 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -18,6 +18,18 @@ 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] @@ -54,6 +66,12 @@ 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) @@ -194,6 +212,7 @@ def update(self, tinygrad_model, sm, frogpilot_toggles): accel_coast = ACCEL_MAX v_ego = max(sm['carState'].vEgo, sm['carState'].vEgoCluster) + min_accel_limit = get_min_accel(self.CP, v_ego) v_cruise = sm['frogpilotPlan'].vCruise v_cruise_initialized = sm['controlsState'].vCruise != V_CRUISE_UNSET @@ -210,6 +229,7 @@ def update(self, tinygrad_model, sm, frogpilot_toggles): if self.mpc.mode == 'acc': accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration] + accel_limits[0] = max(accel_limits[0], min_accel_limit) steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg accel_limits_turns = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_limits, self.CP) else: