v3

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -150,18 +150,7 @@ def get_stopped_equivalence_factor(v_lead):
 def get_safe_obstacle_distance(v_ego, t_follow):
   from openpilot.common.params import Params
   params = Params()
-  stop_str = None
-  try:
-    stop_str = params.get("StopDistance", encoding="utf8")
-  except TypeError:
-    # Compatibility with older params_pyx signatures that do not support encoding kwarg.
-    try:
-      raw = params.get("StopDistance")
-      stop_str = raw.decode("utf8") if isinstance(raw, (bytes, bytearray)) else raw
-    except Exception:
-      stop_str = None
-  except Exception:
-    stop_str = None
+  stop_str = params.get("StopDistance", encoding="utf8")
   stop_distance = float(stop_str) if stop_str else 6.0
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance
 
@@ -308,10 +297,13 @@ class LongitudinalMpc:
     self.dt = dt
     self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
     self.source = SOURCES[2]
-    # Keep a fixed lead filter time; disable speed/uncertainty follow-smoothing modulation.
+    # Initialize smoothing filters with default time constants
     self.current_filter_time = LEAD_FILTER_TIME_LOW
     self.lead_a_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
     self.lead_v_filter = FirstOrderFilter(0.0, self.current_filter_time, self.dt)
+    # Slew-limited filter factor to avoid abrupt 0.50↔1.00 jumps
+    self.filter_time_factor = 1.0
+    self.slew_per_sec = 1.0
     # Instance variables to avoid global modifications
     self.current_x_ego_cost = X_EGO_OBSTACLE_COSTS[0]
     self.current_j_ego_cost = J_EGO_COSTS[0]
@@ -370,7 +362,6 @@ class LongitudinalMpc:
   def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True,
                   personality=log.LongitudinalPersonality.standard, v_ego=0.0, lead_dist=50.0,
                   uncertainty=0.0, accel_reengage=False, panic_bypass=False):
-    _ = uncertainty, accel_reengage, panic_bypass  # compatibility args (follow-smoothing path removed)
     # Update parameters based on current speed with interpolation for smooth scaling
     speed_mph = v_ego * CV.MS_TO_MPH  # Convert m/s to mph
 
@@ -383,12 +374,53 @@ class LongitudinalMpc:
     dist_adapt_array = [0.0, DIST_ADAPTS[1], DIST_ADAPTS[2], DIST_ADAPTS[3]]
     self.current_dist_adapt = get_speed_based_param(speed_mph, dist_adapt_array)
 
+    # Update filter time constants with interp and recreate filters if needed
+    if speed_mph < 47:
+        self.current_filter_time = 0.0
+    else:
+        self.current_filter_time = interp(speed_mph, [47, 65], [0.0, LEAD_FILTER_TIME_HIGH])
+    if abs(self.current_filter_time - getattr(self, 'prev_filter_time', 0)) > 0.1:  # Only update if significant change
+      # Recreate filters with new time constant while preserving current values
+      current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0
+      current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0
+      self.lead_a_filter = FirstOrderFilter(current_a, self.current_filter_time, self.dt)
+      self.lead_v_filter = FirstOrderFilter(current_v, self.current_filter_time, self.dt)
+      self.prev_filter_time = self.current_filter_time
+
     # Adaptive jerk factors for distance with interp scaling
     dist_factor = 1.0 + self.current_dist_adapt * (20.0 / max(lead_dist, 5.0))
     acceleration_jerk *= dist_factor
     danger_jerk *= dist_factor
     speed_jerk *= dist_factor
 
+    # Scene complexity adjustment based on model uncertainty
+    prev_filter_time_factor = getattr(self, 'prev_filter_time_factor', 1.0)
+    # Target factor from uncertainty
+    if uncertainty <= 0.45:
+      tgt_factor = 1.0
+    elif uncertainty >= 0.70:
+      tgt_factor = 0.0
+    else:
+      tgt_factor = float(np.interp(uncertainty, [0.45, 0.70], [1.0, 0.30]))
+
+    if accel_reengage:
+      tgt_factor = min(tgt_factor, 0.5)
+
+    # Hard bypass of smoothing when approaching fast or magnitude trips
+    if panic_bypass:
+      tgt_factor = 0.0
+
+    # Slew-limit changes to avoid step-wise filter jumps
+    max_step = self.slew_per_sec * self.dt
+    delta = np.clip(tgt_factor - self.filter_time_factor, -max_step, max_step)
+    self.filter_time_factor += float(delta)
+    filter_time_factor = float(self.filter_time_factor)
+
+    # When uncertainty is moderately elevated, allow accel but cap jerk by increasing jerk cost
+    if 0.45 <= uncertainty < 0.60:
+      scale = float(np.interp(uncertainty, [0.45, 0.60], [1.2, 1.5]))
+      speed_jerk *= scale
+
     if self.mode == 'acc':
       a_change_cost = acceleration_jerk if prev_accel_constraint else 0
       cost_weights = [self.current_x_ego_cost, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk]
@@ -401,6 +433,15 @@ class LongitudinalMpc:
       raise NotImplementedError(f'Planner mode {self.mode} not recognized in planner cost set')
     self.set_cost_weights(cost_weights, constraint_cost_weights)
 
+    # Adjust filter time constants for complex scenes
+    if abs(filter_time_factor - getattr(self, 'prev_filter_time_factor', 1.0)) > 0.05:
+      current_a = self.lead_a_filter.x if hasattr(self.lead_a_filter, 'x') else 0.0
+      current_v = self.lead_v_filter.x if hasattr(self.lead_v_filter, 'x') else 0.0
+      new_filter_time = self.current_filter_time * filter_time_factor
+      self.lead_a_filter = FirstOrderFilter(current_a, new_filter_time, self.dt)
+      self.lead_v_filter = FirstOrderFilter(current_v, new_filter_time, self.dt)
+      self.prev_filter_time_factor = filter_time_factor
+
   def set_cur_state(self, v, a):
     v_prev = self.x0[1]
     self.x0[1] = v
@@ -450,10 +491,8 @@ class LongitudinalMpc:
       a_lead_tau = LEAD_ACCEL_TAU
 
     # MPC will not converge if immediate crash is expected
-    # Clip lead distance using the currently active vehicle decel capability.
-    # This keeps MPC safety math aligned with per-car/per-speed braking limits.
-    min_decel = min(float(self.cruise_min_a), -0.1)
-    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-min_decel * 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.)
@@ -473,11 +512,10 @@ class LongitudinalMpc:
 
   def update(self, lead_one, lead_two, v_cruise, x, v, a, j, t_follow, tracking_lead, personality=log.LongitudinalPersonality.standard):
     v_ego = self.x0[1]
-    self.status = lead_one.status or lead_two.status
+    self.status = lead_one.status and tracking_lead or lead_two.status
 
-    # Always process valid leads for safety; trackingLead can still be used by higher-level logic/UI.
-    lead_xv_0 = self.process_lead(lead_one, lead_one.status)
-    lead_xv_1 = self.process_lead(lead_two, lead_two.status)
+    lead_xv_0 = self.process_lead(lead_one, tracking_lead)
+    lead_xv_1 = self.process_lead(lead_two, v_ego)
 
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
@@ -485,9 +523,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])
 
-    # Apply the live min-accel envelope from planner/car interface rather than
-    # a single global constant (important for regen-limited low-speed behavior).
-    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)
 

selfdrive/controls/lib/longitudinal_planner.py

@@ -23,9 +23,11 @@ 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]
 ALLOW_THROTTLE_THRESHOLD = 0.4
 MIN_ALLOW_THROTTLE_SPEED = 2.5
-COMFORT_BRAKE_MPS2 = 2.5
 
 # Uncertainty-based filter disable thresholds
+UNCERT_SLOPE_TRIG = 0.12  # per second
+UNCERT_MAG_TRIG = 0.50
+
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
 _A_TOTAL_MAX_BP = [20., 40.]
@@ -52,30 +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_vehicle_min_accel(CP, v_ego):
-  # Planner-side physical decel capability estimate used for safety bounds.
-  # Keep this aligned with GM pedal-long limits used by car interface.
-  if getattr(CP, "carName", "") == "gm" and getattr(CP, "enableGasInterceptor", False):
-    try:
-      from openpilot.selfdrive.car.gm.values import GMFlags, CAR
-      if bool(CP.flags & GMFlags.PEDAL_LONG.value):
-        bolt_pedal_long_cars = {
-          CAR.CHEVROLET_BOLT_CC_2017,
-          CAR.CHEVROLET_BOLT_CC_2019_2021,
-          CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL,
-          CAR.CHEVROLET_BOLT_CC_2022_2023,
-          CAR.CHEVROLET_MALIBU_HYBRID_CC,
-        }
-        if CP.carFingerprint in bolt_pedal_long_cars:
-          return float(interp(v_ego, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0],
-                              [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]))
-        return float(interp(v_ego, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0],
-                            [-0.95, -1.3, -1.85, -2.3, -2.6, -2.8]))
-    except Exception:
-      pass
-  return float(ACCEL_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)
@@ -141,13 +119,13 @@ class LongitudinalPlanner:
     # Lead stability tracking
     self.prev_lead_dist = None
     self.last_big_brake_t = 0.0
-    self.last_lead_brake_cmd_t = 0.0
     self.stable_lead = False
     # Smoothed lead distance
     self.lead_dist_f = None
-    self.last_safety_log_t = 0.0
 
     # Uncertainty slope tracking
+    self._uncert_last = 0.0
+    self._uncert_last_t = None
 
   @property
   def mlsim(self):
@@ -225,41 +203,6 @@ class LongitudinalPlanner:
       accel_limits = [sm['frogpilotPlan'].minAcceleration, sm['frogpilotPlan'].maxAcceleration]
       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)
-      vehicle_min_accel = get_vehicle_min_accel(self.CP, v_ego)
-
-      # Safety override: keep profile comfort limits, but increase available braking
-      # when lead-closing risk rises so chill profiles cannot under-brake.
-      lead_one = sm['radarState'].leadOne
-      if lead_one.status:
-        lead_dist = float(lead_one.dRel)
-        rel_v = max(0.0, v_ego - float(lead_one.vLead))
-        ttc = lead_dist / max(rel_v, 0.1) if rel_v > 0.1 else 1e6
-        desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
-
-        floor_ttc = interp(ttc, [1.6, 2.8, 4.0, 6.0, 10.0],
-                           [vehicle_min_accel, -2.6, -1.8, -1.2, accel_limits_turns[0]])
-        floor_rel_v = interp(rel_v, [0.0, 1.0, 2.5, 5.0, 8.0],
-                             [accel_limits_turns[0], -1.1, -1.7, -2.5, vehicle_min_accel])
-        gap_shortfall = max(0.0, desired_gap - lead_dist)
-        floor_gap = interp(gap_shortfall, [0.0, 2.0, 5.0, 9.0],
-                           [accel_limits_turns[0], -1.2, -2.0, -2.8])
-
-        # Approaching a near-stationary lead close to the stopping envelope:
-        # disallow positive accel and bias toward stronger decel in the final meters.
-        if float(lead_one.vLead) < 1.0:
-          stopped_lead_req_dist = (v_ego ** 2) / (2 * COMFORT_BRAKE_MPS2) + desired_gap
-          no_accel_margin = interp(v_ego, [0.0, 8.0, 15.0, 25.0, 35.0], [2.0, 3.5, 6.0, 9.0, 12.0])
-          if lead_dist < (stopped_lead_req_dist + no_accel_margin):
-            accel_limits_turns[1] = min(accel_limits_turns[1], 0.0)
-
-          floor_stopped_lead = interp(lead_dist, [0.4, 0.8, 1.5, 3.0, 6.0, 12.0],
-                                      [vehicle_min_accel, -2.4, -2.0, -1.5, -1.0, accel_limits_turns[0]])
-          floor_ttc = min(floor_ttc, floor_stopped_lead)
-
-        safety_floor = min(accel_limits_turns[0], floor_ttc, floor_rel_v, floor_gap)
-        accel_limits_turns[0] = max(vehicle_min_accel, safety_floor)
-      else:
-        accel_limits_turns[0] = max(vehicle_min_accel, accel_limits_turns[0])
     else:
       accel_limits = [ACCEL_MIN, ACCEL_MAX]
       accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]
@@ -268,7 +211,6 @@ class LongitudinalPlanner:
       self.v_desired_filter.x = v_ego
       # Clip aEgo to cruise limits to prevent large accelerations when becoming active
       self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1])
-      self.last_lead_brake_cmd_t = 0.0
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -295,14 +237,8 @@ class LongitudinalPlanner:
 
     lead_dist = self.lead_one.dRel if self.lead_one.status else 50.0
 
-    # Keep only light smoothing on lead distance so ACC reacts quickly like stock.
-    closing_speed = max(0.0, v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0
-    opening_speed = max(0.0, self.lead_one.vLead - v_ego) if self.lead_one.status else 0.0
-    alpha = interp(v_ego, [0.0, 8.0, 15.0, 25.0, 35.0], [0.22, 0.28, 0.34, 0.42, 0.48])
-    if closing_speed > 0.8:
-      alpha = max(alpha, interp(closing_speed, [0.8, 2.0, 4.0], [0.48, 0.58, 0.66]))
-    elif opening_speed > 1.0:
-      alpha = min(alpha, interp(opening_speed, [1.0, 2.5, 4.0], [alpha, 0.22, 0.18]))
+    # Smooth lead distance (EMA) to avoid chatter in thresholds
+    alpha = max(0.02, min(0.15, 0.05 + 0.002 * v_ego))
     if self.lead_dist_f is None:
       self.lead_dist_f = float(lead_dist)
     else:
@@ -378,34 +314,34 @@ class LongitudinalPlanner:
       uncertainty = self.uncert_slow.x
     uncertainty_accel = min(self.uncert_slow.x, self.uncert_fast.x)
 
-    accel_jerk_w = sm['frogpilotPlan'].accelerationJerk
-    danger_jerk_w = sm['frogpilotPlan'].dangerJerk
-    speed_jerk_w = sm['frogpilotPlan'].speedJerk
+    # --- Slope-based panic bypass ---
+    if self._uncert_last_t is None:
+      uncert_slope = 0.0
+    else:
+      dt_u = max(1e-3, now_t - self._uncert_last_t)
+      uncert_slope = (uncertainty - self._uncert_last) / dt_u
+    self._uncert_last = uncertainty
+    self._uncert_last_t = now_t
 
-    # In stable, low-risk car-following, increase smoothing to reduce rubberbanding.
-    if self.lead_one.status and self.stable_lead:
-      lead_dist_used = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel
-      desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
-      gap_err = abs(lead_dist_used - desired_gap)
-      rel_v_abs = abs(v_ego - self.lead_one.vLead)
-      closing_v = max(0.0, v_ego - self.lead_one.vLead)
-      ttc = lead_dist_used / max(closing_v, 0.1) if closing_v > 0.1 else 1e6
+    closing_fast = (self.lead_one.status and (v_ego - self.lead_one.vLead) > 0.5)
+    # Trigger if either slope is high or magnitude is high; require a valid lead and closing
+    panic_bypass = closing_fast and (uncert_slope > UNCERT_SLOPE_TRIG or uncertainty >= UNCERT_MAG_TRIG)
 
-      gap_ok = gap_err < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.0, 2.0, 3.5, 5.0])
-      rel_v_ok = rel_v_abs < interp(v_ego, [0.0, 10.0, 20.0, 35.0], [0.30, 0.60, 0.90, 1.20])
-      low_risk = (ttc > 3.0) and gap_ok and rel_v_ok
-      if low_risk:
-        accel_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.08, 1.18, 1.26])
-        speed_jerk_w *= interp(v_ego, [0.0, 10.0, 20.0, 35.0], [1.00, 1.04, 1.10, 1.16])
+    if panic_bypass:
+      try:
+        cloudlog.error(f"LON_SLOPE; slope={uncert_slope:.3f}/s; uncertainty={uncertainty:.3f}; v_ego={v_ego:.2f}; v_rel={(v_ego - self.lead_one.vLead) if self.lead_one.status else 0.0:.2f}; lead_dist={self.lead_dist_f if self.lead_dist_f is not None else -1:.2f}; trigger=True")
+      except Exception:
+        pass
 
-    self.mpc.set_weights(accel_jerk_w,
-                         danger_jerk_w,
-                         speed_jerk_w,
+    self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk,
+                         sm['frogpilotPlan'].dangerJerk,
+                         sm['frogpilotPlan'].speedJerk,
                          prev_accel_constraint,
                          personality=sm['controlsState'].personality,
                          v_ego=v_ego,
                          lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist,
-                         uncertainty=uncertainty)
+                         uncertainty=uncertainty,
+                         panic_bypass=panic_bypass)
     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
@@ -428,14 +364,10 @@ class LongitudinalPlanner:
     # Safety checks for rubber-banding mitigation
     max_jerk = np.max(np.abs(self.mpc.j_solution))
     max_accel_change = np.max(np.abs(np.diff(self.mpc.a_solution)))
-    now_t = time.monotonic()
-    if now_t - self.last_safety_log_t > 2.0:
-      if max_jerk > 5.0:  # m/s^3
-        cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3")
-        self.last_safety_log_t = now_t
-      if max_accel_change > 2.0:  # m/s^2
-        cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2")
-        self.last_safety_log_t = now_t
+    if max_jerk > 5.0:  # m/s^3
+      cloudlog.warning(f"High jerk detected: {max_jerk:.2f} m/s^3")
+    if max_accel_change > 2.0:  # m/s^2
+      cloudlog.warning(f"High acceleration change: {max_accel_change:.2f} m/s^2")
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
@@ -445,36 +377,16 @@ class LongitudinalPlanner:
     # Anticipatory pre-brake to avoid "coming in hot" when closing on a lead
     if self.lead_one.status:
       rel_v = max(0.0, v_ego - self.lead_one.vLead)
-      lead_dist_f = self.lead_dist_f if self.lead_dist_f is not None else self.lead_one.dRel
-      ttc = lead_dist_f / max(rel_v, 0.1) if rel_v > 0.1 else 1e6
-      desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0
-      gap_shortfall = max(0.0, desired_gap - lead_dist_f)
-
-      pre_brake_dist_trigger = desired_gap + interp(v_ego, [0.0, 10.0, 20.0, 30.0], [5.0, 5.8, 6.8, 8.0])
-      if rel_v > 0.5 and lead_dist_f < pre_brake_dist_trigger:
-        pre_brake = 0.0
-        pre_brake += interp(rel_v, [0.5, 2.0, 5.0, 8.0], [0.0, 0.02, 0.06, 0.11])
-        pre_brake += interp(ttc, [1.4, 2.2, 3.5, 5.0, 7.5], [0.16, 0.09, 0.04, 0.01, 0.0])
-        pre_brake += interp(gap_shortfall, [0.0, 2.0, 6.0, 10.0], [0.0, 0.015, 0.04, 0.07])
-        pre_brake += 0.10 * max(0.0, uncertainty - 0.35)
-        # Mild low-speed soften to avoid excess early braking while retaining high-speed safety.
-        pre_brake *= interp(v_ego, [0.0, 8.0, 15.0, 25.0], [0.50, 0.68, 0.88, 1.00])
-        pre_brake = min(pre_brake, interp(v_ego, [0.0, 5.0, 15.0, 30.0], [0.05, 0.08, 0.13, 0.16]))
+      # dynamic time headway adds a small buffer when uncertainty is elevated
+      base_th = 1.6
+      th = base_th + 0.6 * max(0.0, uncertainty - 0.42)
+      desired_gap = th * v_ego
+      if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5):
+        k_rel, k_unc = 0.04, 0.20
+        pre_brake = k_rel * rel_v + k_unc * max(0.0, uncertainty - 0.42)
+        pre_brake = min(pre_brake, 0.06)
         self.a_desired = float(self.a_desired - pre_brake)
 
-      # Shape accel release after low-speed lead-brake events to reduce stop-and-go brake->surge snapback.
-      if v_ego < 8.0 and rel_v > 0.2 and lead_dist_f < desired_gap + 2.5 and self.a_desired < -0.35:
-        self.last_lead_brake_cmd_t = now_t
-
-      t_since_brake = now_t - self.last_lead_brake_cmd_t
-      release_window = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.6, 0.7, 0.8, 0.9])
-      low_risk_release = ttc > 2.0 and rel_v < interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.3, 0.45, 0.6, 0.75])
-      near_lead = lead_dist_f < desired_gap + 2.0
-      if 0.0 < t_since_brake < release_window and v_ego < 8.0 and near_lead and low_risk_release and self.a_desired > -0.05:
-        release_cap_t = interp(t_since_brake, [0.0, 0.15, 0.35, 0.60, release_window], [0.05, 0.14, 0.24, 0.34, 0.48])
-        release_cap_v = interp(v_ego, [0.0, 3.0, 6.0, 8.0], [0.15, 0.24, 0.34, 0.42])
-        self.a_desired = float(min(self.a_desired, min(release_cap_t, release_cap_v)))
-
     # Small deadzone around zero accel to kill micro-dithers
     if -0.05 < self.a_desired < 0.05:
       self.a_desired = 0.0