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

IQ.Pilot Release Commit @ fddcfba

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 13:46:05 -05:00
parent eea1278105
commit b6c0232c12
667 changed files with 3218 additions and 1780 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/controls/controlsd.py
View File

@@ -11,8 +11,8 @@ from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, lock_memory, DT_CTRL, Priority, Ratekeeper
from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -99,7 +99,7 @@ class Controls(IQControlsLayer):
try:
if self.CP.brand != 'volkswagen':
return False
from opendbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.values import VolkswagenFlags
return bool(self.CP.flags & VolkswagenFlags.PQ)
except Exception:
cloudlog.exception("pq torque selection failed; using generic torque")

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()

2
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

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

@@ -3,7 +3,7 @@ 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
@@ -11,7 +11,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, LongitudinalPlanSource
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
@@ -72,6 +72,7 @@ def get_cruise_accel(e2e, v_cruise, v_ego, a_cruise_prev, angle_steers, CP, dt,
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
@@ -167,7 +168,7 @@ class LongitudinalPlanner(LongitudinalPlannerIQ):
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
@@ -183,7 +184,7 @@ 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))

6
selfdrive/controls/radard.py
View File

@@ -12,9 +12,9 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process
from openpilot.common.swaglog import cloudlog
from openpilot.common.simple_kalman import KF1D
from opendbc.car import structs
from opendbc.car.hyundai.values import HyundaiFlags
from opendbc.iqpilot.car.hyundai.values import HyundaiFlagsIQ
from iqdbc.car import structs
from iqdbc.car.hyundai.values import HyundaiFlags
from iqdbc.iqpilot.car.hyundai.values import HyundaiFlagsIQ
from openpilot.iqpilot.selfdrive.controls.lib.custom_stop_distance import CustomStopDistance

20
selfdrive/controls/tests/test_drive_helpers.py Normal file
View File

@@ -0,0 +1,20 @@
import pytest
from openpilot.selfdrive.controls.lib.drive_helpers import DEFAULT_STOPPING_SPEED, should_stop
class TestShouldStop:
@pytest.mark.parametrize("v_ego, expected", [
(DEFAULT_STOPPING_SPEED - 0.01, True),
(DEFAULT_STOPPING_SPEED, False),
])
def test_upstream_default(self, v_ego, expected):
assert should_stop(v_ego, -0.1) == expected
@pytest.mark.parametrize("stopping_speed", [0.55 / 3.6, 1.5 / 3.6])
def test_car_override(self, stopping_speed):
assert should_stop(stopping_speed - 0.01, -0.1, stopping_speed)
assert not should_stop(stopping_speed, -0.1, stopping_speed)
def test_requires_deceleration(self):
assert not should_stop(0.0, 0.1, 1.0)

12
selfdrive/controls/tests/test_latcontrol.py
View File

@@ -1,12 +1,12 @@
from parameterized import parameterized
from cereal import car, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.nissan.values import CAR as NISSAN
from opendbc.car.gm.values import CAR as GM
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.honda.values import CAR as HONDA
from iqdbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.nissan.values import CAR as NISSAN
from iqdbc.car.gm.values import CAR as GM
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.car.helpers import convert_to_capnp
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID

6
selfdrive/controls/tests/test_latcontrol_torque_buffer.py
View File

@@ -1,9 +1,9 @@
from parameterized import parameterized
from cereal import car, log
from opendbc.car.car_helpers import interfaces
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.vehicle_model import VehicleModel
from iqdbc.car.car_helpers import interfaces
from iqdbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque, LAT_ACCEL_REQUEST_BUFFER_SECONDS

2
selfdrive/controls/tests/test_leads.py
View File

@@ -1,6 +1,6 @@
import cereal.messaging as messaging
from opendbc.car.toyota.values import CAR as TOYOTA
from iqdbc.car.toyota.values import CAR as TOYOTA
from openpilot.selfdrive.test.process_replay import replay_process_with_name

36
selfdrive/controls/tests/test_longcontrol.py
View File

@@ -1,59 +1,43 @@
from cereal import car, custom
from cereal import custom
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
class TestLongControlStateTransition:
def test_stay_stopped(self):
CP = car.CarParams.new_message()
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
active = False
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.off
def test_engage():
CP = car.CarParams.new_message()
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.off
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
def test_starting():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
CP_IQ = custom.IQCarParams.new_message()
active = True
current_state = LongCtrlState.starting
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.starting
next_state = long_control_state_trans(CP, CP_IQ, active, current_state, v_ego=1.0,
next_state = long_control_state_trans(CP_IQ, active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid

6
selfdrive/controls/tests/test_torqued_lat_accel_offset.py
View File

@@ -1,8 +1,8 @@
import numpy as np
from cereal import car, messaging
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
from opendbc.car import structs
from opendbc.car.lateral import get_friction, FRICTION_THRESHOLD
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY
from iqdbc.car import structs
from iqdbc.car.lateral import get_friction, FRICTION_THRESHOLD
from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.locationd.torqued import TorqueEstimator, MIN_BUCKET_POINTS, POINTS_PER_BUCKET, STEER_BUCKET_BOUNDS