mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-08 07:02:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

LongPlanV2

Browse Source
This commit is contained in:
firestar5683
2026-03-11 15:18:04 -05:00
parent 79e28d6f2e
commit fc79fe1248
2 changed files with 62 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+19 -4
View File
@@ -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)
selfdrive/controls/lib/longitudinal_planner.py
+43 -3
View File
@@ -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]