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

IQ.Pilot Release Commit @ d23cc80

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-27 18:01:16 -05:00
parent 46277b19ec
commit a70938008a
74 changed files with 715 additions and 1296 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

@@ -7,7 +7,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
def get_safety_CP():
@@ -20,7 +20,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)
@@ -66,7 +66,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

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

@@ -10,7 +10,7 @@ from iqdbc.car.values import PLATFORMS
CAR_LIST_JSON_OUT = os.path.join(BASEDIR, "../", "iqpilot", "car", "car_list.json")
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
@@ -62,8 +62,8 @@ def collect_car_docs(platforms, footnotes) -> dict[str, dict[str, list[str] | st
if __name__ == "__main__":
platform_list = get_car_list()
catalog = build_car_catalog()
with open(CAR_LIST_JSON_OUT, "w") as json_file:
json.dump(platform_list, json_file, indent=2, ensure_ascii=False)
json.dump(catalog, json_file, indent=2, ensure_ascii=False)
print(f"Generated and written to {CAR_LIST_JSON_OUT}")

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

@@ -79,14 +79,14 @@ 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)
_apply_long_tuning(CI, CP, CP_IQ, params_dict)
_apply_torque_blend(CP, CP_IQ, params_dict)
_initialize_radar_tracks(CP, CP_IQ, can_recv, can_send)
_initialize_stop_and_go(CP, CP_IQ, params_dict)
_initialize_toyota(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:
# Hyundai Custom Longitudinal Tuning
@@ -100,11 +100,11 @@ def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_IQ: str
_ = 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
@@ -116,10 +116,10 @@ def _initialize_radar_tracks(CP: structs.CarParams, CP_IQ: structs.IQCarParams,
CP.radarUnavailable = not tracks_enabled
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:
if CP.brand == 'subaru' and not CP.flags & (SubaruFlags.GLOBAL_GEN2 | SubaruFlags.HYBRID):
stop_and_go = int(params_dict.get("SubaruStopAndGo", 0)) == 1
stop_and_go_manual_parking_brake = int(params_dict.get("SubaruStopAndGoManualParkingBrake", 0)) == 1
stop_and_go = int(params_dict.get("IQSubaruCreepAssist", 0)) == 1
stop_and_go_manual_parking_brake = int(params_dict.get("IQSubaruCreepAssistManualBrake", 0)) == 1
if stop_and_go:
CP_IQ.flags |= SubaruFlagsIQ.STOP_AND_GO.value
@@ -129,9 +129,9 @@ def _initialize_stop_and_go(CP: structs.CarParams, CP_IQ: structs.IQCarParams, p
CP_IQ.safetyParam |= 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

@@ -88,7 +88,7 @@ class IQStopAndGoController:
return send_resume
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]:
can_sends = []
if not self.enabled:

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

12
iqdbc_repo/iqdbc/lvbs/car/tests/test_car_catalog.py Normal file
View File

@@ -0,0 +1,12 @@
import json
from iqdbc.lvbs.car.car_catalog import build_car_catalog, CAR_LIST_JSON_OUT
class TestCarList:
def test_generator(self):
generated_car_list = json.dumps(build_car_catalog(), indent=2, ensure_ascii=False)
with open(CAR_LIST_JSON_OUT) as f:
current_car_list = f.read()
assert generated_car_list == current_car_list, "Run iqdbc/lvbs/car/car_catalog.py to update the car list"

12
iqdbc_repo/iqdbc/lvbs/car/tests/test_car_list.py
View File

@@ -1,12 +0,0 @@
import json
from iqdbc.lvbs.car.platform_list import get_car_list, CAR_LIST_JSON_OUT
class TestCarList:
def test_generator(self):
generated_car_list = json.dumps(get_car_list(), indent=2, ensure_ascii=False)
with open(CAR_LIST_JSON_OUT) as f:
current_car_list = f.read()
assert generated_car_list == current_car_list, "Run iqdbc/iqpilot/car/platform_list.py to update the car list"