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

IQ.Pilot Release Commit @ 773dae9

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-27 18:08:12 -05:00
parent 15c14e1369
commit 1a98a22c7f
74 changed files with 705 additions and 1286 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
iqdbc_repo/iqdbc/car/subaru/carcontroller.py
View File

@@ -6,7 +6,7 @@ from iqdbc.car.interfaces import CarControllerBase
from iqdbc.car.subaru import subarucan
from iqdbc.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags
from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoController
from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoController
# FIXME: These limits aren't exact. The real limit is more than likely over a larger time period and
# involves the total steering angle change rather than rate, but these limits work well for now
@@ -139,7 +139,7 @@ class CarController(CarControllerBase, IQStopAndGoController):
if self.frame % 2 == 0:
can_sends.append(subarucan.create_es_static_2(self.packer))
can_sends.extend(IQStopAndGoController.create_stop_and_go(self, self.packer, CC, CS, self.frame))
can_sends.extend(IQStopAndGoController.create_creep_assist(self, self.packer, CC, CS, self.frame))
new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.p.STEER_MAX

2
iqdbc_repo/iqdbc/car/subaru/carstate.py
View File

@@ -7,7 +7,7 @@ from iqdbc.car.subaru.values import DBC, CanBus, SubaruFlags
from iqdbc.car import CanSignalRateCalculator
from iqdbc.lvbs.car.subaru.aol import AolCarState
from iqdbc.lvbs.car.subaru.stop_and_go import IQStopAndGoState
from iqdbc.lvbs.car.subaru.creep_assist import IQStopAndGoState
class CarState(CarStateBase, AolCarState, IQStopAndGoState):

6
iqdbc_repo/iqdbc/car/tesla/carcontroller.py
View File

@@ -8,7 +8,7 @@ from iqdbc.car.tesla.teslacan import TeslaCAN
from iqdbc.car.tesla.values import CarControllerParams
from iqdbc.car.vehicle_model import VehicleModel
from openpilot.iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import get_set_speed_kph_from_params
from iqdbc.lvbs.car.tesla.coop_steering import CoopSteeringCarController
from iqdbc.lvbs.car.tesla.torque_blend import TorqueBlendController
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
@@ -22,7 +22,7 @@ def get_safety_CP():
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ):
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
self.coop_steer = CoopSteeringCarController()
self.coop_steer = TorqueBlendController()
self.apply_angle_last = 0
self.packer = CANPacker(dbc_names[Bus.party])
self.tesla_can = TeslaCAN(CP, self.packer)
@@ -99,7 +99,7 @@ class CarController(CarControllerBase):
# TODO: HUD control
new_actuators = actuators.as_builder()
new_actuators.steeringAngleDeg = self.apply_angle_last
new_actuators.accel = self.coop_steer.coop_apply_angle_last_sat # debug
new_actuators.accel = self.coop_steer.blend_apply_angle_last_sat # debug
new_actuators.curvature = float(self.coop_steer.debug_angle_desired_limited) # debug
new_actuators.torque = float(self.coop_steer.override_angle_accu) # debug

2
iqdbc_repo/iqdbc/car/tesla/teslacan.py
View File

@@ -10,7 +10,7 @@ class TeslaCAN:
self.l_jerk = 0.0
def create_steering_control(self, angle, enabled, control_type):
# control_type comes from coop_steering: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled
# control_type comes from torque_blend: ANGLE_CONTROL (1) normally, LANE_KEEP_ASSIST (2) when cooperative steering is enabled
control_type = control_type if enabled else 0
if self.CP.flags & TeslaFlags.LEGACY_DAS_STEERING:
control_type <<= 1 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal

4
iqdbc_repo/iqdbc/lvbs/car/platform_list.py → iqdbc_repo/iqdbc/lvbs/car/car_catalog.py
View File

@@ -5,7 +5,7 @@ from iqdbc.car.docs import get_all_footnotes, get_params_for_docs
from iqdbc.car.values import PLATFORMS
def get_car_list() -> dict[str, dict[str, list[str] | str]]:
def build_car_catalog() -> dict[str, dict[str, list[str] | str]]:
collected_footnote = get_all_footnotes()
sorted_list: dict[str, dict[str, list[str] | str]] = collect_car_docs(PLATFORMS, collected_footnote)
return sorted_list
@@ -57,6 +57,6 @@ def collect_car_docs(platforms, footnotes) -> dict[str, dict[str, list[str] | st
if __name__ == "__main__":
# get_car_list() is the raw platform source; the shipped catalog is generated
# build_car_catalog() is the raw platform source; the shipped catalog is generated
# (and encoded to its on-disk envelope) by the main-repo entry point:
print("run: python -m openpilot.iqpilot.selfdrive.car.vehicle_catalog")

26
iqdbc_repo/iqdbc/lvbs/car/interfaces.py
View File

@@ -75,43 +75,43 @@ def apply_iq_car_config(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict = {k: v for param in params_list for k, v in param.items()}
_initialize_custom_longitudinal_tuning(CI, CP, CP_IQ, params_dict)
_initialize_coop_steering(CP, CP_IQ, params_dict)
_initialize_stop_and_go(CP, CP_IQ, params_dict)
_initialize_toyota(CP, CP_IQ, params_dict)
_apply_long_tuning(CI, CP, CP_IQ, params_dict)
_apply_torque_blend(CP, CP_IQ, params_dict)
_apply_creep_assist(CP, CP_IQ, params_dict)
_apply_toyota_options(CP, CP_IQ, params_dict)
def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
def _apply_long_tuning(CI, CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict: dict[str, str]) -> None:
_ = CI.get_longitudinal_tuning_iq(CP, CP_IQ)
def _initialize_coop_steering(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
def _apply_torque_blend(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
params_dict: dict[str, str]) -> None:
if CP.brand == 'tesla':
coop_steering = int(params_dict.get("TeslaCoopSteering", 0)) == 1
if coop_steering:
torque_blend = int(params_dict.get("IQTeslaTorqueBlend", 0)) == 1
if torque_blend:
CP_IQ.flags |= TeslaFlagsIQ.COOP_STEERING.value
def _initialize_stop_and_go(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
def _apply_creep_assist(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
# Subaru stop-and-go; unsupported on gen2-global and hybrid platforms.
if CP.brand != 'subaru' or CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
return
if int(params_dict.get("SubaruStopAndGo", 0)) == 1:
if int(params_dict.get("IQSubaruCreepAssist", 0)) == 1:
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
if int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1:
if int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1:
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE.value
if CP_IQ.flags & (SubaruFlagsIQ.STOP_AND_GO | SubaruFlagsIQ.STOP_AND_GO_MANUAL_PARKING_BRAKE):
CP_IQ.iqSafetyFlags |= SubaruSafetyFlagsIQ.STOP_AND_GO
def _initialize_toyota(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
def _apply_toyota_options(CP: structs.CarParams, CP_IQ: structs.IQCarParams, params_dict: dict[str, str]) -> None:
if CP.brand == 'toyota':
toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1
toyota_stock_long = int(params_dict.get("IQToyotaFactoryLong", 0)) == 1
toyota_sng_hack = int(params_dict.get("ToyotaSnGHack", 0)) == 1
if toyota_stock_long:

2
iqdbc_repo/iqdbc/lvbs/car/subaru/stop_and_go.py → iqdbc_repo/iqdbc/lvbs/car/subaru/creep_assist.py
View File

@@ -68,7 +68,7 @@ class IQStopAndGoController:
return held_long_enough
return self._epb_pulse(standing and lead_pulling_away)
def create_stop_and_go(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
def create_creep_assist(self, packer, CC: structs.CarControl, CS: CarStateBase, frame: int) -> list[CanData]:
if not self.enabled:
return []

24
iqdbc_repo/iqdbc/lvbs/car/tesla/coop_steering.py → iqdbc_repo/iqdbc/lvbs/car/tesla/torque_blend.py
View File

@@ -15,7 +15,7 @@ from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
DT_LAT_CTRL = DT_CTRL * CarControllerParams.STEER_STEP
class CoopSteeringCarControllerParams(CarControllerParams):
class TorqueBlendParams(CarControllerParams):
ANGLE_LIMITS = replace(CarControllerParams.ANGLE_LIMITS, MAX_ANGLE_RATE=5)
STEERING_DEG_PHASE_LEAD_COEFF = 8.0
@@ -28,7 +28,7 @@ STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT = 10 # deg/Nm stability and smoothness for a
# angle ramping
STEER_OVERRIDE_MAX_LAT_JERK = 2.0 # m/s^3 - determines angle ramping rate - speed dependent
STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down
STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 - for low speed angle ramp down
# stability and smoothness for angle ramp control - at very low speeds this takes precedence over jerk settings
STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE
STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE
@@ -42,7 +42,7 @@ STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER = 0.7 # second
STEER_RESUME_RATE_LIMIT_RAMP_RATE = 500 # deg/s^2 - controls rate of rise of angle rate limit, not angle directly
CoopSteeringDataIQ = namedtuple("CoopSteeringDataIQ",
TorqueBlendDataIQ = namedtuple("TorqueBlendDataIQ",
["steeringAngleDeg", "lat_active", "control_type"])
def get_steer_from_lat_accel(lat_accel, v_ego: float, VM: VehicleModel):
@@ -84,7 +84,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
"""
# prevents windup in carcontroller rate limiter
lat_jerk = min(lat_jerk, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_LATERAL_JERK)
lat_jerk = min(lat_jerk, TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK)
# lateral accel is linear in respect to angle so it's fine to interpolate it with torque
torque_to_angle = get_steer_from_lat_accel(lat_jerk, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE
@@ -93,7 +93,7 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
override_angle_rate = torque * min(torque_to_angle, gain_limit)
# prevent windup in angle rate limiter
return apply_bounds(override_angle_rate * DT_LAT_CTRL, CoopSteeringCarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
return apply_bounds(override_angle_rate * DT_LAT_CTRL, TorqueBlendParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
class SteerRateLimiter:
@@ -160,10 +160,10 @@ class SteerAccelLimiter:
return angle_out
class CoopSteeringCarController:
class TorqueBlendController:
def __init__(self):
self.coop_apply_angle_last = 0
self.coop_apply_angle_last_sat = 0
self.blend_apply_angle_last_sat = 0
self.override_angle_accu = 0
self.override_active_counter = 0 # Counter for how many cycles torque is below threshold
self.resume_rate_limiter_delta = SteerRateLimiter()
@@ -201,7 +201,7 @@ class CoopSteeringCarController:
return 0
# unwind accumulator toward zero if the previous loop saturated (apply_steer_angle_limits_vm)
unwind = (self.coop_apply_angle_last - self.coop_apply_angle_last_sat) * unwind_weight
unwind = (self.coop_apply_angle_last - self.blend_apply_angle_last_sat) * unwind_weight
if self.override_angle_accu * unwind > 0:
unwind = apply_bounds(unwind, abs(self.override_angle_accu))
self.override_angle_accu -= unwind
@@ -289,7 +289,7 @@ class CoopSteeringCarController:
apply_angle_lim = self.resume_rate_limiter.update(apply_angle, angle_rate_delta_lim)
return apply_angle_lim
def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> CoopSteeringDataIQ:
def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> TorqueBlendDataIQ:
# estimate real steering angle by adding rate to the tesla filtered angle
steeringAngleDegPhaseLead = CS.out.steeringAngleDeg + CS.out.steeringRateDeg / STEERING_DEG_PHASE_LEAD_COEFF
@@ -306,7 +306,7 @@ class CoopSteeringCarController:
# final rate limit - matching panda safety
self.coop_apply_angle_last = apply_angle
self.coop_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.coop_apply_angle_last_sat, CS.out.vEgoRaw,
CS.out.steeringAngleDeg, lat_active, CoopSteeringCarControllerParams, VM)
self.blend_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.blend_apply_angle_last_sat, CS.out.vEgoRaw,
CS.out.steeringAngleDeg, lat_active, TorqueBlendParams, VM)
return CoopSteeringDataIQ(self.coop_apply_angle_last_sat, lat_active, 1) # 1 = angle control
return TorqueBlendDataIQ(self.blend_apply_angle_last_sat, lat_active, 1) # 1 = angle control

4
iqdbc_repo/iqdbc/lvbs/car/tests/test_car_list.py → iqdbc_repo/iqdbc/lvbs/car/tests/test_car_catalog.py
View File

@@ -2,7 +2,7 @@ import json
import os
from iqdbc.car.common.basedir import BASEDIR
from iqdbc.lvbs.car.platform_list import get_car_list
from iqdbc.lvbs.car.car_catalog import build_car_catalog
CATALOG_JSON = os.path.join(BASEDIR, "..", "..", "iqpilot", "selfdrive", "car", "vehicle_catalog.json")
@@ -18,7 +18,7 @@ def _decode(envelope) -> dict:
class TestCarList:
def test_generator(self):
generated = get_car_list()
generated = build_car_catalog()
with open(CATALOG_JSON) as f:
shipped = _decode(json.load(f))