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

planner bounds

Browse Source
This commit is contained in:
firestar5683
2026-03-31 11:31:40 -05:00
parent 22669da61c
commit 95107573aa
2 changed files with 28 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+4 -3
View File
@@ -498,8 +498,9 @@ 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.
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 = np.clip(x_lead, min_x_lead, 1e8)
v_lead = np.clip(v_lead, 0.0, 1e8)
a_lead = np.clip(a_lead, -10., 5.)
@@ -533,7 +534,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])
self.params[:,0] = ACCEL_MIN
self.params[:,0] = self.cruise_min_a
self.params[:,1] = max(0.0, self.max_a)
# Update in ACC mode or ACC/e2e blend
selfdrive/controls/lib/longitudinal_planner.py
+24
View File
@@ -64,6 +64,29 @@ 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 for GM pedal-long paths.
if getattr(CP, "carName", "") == "gm" and getattr(CP, "enableGasInterceptorDEPRECATED", False):
try:
from opendbc.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_2018_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(np.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(np.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 = np.interp(DT_MDL, CONTROL_N_T_IDX, speeds)
@@ -215,6 +238,7 @@ class LongitudinalPlanner:
accel_limits = [sm['starpilotPlan'].minAcceleration, sm['starpilotPlan'].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)
accel_limits_turns[0] = max(get_vehicle_min_accel(self.CP, v_ego), accel_limits_turns[0])
else:
accel_limits = [ACCEL_MIN, ACCEL_MAX]
accel_limits_turns = [ACCEL_MIN, ACCEL_MAX]