From fc79fe12487b3b1cde93c8495825110d96d1578b Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 11 Mar 2026 15:18:04 -0500 Subject: [PATCH] LongPlanV2 --- .../lib/longitudinal_mpc_lib/long_mpc.py | 23 ++++++++-- .../controls/lib/longitudinal_planner.py | 46 +++++++++++++++++-- 2 files changed, 62 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index cc409f8fb..9def174f0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -150,7 +150,18 @@ 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 = params.get("StopDistance", encoding="utf8") + 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_distance = float(stop_str) if stop_str else 6.0 return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance @@ -439,8 +450,10 @@ 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 - min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-ACCEL_MIN * 2) + # 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) x_lead = clip(x_lead, min_x_lead, 1e8) v_lead = clip(v_lead, 0.0, 1e8) a_lead = clip(a_lead, -10., 5.) @@ -472,7 +485,9 @@ 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] = ACCEL_MIN + # 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 # 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 0708c07ab..5c88f3c2b 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -23,6 +23,7 @@ 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 # Lookup table for turns @@ -51,6 +52,30 @@ 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) @@ -200,6 +225,7 @@ 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. @@ -211,15 +237,29 @@ class LongitudinalPlanner: desired_gap = sm['frogpilotPlan'].tFollow * v_ego + 6.0 floor_ttc = interp(ttc, [1.6, 2.8, 4.0, 6.0, 10.0], - [ACCEL_MIN, -2.6, -1.8, -1.2, accel_limits_turns[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, ACCEL_MIN]) + [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(ACCEL_MIN, safety_floor) + 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]