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

10
selfdrive/controls/lib/drive_helpers.py
View File

@@ -12,6 +12,11 @@ MAX_VEL_ERR = 5.0 # m/s
MAX_LATERAL_JERK = 5.0 # m/s^3
MAX_LATERAL_ACCEL_NO_ROLL = 3.0 # m/s^2
MAX_LATERAL_ACCEL_NO_ROLL_OVERRIDE = 5.0 # m/s^2
DEFAULT_STOPPING_SPEED = 0.25 # m/s
def should_stop(v_ego: float, a_target: float, stopping_speed: float = DEFAULT_STOPPING_SPEED) -> bool:
return bool(v_ego < stopping_speed and a_target < 0.1)
def clamp(val, min_val, max_val):
@@ -52,7 +57,7 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll, override=False) -
return float(new_curvature), limited_accel or limited_max_curv
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.3):
def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, stopping_speed=DEFAULT_STOPPING_SPEED):
if len(speeds) == len(t_idxs):
v_now = speeds[0]
a_now = accels[0]
@@ -62,8 +67,7 @@ def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.
v_now = 0.0
v_target = 0.0
a_target = 0.0
should_stop = (v_now < vEgoStopping and a_target < 0.1)
return a_target, should_stop
return a_target, should_stop(v_now, a_target, stopping_speed)
def curv_from_psis(psi_target, psi_rate, vego, action_t):
vego = np.clip(vego, MIN_SPEED, np.inf)

10
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -8,10 +8,10 @@ from difflib import SequenceMatcher
import numpy as np
from cereal import log, custom # noqa: F401 (custom kept available for downstream imports)
from opendbc.car import structs
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from opendbc.iqpilot.car.interfaces import LatControlInputs
from opendbc.iqpilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from iqdbc.car import structs
from iqdbc.car.lateral import FRICTION_THRESHOLD, get_friction
from iqdbc.iqpilot.car.interfaces import LatControlInputs
from iqdbc.iqpilot.car.lateral_ext import get_friction as get_friction_in_torque_space
from openpilot.common.basedir import BASEDIR
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
@@ -27,7 +27,7 @@ from openpilot.iqpilot.selfdrive.controls.lib.helpers.nav_torque_pulse import Na
# ===== locator =====
TORQUE_NN_MODEL_PATH = os.path.join(BASEDIR, "iqpilot", "iqpilot_iq_nnff_models", "neural_network_lateral_control")
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "opendbc", "car", "torque_data", "substitute.toml")
TORQUE_NN_MODEL_SUBSTITUTE_PATH = os.path.join(BASEDIR, "iqdbc", "car", "torque_data", "substitute.toml")
MOCK_MODEL_PATH = os.path.join(TORQUE_NN_MODEL_PATH, "MOCK.json")
# A candidate must reach this score for the fingerprint(+fw) match to count as exact.

2
selfdrive/controls/lib/latcontrol_torque_pq.py
View File

@@ -3,7 +3,7 @@ import numpy as np
from collections import deque
from cereal import log
from opendbc.car.lateral import get_friction
from iqdbc.car.lateral import get_friction
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol import LatControl

31
selfdrive/controls/lib/longcontrol.py
View File

@@ -11,16 +11,13 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState
def long_control_state_trans(CP, CP_IQ, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill):
def long_control_state_trans(CP_IQ, active, long_control_state, should_stop, brake_pressed, cruise_standstill):
# Gas Interceptor
cruise_standstill = cruise_standstill and not CP_IQ.enableGasInterceptor
stopping_condition = should_stop
starting_condition = (not should_stop and
not cruise_standstill and
not brake_pressed)
started_condition = v_ego > CP.vEgoStarting
if not active:
long_control_state = LongCtrlState.off
@@ -30,22 +27,15 @@ def long_control_state_trans(CP, CP_IQ, active, long_control_state, v_ego,
if not starting_condition:
long_control_state = LongCtrlState.stopping
else:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
else:
long_control_state = LongCtrlState.pid
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.stopping:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
elif starting_condition:
if starting_condition:
long_control_state = LongCtrlState.pid
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
if stopping_condition:
elif long_control_state == LongCtrlState.pid:
if should_stop:
long_control_state = LongCtrlState.stopping
elif started_condition:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:
@@ -73,8 +63,7 @@ class LongControl:
else:
stop_now = should_stop
self.long_control_state = long_control_state_trans(self.CP, self.CP_IQ, active, self.long_control_state, CS.vEgo,
stop_now, CS.brakePressed,
self.long_control_state = long_control_state_trans(self.CP_IQ, active, self.long_control_state, stop_now, CS.brakePressed,
CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset()
@@ -85,12 +74,8 @@ class LongControl:
output_accel = self.last_output_accel
if output_accel > self.CP.stopAccel:
output_accel = min(output_accel, 0.0)
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset()
self.smooth.reset()
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
# TODO: can we just go straight to stopAccel?
output_accel -= 1.0 * DT_CTRL # m/s^2/s while trying to stop
self.reset()
self.smooth.reset()

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:

110
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -3,17 +3,15 @@ import math
import numpy as np
import cereal.messaging as messaging
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from iqdbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
from openpilot.common.constants import CV
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from cereal import log
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import CRUISE_MIN_ACCEL, CRUISE_MAX_ACCEL
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_from_plan
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, DEFAULT_STOPPING_SPEED, get_accel_from_plan
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from openpilot.common.issue_debug import log_issue_limited
@@ -22,15 +20,11 @@ from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import Longit
A_CRUISE_MAX_VALS = [2.0, 1.6, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
A_CRUISE_MIN = -1.2
J_CRUISE = 1.0
CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
ALLOW_THROTTLE_THRESHOLD = 0.4
MIN_ALLOW_THROTTLE_SPEED = 2.5
# scales the in-MPC cruise envelope's decel bound per drive personality
PERSONALITY_CRUISE_DECEL_SCALE = {
log.LongitudinalPersonality.relaxed: 0.85,
log.LongitudinalPersonality.standard: 1.0,
log.LongitudinalPersonality.aggressive: 1.15,
}
LAUNCH_DISARM_SPEED = 2.0
LAUNCH_COMMIT_T = 3.5
@@ -54,23 +48,31 @@ def get_lead_distance(radarState):
return radarState.leadTwo.dRel
return 0
def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
This function returns a limited long acceleration allowed, depending on the existing lateral acceleration
this should avoid accelerating when losing the target in turns
"""
# FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
# The lookup table for turns should also be updated if we do this
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt, accel_coast, allow_throttle):
max_accel = ACCEL_MAX if e2e else get_max_accel(v_ego)
return [a_target[0], min(a_target[1], a_x_allowed)]
if not e2e:
a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
max_accel = min(max_accel, a_x_allowed)
if not allow_throttle:
clipped_accel_coast = max(accel_coast, ACCEL_MIN)
coast_limit = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [max_accel, clipped_accel_coast])
max_accel = min(max_accel, coast_limit)
target_accel = np.clip(v_cruise - v_ego, A_CRUISE_MIN, max_accel)
if not e2e:
target_accel = float(np.clip(target_accel, a_cruise_prev - J_CRUISE * dt, a_cruise_prev + J_CRUISE * dt))
cruise_should_stop = v_cruise == 0.0
return target_accel, cruise_should_stop
class LongitudinalPlanner(LongitudinalPlannerIQ):
def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
self.stopping_speed = CP_IQ.longitudinalStoppingSpeedOverride or DEFAULT_STOPPING_SPEED
self.mpc = LongitudinalMpc(dt=dt)
LongitudinalPlannerIQ.__init__(self, self.CP, CP_IQ, self.mpc)
self.fcw = False
@@ -79,7 +81,7 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
self.a_desired = init_a
self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt)
self.prev_accel_clip = [ACCEL_MIN, ACCEL_MAX]
self.a_cruise = 0.0
self.output_a_target = 0.0
self.output_should_stop = False
self.launch_armed = False
@@ -119,27 +121,21 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
v_ego = sm['carState'].vEgo
v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
v_cruise = v_cruise_kph * CV.KPH_TO_MS
v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
if sm['controlsState'].forceDecel:
v_cruise = 0.0
long_control_off = sm['controlsState'].longControlState == LongCtrlState.off
force_slow_decel = sm['controlsState'].forceDecel
# Reset current state when not engaged, or user is controlling the speed
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled
# PCM cruise speed may be updated a few cycles later, check if initialized
v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET
reset_state = reset_state or not v_cruise_initialized
# No change cost when user is controlling the speed, or when standstill
prev_accel_constraint = not (reset_state or sm['carState'].standstill)
accel_clip = [ACCEL_MIN, get_max_accel(v_ego)]
steer_angle_without_offset = sm['carState'].steeringAngleDeg - sm['liveParameters'].angleOffsetDeg
accel_clip = limit_accel_in_turns(v_ego, steer_angle_without_offset, accel_clip, self.CP)
if reset_state:
self.v_desired_filter.x = v_ego
# Clip aEgo to cruise limits to prevent large accelerations when becoming active
self.a_desired = np.clip(sm['carState'].aEgo, accel_clip[0], accel_clip[1])
self.a_desired = np.clip(sm['carState'].aEgo, ACCEL_MIN, ACCEL_MAX)
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
@@ -147,25 +143,16 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
# Don't clip at low speeds since throttle_prob doesn't account for creep
self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED
if not self.allow_throttle:
clipped_accel_coast = max(accel_coast, accel_clip[0])
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
v_cruise, self.a_desired = LongitudinalPlannerIQ.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
if force_slow_decel:
if sm['controlsState'].forceDecel:
v_cruise = 0.0
personality = sm['selfdriveState'].personality
v_cruise_envelope = LongitudinalPlannerIQ.cruise_envelope(self, v_cruise, v_ego, T_IDXS_MPC)
decel_scale = PERSONALITY_CRUISE_DECEL_SCALE.get(personality, 1.0)
cruise_accel_limits = (CRUISE_MIN_ACCEL * decel_scale, min(CRUISE_MAX_ACCEL, accel_clip[1]))
self.mpc.set_weights(prev_accel_constraint, personality=personality)
self.mpc.set_weights(personality=personality)
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
self.mpc.update(sm['modelV2'], sm['radarState'], v_cruise_envelope, cruise_accel_limits, personality=personality)
self.mpc.update(sm['modelV2'], sm['radarState'], personality=personality)
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -176,14 +163,12 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
if self.fcw:
cloudlog.info("FCW triggered")
# Interpolate 0.05 seconds and save as starting point for next iteration
# Save starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
action_t=action_t, stopping_speed=self.stopping_speed)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
@@ -199,25 +184,28 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
t_shifted = T_IDXS_MPC + t_cut
v_shifted = np.interp(t_shifted, T_IDXS_MPC, model_v)
a_shifted = np.interp(t_shifted, T_IDXS_MPC, model_a)
a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t, vEgoStopping=self.CP.vEgoStopping)[0]
a_launch = get_accel_from_plan(v_shifted, a_shifted, T_IDXS_MPC, action_t=action_t)[0]
a_launch_max = np.interp(v_ego, [LAUNCH_MOVING_SPEED, LAUNCH_DISARM_SPEED], [LAUNCH_MAX_ACCEL, 0.])
output_a_target_e2e = max(output_a_target_e2e, min(a_launch, a_launch_max))
if self.is_e2e(sm):
output_a_target = min(output_a_target_e2e, output_a_target_mpc)
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
if output_a_target < output_a_target_mpc:
self.mpc.source = LongitudinalPlanSource.e2e
else:
output_a_target = output_a_target_mpc
self.output_should_stop = output_should_stop_mpc
e2e = self.is_e2e(sm)
self.a_cruise, cruise_should_stop = get_cruise_accel(e2e, v_cruise, v_ego, self.a_cruise,
steer_angle_without_offset, self.CP, self.dt,
accel_coast, self.allow_throttle)
candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
(self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
if e2e:
candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
self.output_should_stop = any(should_stop for _, _, should_stop in candidates)
self.output_should_stop = self.output_should_stop or self.forcing_stop
self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
for idx in range(2):
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
self.output_a_target = np.clip(output_a_target, accel_clip[0], accel_clip[1])
self.prev_accel_clip = accel_clip
self.a_desired = float(self.output_a_target)
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')