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

IQ.Pilot Release Commit @ 4fcea4d

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-20 11:06:57 -05:00
commit 7b20edda67
4602 changed files with 1122468 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

264
selfdrive/controls/lib/longitudinal_planner.py Executable file
View File

@@ -0,0 +1,264 @@
#!/usr/bin/env python3
import math
import numpy as np
import cereal.messaging as messaging
from opendbc.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.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
from openpilot.common.swaglog import cloudlog
from openpilot.common.issue_debug import log_issue_limited
from openpilot.iqpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerIQ
A_CRUISE_MAX_VALS = [2.0, 1.6, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 10.0, 25., 40.]
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
LAUNCH_MOVING_SPEED = 1.2
LAUNCH_MAX_ACCEL = 1.5
# Lookup table for turns
_A_TOTAL_MAX_V = [1.7, 3.2]
_A_TOTAL_MAX_BP = [20., 40.]
def get_max_accel(v_ego):
return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
def get_coast_accel(pitch):
return np.sin(pitch) * -5.65 - 0.3 # fitted from data using xx/projects/allow_throttle/compute_coast_accel.py
def get_lead_distance(radarState):
if radarState.leadOne.status and (not radarState.leadTwo.status or radarState.leadOne.dRel < radarState.leadTwo.dRel):
return radarState.leadOne.dRel
if radarState.leadTwo.status:
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.))
return [a_target[0], min(a_target[1], a_x_allowed)]
class LongitudinalPlanner(LongitudinalPlannerIQ):
def __init__(self, CP, CP_IQ, init_v=0.0, init_a=0.0, dt=DT_MDL):
self.CP = CP
self.mpc = LongitudinalMpc(dt=dt)
LongitudinalPlannerIQ.__init__(self, self.CP, CP_IQ, self.mpc)
self.fcw = False
self.dt = dt
self.allow_throttle = True
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.output_a_target = 0.0
self.output_should_stop = False
self.launch_armed = False
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
@staticmethod
def parse_model(model_msg):
if (len(model_msg.position.x) == ModelConstants.IDX_N and
len(model_msg.velocity.x) == ModelConstants.IDX_N and
len(model_msg.acceleration.x) == ModelConstants.IDX_N):
x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x)
v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x)
a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
j = np.zeros(len(T_IDXS_MPC))
else:
x = np.zeros(len(T_IDXS_MPC))
v = np.zeros(len(T_IDXS_MPC))
a = np.zeros(len(T_IDXS_MPC))
j = np.zeros(len(T_IDXS_MPC))
if len(model_msg.meta.disengagePredictions.gasPressProbs) > 1:
throttle_prob = model_msg.meta.disengagePredictions.gasPressProbs[1]
else:
throttle_prob = 1.0
return x, v, a, j, throttle_prob
def update(self, sm):
LongitudinalPlannerIQ.update(self, sm)
if len(sm['carControl'].orientationNED) == 3:
accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
else:
accel_coast = ACCEL_MAX
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
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
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])
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
_, model_v, model_a, _, throttle_prob = self.parse_model(sm['modelV2'])
# 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:
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_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.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)
self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
# TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
if self.fcw:
cloudlog.info("FCW triggered")
# Interpolate 0.05 seconds and save as 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)
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
output_should_stop_e2e = sm['modelV2'].action.shouldStop
output_a_target_e2e, output_should_stop_e2e = self.apply_e2e_stop_distance(sm, v_ego, output_a_target_e2e, output_should_stop_e2e)
if sm['carState'].standstill:
self.launch_armed = True
elif v_ego > LAUNCH_DISARM_SPEED:
self.launch_armed = False
if (self.launch_armed and self.is_e2e(sm) and not output_should_stop_e2e and
np.interp(LAUNCH_COMMIT_T, T_IDXS_MPC, model_v) > LAUNCH_DISARM_SPEED):
t_cut = min(float(T_IDXS_MPC[np.argmax(model_v > LAUNCH_MOVING_SPEED)]), LAUNCH_COMMIT_T)
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_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
self.output_should_stop = self.output_should_stop or self.forcing_stop
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
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
gate_services = ['carState', 'controlsState', 'selfdriveState', 'radarState']
plan_send.valid = sm.all_checks(service_list=gate_services)
if not plan_send.valid:
log_issue_limited(
"longitudinal_plan_invalid",
"planner",
f"longitudinalPlan invalid alive={ {s: sm.alive[s] for s in gate_services} } "
f"freq_ok={ {s: sm.freq_ok[s] for s in gate_services} } valid={ {s: sm.valid[s] for s in gate_services} } "
f"subchecks=({sm.all_alive(gate_services)},{sm.all_freq_ok(gate_services)},{sm.all_valid(gate_services)}) "
f"recheck={sm.all_checks(service_list=gate_services)}",
interval_sec=5.0,
)
longitudinalPlan = plan_send.longitudinalPlan
longitudinalPlan.modelMonoTime = sm.logMonoTime['modelV2']
longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
longitudinalPlan.solverExecutionTime = self.mpc.solve_time
longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
longitudinalPlan.accels = self.a_desired_trajectory.tolist()
longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
longitudinalPlan.hasLead = (sm['modelV2'].leadsV3[0].prob > 0.5) if self.mpc.new_lead_mpc else sm['radarState'].leadOne.status
longitudinalPlan.leadDistance = get_lead_distance(sm['radarState'])
longitudinalPlan.longitudinalPlanSource = self.mpc.source
longitudinalPlan.fcw = self.fcw
longitudinalPlan.leadTrajectoryX0 = self.mpc.lead_xv_0[:, 0].tolist()
longitudinalPlan.leadTrajectoryV0 = self.mpc.lead_xv_0[:, 1].tolist()
longitudinalPlan.leadTrajectoryX1 = self.mpc.lead_xv_1[:, 0].tolist()
longitudinalPlan.leadTrajectoryV1 = self.mpc.lead_xv_1[:, 1].tolist()
longitudinalPlan.aTarget = float(self.output_a_target)
longitudinalPlan.shouldStop = bool(self.output_should_stop)
longitudinalPlan.allowBrake = True
longitudinalPlan.allowThrottle = bool(self.allow_throttle)
pm.send('longitudinalPlan', plan_send)
self.publish_longitudinal_plan_iq(sm, pm)