superrob/IQ.Pilot
1
0
Fork 0
forked from IQ.Lvbs/IQ.Pilot
Code Pull Requests Activity

IQ.Pilot Prebuilt Release @ 67fd9c2

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-01 20:15:16 -05:00
commit 13523543ee
2549 changed files with 678222 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
artifacts/package_runtime/iqdbc/car/gm/interface.py Normal file
View File

@@ -0,0 +1,265 @@
#!/usr/bin/env python3
from math import fabs, exp
import numpy as np
from iqdbc.car import get_safety_config, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.gm.carcontroller import CarController
from iqdbc.car.gm.carstate import CarState
from iqdbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAMERA_DATA_HEADER_MSG
from iqdbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags
from iqdbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
from iqdbc.lvbs.car.gm.iq_interface import IQCarInterface
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ, GMSafetyFlagsIQ
TransmissionType = structs.CarParams.TransmissionType
NetworkLocation = structs.CarParams.NetworkLocation
# Bolt Non-ACC uses the 4th (d) tune parameter; stock tunes zero it out.
NON_LINEAR_TORQUE_PARAMS_IQ = {
CAR.CHEVROLET_BOLT_NON_ACC: [2.24, 1.1, 0.28, -0.07],
CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN: [1.8, 1.1, 0.3, -0.045],
}
NON_LINEAR_TORQUE_PARAMS = {
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122],
**NON_LINEAR_TORQUE_PARAMS_IQ,
}
class CarInterface(CarInterfaceBase, IQCarInterface):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low,
structs.CarState.GearShifter.eco, structs.CarState.GearShifter.manumatic)
def __init__(self, CP, CP_IQ):
CarInterfaceBase.__init__(self, CP, CP_IQ)
IQCarInterface.__init__(self, CP, CarInterfaceBase)
@staticmethod
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
@staticmethod
def get_steer_feedforward_volt(desired_angle, v_ego):
desired_angle *= 0.02904609
sigmoid = desired_angle / (1 + fabs(desired_angle))
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
def get_steer_feedforward_function(self):
if self.CP.carFingerprint == CAR.CHEVROLET_VOLT:
return self.get_steer_feedforward_volt
else:
return CarInterfaceBase.get_steer_feedforward_default
def get_lataccel_torque_siglin(self) -> tuple[list[float], np.ndarray]:
def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float:
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, d = non_linear_torque_params
d = d if NON_LINEAR_TORQUE_PARAMS_IQ.get(self.CP.carFingerprint) else 0.0
sig_input = a * lateral_acceleration
sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
steer_torque = (sig * b) + (lateral_acceleration * c) + d
return float(steer_torque)
lataccel_values = np.arange(-5.0, 5.0, 0.01)
torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values]
assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]"
return torque_values, lataccel_values
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(lateral_acceleration, lataccel_values, torque_values)
return torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def lateral_accel_from_torque_siglin(torque: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(torque, torque_values, lataccel_values)
return lateral_accel_from_torque_siglin
else:
return self.lateral_accel_from_torque_linear
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "gm"
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)]
ret.autoResumeSng = False
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN]
if candidate in EV_CAR:
ret.transmissionType = TransmissionType.direct
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.EV.value
else:
ret.transmissionType = TransmissionType.automatic
ret.longitudinalTuning.kiBP = [5., 35.]
if candidate in (CAMERA_ACC_CAR | SDGM_CAR):
ret.alphaLongitudinalAvailable = candidate not in SDGM_CAR
ret.networkLocation = NetworkLocation.fwdCamera
ret.radarUnavailable = True # no radar
ret.pcmCruise = True
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
ret.minEnableSpeed = -1 if candidate in SDGM_CAR else 5 * CV.KPH_TO_MS
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
# Tuning for experimental long
ret.longitudinalTuning.kiV = [2.0, 1.5]
if alpha_long:
ret.pcmCruise = False
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM_LONG.value
if candidate in ALT_ACCS:
ret.alphaLongitudinalAvailable = False
ret.openpilotLongitudinalControl = False
ret.minEnableSpeed = -1. # engage speed is decided by PCM
else: # ASCM, OBD-II harness
ret.openpilotLongitudinalControl = True
ret.networkLocation = NetworkLocation.gateway
# LRR messages can take up to a few seconds to start sending after ignition, check camera data as well which starts earlier
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and CAMERA_DATA_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
# supports stop and go, but initial engage must (conservatively) be above 18mph
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
# Tuning
ret.longitudinalTuning.kiV = [2.4, 1.5]
# These cars have been put into dashcam only due to both a lack of users and test coverage.
# These cars likely still work fine. Once a user confirms each car works and a test route is
# added to iqdbc/car/tests/routes.py, we can remove it from this list.
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \
(ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable)
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
ret.steerLimitTimer = 0.4
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
if candidate == CAR.CHEVROLET_VOLT:
ret.lateralTuning.pid.kpBP = [0., 40.]
ret.lateralTuning.pid.kpV = [0., 0.17]
ret.lateralTuning.pid.kiBP = [0.]
ret.lateralTuning.pid.kiV = [0.]
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
ret.steerActuatorDelay = 0.2
elif candidate == CAR.GMC_ACADIA:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.BUICK_LACROSSE:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_ESCALADE:
ret.minEnableSpeed = -1. # engage speed is decided by pcm
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019):
ret.minEnableSpeed = -1. # engage speed is decided by pcm
if candidate == CAR.CADILLAC_ESCALADE_ESV:
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
ret.lateralTuning.pid.kf = 0.000045
else:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_BOLT_EUV:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_SILVERADO:
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
# with foot on brake to allow engagement, but this platform only has that check in the camera.
# TODO: check if this is split by EV/ICE with more platforms in the future
if ret.openpilotLongitudinalControl:
ret.minEnableSpeed = -1.
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_EQUINOX:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_TRAILBLAZER:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CADILLAC_XT4:
ret.steerActuatorDelay = 0.2
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_VOLT_2019:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.CHEVROLET_TRAVERSE:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.GMC_YUKON:
ret.steerActuatorDelay = 0.5
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.dashcamOnly = True # Needs steerRatio, tireStiffness, and lat accel factor tuning
return ret
@staticmethod
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
if candidate in (CAR.CHEVROLET_MALIBU_NON_ACC_9TH_GEN, CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN,
CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN):
stock_cp.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
elif candidate in (CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN, ):
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
# Non-ACC cars steer/long via the forward camera and pcmCruise, not the ASCM.
if ret.flags & GMFlagsIQ.NON_ACC:
stock_cp.dashcamOnly = False
stock_cp.alphaLongitudinalAvailable = False
stock_cp.networkLocation = NetworkLocation.fwdCamera
stock_cp.openpilotLongitudinalControl = False
stock_cp.pcmCruise = True
stock_cp.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
ret.iqSafetyFlags |= GMSafetyFlagsIQ.NON_ACC
stock_cp.minEnableSpeed = 24 * CV.MPH_TO_MS
stock_cp.minSteerSpeed = 3.0
# Untested Non-ACC platforms ship dashcam-only pending user validation.
if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN,
CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN,
CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, CAR.CADILLAC_XT5_NON_ACC_1ST_GEN):
stock_cp.dashcamOnly = True
return ret