diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9def174f0..b54420ffb 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5c88f3c2b..e5da54aeb 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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