IQ.Lvbs/IQ.Pilot
8
7
Fork 3
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Release Commit @ f82ff4d

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 13:43:46 -05:00
parent 7b20edda67
commit b712a31728
717 changed files with 4347 additions and 3060 deletions
Download Patch File Download Diff File Expand all files Collapse all files

55
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -3,7 +3,7 @@ import os
import time
import numpy as np
from cereal import log
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.realtime import DT_MDL
from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
@@ -26,15 +26,12 @@ EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
JSON_FILE = os.path.join(LONG_MPC_DIR, "acados_ocp_long.json")
LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1, LongitudinalPlanSource.cruise)
CRUISE_MIN_ACCEL = -1.2
CRUISE_MAX_ACCEL = 1.6
MPC_SOURCES = (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
X_DIM = 3
U_DIM = 1
PARAM_DIM = 6
COST_E_DIM = 5
PARAM_DIM = 5
COST_E_DIM = 4
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
@@ -42,8 +39,7 @@ X_EGO_OBSTACLE_COST = 3.
X_EGO_COST = 0.
V_EGO_COST = 0.
A_EGO_COST = 0.
J_EGO_COST = 5.
A_CHANGE_COST = 200.
J_EGO_COST = 20.
DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .25
LEAD_DANGER_FACTOR = 0.75
@@ -114,10 +110,9 @@ def gen_long_model():
a_min = SX.sym('a_min')
a_max = SX.sym('a_max')
x_obstacle = SX.sym('x_obstacle')
prev_a = SX.sym('prev_a')
lead_t_follow = SX.sym('lead_t_follow')
lead_danger_factor = SX.sym('lead_danger_factor')
model.p = vertcat(a_min, a_max, x_obstacle, prev_a, lead_t_follow, lead_danger_factor)
model.p = vertcat(a_min, a_max, x_obstacle, lead_t_follow, lead_danger_factor)
# dynamics model
f_expl = vertcat(v_ego, a_ego, j_ego)
@@ -149,9 +144,8 @@ def gen_long_ocp():
a_min, a_max = ocp.model.p[0], ocp.model.p[1]
x_obstacle = ocp.model.p[2]
prev_a = ocp.model.p[3]
lead_t_follow = ocp.model.p[4]
lead_danger_factor = ocp.model.p[5]
lead_t_follow = ocp.model.p[3]
lead_danger_factor = ocp.model.p[4]
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
@@ -166,7 +160,6 @@ def gen_long_ocp():
x_ego,
v_ego,
a_ego,
a_ego - prev_a,
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
@@ -182,7 +175,7 @@ def gen_long_ocp():
x0 = np.zeros(X_DIM)
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
ocp.parameter_values = np.array([-1.2, 1.2, 0.0, get_T_FOLLOW(), LEAD_DANGER_FACTOR])
# We put all constraint cost weights to 0 and only set them at runtime
@@ -242,7 +235,6 @@ class LongitudinalMpc:
self.v_solution = np.zeros(N+1)
self.a_solution = np.zeros(N+1)
self.j_solution = np.zeros(N)
self.prev_a = np.array(self.a_solution)
self.yref = np.zeros((N+1, COST_DIM))
for i in range(N):
@@ -270,9 +262,6 @@ class LongitudinalMpc:
def set_cost_weights(self, cost_weights, constraint_cost_weights):
W = np.asfortranarray(np.diag(cost_weights))
for i in range(N):
# TODO don't hardcode A_CHANGE_COST idx
# reduce the cost on (a-a_prev) later in the horizon.
W[4,4] = cost_weights[4] * np.interp(T_IDXS[i], [0.0, 1.0, 2.0], [1.0, 1.0, 0.0])
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
@@ -283,10 +272,9 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
def set_weights(self, personality=log.LongitudinalPersonality.standard):
jerk_factor = get_jerk_factor(personality)
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
self.set_cost_weights(cost_weights, constraint_cost_weights)
@@ -361,8 +349,7 @@ class LongitudinalMpc:
x_lead_mpc = np.maximum(x_lead_mpc, radar_distance_floor)
return np.column_stack((x_lead_mpc, v_lead_mpc))
def update(self, modelV2, radarstate, v_cruise, cruise_accel_limits=(CRUISE_MIN_ACCEL, CRUISE_MAX_ACCEL),
personality=log.LongitudinalPersonality.standard):
def update(self, modelV2, radarstate, personality=log.LongitudinalPersonality.standard):
self.new_lead_mpc = self._read_new_lead_mpc()
t_follow = get_T_FOLLOW(personality)
model_leads = modelV2.leadsV3
@@ -386,16 +373,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])
# v_cruise: scalar or per-timestep (N+1) envelope
v_ego = self.x0[1]
min_a, max_a = cruise_accel_limits
v_lower = np.maximum(v_ego + (T_IDXS * min(min_a, -0.1) * 1.05), 0.0)
# max_a can be negative (coast/turn clip); the reachable band must stay ordered
v_upper = np.maximum(v_ego + (T_IDXS * max_a * 1.05), v_lower)
v_cruise_clipped = np.clip(np.broadcast_to(v_cruise, (N+1,)), v_lower, v_upper)
cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle])
self.source = MPC_SOURCES[np.argmin(x_obstacles[0])]
self.yref[:,:] = 0.0
@@ -406,9 +384,8 @@ class LongitudinalMpc:
self.params[:,0] = ACCEL_MIN
self.params[:,1] = ACCEL_MAX
self.params[:,2] = np.min(x_obstacles, axis=1)
self.params[:,3] = np.copy(self.prev_a)
self.params[:,4] = t_follow
self.params[:,5] = LEAD_DANGER_FACTOR
self.params[:,3] = t_follow
self.params[:,4] = LEAD_DANGER_FACTOR
self.run()
lead_crash_prob = model_leads[0].prob if self.new_lead_mpc else radarstate.leadOne.modelProb
@@ -439,8 +416,6 @@ class LongitudinalMpc:
self.a_solution = self.x_sol[:,2]
self.j_solution = self.u_sol[:,0]
self.prev_a = np.interp(T_IDXS + self.dt, T_IDXS, self.a_solution)
t = time.monotonic()
if self.solution_status != 0:
if t > self.last_cloudlog_t + 5.0: