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

IQ.Pilot Release Commit @ f82ff4d

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-07-21 13:43:46 -05:00
parent 7b20edda67
commit b712a31728
717 changed files with 4347 additions and 3060 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
iqdbc_repo/iqdbc/car/tesla/__init__.py Normal file
View File

@@ -0,0 +1,2 @@
# FIXME: gate by FingerPrint
TESLA_BLINKERS = False

107
iqdbc_repo/iqdbc/car/tesla/carcontroller.py Normal file
View File

@@ -0,0 +1,107 @@
import numpy as np
from iqdbc.can import CANPacker
from iqdbc.car import Bus
from iqdbc.car.lateral import apply_steer_angle_limits_vm
from iqdbc.car.interfaces import CarControllerBase
from iqdbc.car.tesla import TESLA_BLINKERS
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.iqpilot.car.tesla.coop_steering import CoopSteeringCarController
from iqdbc.iqpilot.car.tesla.values import TeslaFlagsIQ
def get_safety_CP():
# We use the TESLA_MODEL_Y platform for lateral limiting to match safety
# A Model 3 at 40 m/s using the Model Y limits sees a <0.3% difference in max angle (from curvature factor)
from iqdbc.car.tesla.interface import CarInterface
return CarInterface.get_non_essential_params("TESLA_MODEL_Y")
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_IQ):
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
self.coop_steer = CoopSteeringCarController()
self.apply_angle_last = 0
self.packer = CANPacker(dbc_names[Bus.party])
self.tesla_can = TeslaCAN(CP, self.packer)
# Vehicle model used for lateral limiting
self.VM = VehicleModel(get_safety_CP())
self.has_vehicle_bus = bool(CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS)
self.body_controls_counter_last = -1
self.blinker_request_prev = False
self.blinker_cancel_frame = 0
def update(self, CC, CC_IQ, CS, now_nanos):
actuators = CC.actuators
can_sends = []
# Tesla EPS enforces disabling steering on heavy lateral override force.
# When enabling in a tight curve, we wait until user reduces steering force to start steering.
# Canceling is done on rising edge and is handled generically with CC.cruiseControl.cancel
lat_active = CC.latActive and CS.hands_on_level < 3
if self.frame % CarControllerParams.STEER_STEP == 0:
# Angular rate limit based on speed
self.apply_angle_last = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg,
lat_active, CarControllerParams, self.VM)
can_sends.append(self.tesla_can.create_steering_control(*self.coop_steer.update(self.apply_angle_last, lat_active, self.CP_IQ, CS, self.VM)))
if self.frame % 10 == 0:
can_sends.append(self.tesla_can.create_steering_allowed())
# Longitudinal control
if self.CP.openpilotLongitudinalControl:
if self.frame % 4 == 0:
state = 13 if CC.cruiseControl.cancel or CS.das_accCancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
if not CC.longActive:
accel = 0.
cntr = (self.frame // 4) % 8
set_speed_kph = get_set_speed_kph_from_params(CC_IQ.params)
can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CS.out.vEgo, CC.longActive,
CS.cruise_override, set_speed_kph=set_speed_kph))
else:
# Increment counter so cancel is prioritized even without openpilot longitudinal
if CC.cruiseControl.cancel:
cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False, True))
# Nav blinker control via DAS_bodyControls on the vehicle bus, phase-locked to the car's
# counter. Cancel on the trailing edge since the body controller latches the signal.
stock_dat = getattr(CS, 'das_body_controls_dat', b"")
# FIXME: gate by FingerPrint
if TESLA_BLINKERS and self.has_vehicle_bus and len(stock_dat) >= 8:
left_blinker = CC.leftBlinker
right_blinker = CC.rightBlinker
driver_opposes = (left_blinker and CS.out.rightBlinker) or (right_blinker and CS.out.leftBlinker)
if driver_opposes:
left_blinker = right_blinker = False
nav_requesting = left_blinker or right_blinker
if self.blinker_request_prev and not nav_requesting and not driver_opposes:
self.blinker_cancel_frame = self.frame + 150 # ~1.5 s
self.blinker_request_prev = nav_requesting
cancel = not nav_requesting and not driver_opposes and self.frame < self.blinker_cancel_frame
body_counter = stock_dat[6] >> 4
if body_counter != self.body_controls_counter_last:
can_sends.append(self.tesla_can.create_body_controls(stock_dat, left_blinker, right_blinker, cancel))
self.body_controls_counter_last = body_counter
# 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.curvature = float(self.coop_steer.debug_angle_desired_limited) # debug
new_actuators.torque = float(self.coop_steer.override_angle_accu) # debug
self.frame += 1
return new_actuators, can_sends

171
iqdbc_repo/iqdbc/car/tesla/carstate.py Normal file
View File

@@ -0,0 +1,171 @@
import copy
from iqdbc.can import CANDefine, CANParser
from iqdbc.car import Bus, create_button_events, structs
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.interfaces import CarStateBase
from iqdbc.car.tesla import TESLA_BLINKERS
from iqdbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD, TeslaFlags
from iqdbc.iqpilot.car.tesla.carstate_ext import CarStateExt
from iqdbc.iqpilot.car.tesla.values import TeslaFlagsIQ
ButtonType = structs.CarState.ButtonEvent.Type
class CarState(CarStateBase, CarStateExt):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
CarStateExt.__init__(self, CP, CP_IQ)
self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"]
self.summon = False
self.summon_prev = False
self.cruise_enabled_prev = False
self.hands_on_level = 0
self.acc_state_last = 0
self.das_control = None
self.das_body_controls_dat = b""
self.cruise_override = False
def update_summon_state(self, summon_state: str, cruise_enabled: bool):
summon_now = summon_state in ("ACTIVE", "COMPLETE", "SELFPARK_STARTED")
if summon_now and not self.summon_prev and not self.cruise_enabled_prev:
self.summon = True
if not summon_now:
self.summon = False
self.summon_prev = summon_now
self.cruise_enabled_prev = cruise_enabled
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
cp_party = can_parsers[Bus.party]
cp_ap_party = can_parsers[Bus.ap_party]
ret = structs.CarState()
ret_iq = structs.IQCarState()
scale_speed = 1.01
length = 0.11
# Vehicle speed
ret.vEgoRaw = cp_party.vl["DI_speed"]["DI_vehicleSpeed"] * CV.KPH_TO_MS
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
# Displayed speed
ui_speed_units = self.can_define.dv["DI_speed"]["DI_uiSpeedUnits"].get(int(cp_party.vl["DI_speed"]["DI_uiSpeedUnits"]), None)
if ui_speed_units == "DI_SPEED_KPH":
ret.vEgoCluster = cp_party.vl["DI_speed"]["DI_uiSpeed"] * CV.KPH_TO_MS
elif ui_speed_units == "DI_SPEED_MPH":
ret.vEgoCluster = cp_party.vl["DI_speed"]["DI_uiSpeed"] * CV.MPH_TO_MS
# Gas pedal
ret.gasPressed = cp_party.vl["DI_systemStatus"]["DI_accelPedalPos"] > 0
# Brake pedal
ret.brake = 0
ret.brakePressed = cp_party.vl["ESP_status"]["ESP_driverBrakeApply"] == 2
# Steering wheel
epas_status = cp_party.vl["EPAS3S_sysStatus"]
self.hands_on_level = epas_status["EPAS3S_handsOnLevel"]
ret.steeringAngleDeg = -epas_status["EPAS3S_internalSAS"]
ret.steeringRateDeg = -cp_ap_party.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"]
ret.steeringTorque = -epas_status["EPAS3S_torsionBarTorque"]
ret.steeringTorqueEps = -epas_status["EPAS3S_steeringRackForce"] * length / self.CP.steerRatio
# stock handsOnLevel uses >0.5 for 0.25s, but is too slow
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 5)
eac_status = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacStatus"].get(int(epas_status["EPAS3S_eacStatus"]), None)
ret.steerFaultPermanent = eac_status == "EAC_FAULT"
ret.steerFaultTemporary = eac_status == "EAC_INHIBITED"
# FSD disengages using union of handsOnLevel (slow overrides) and high angle rate faults (fast overrides, high speed)
eac_error_code = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacErrorCode"].get(int(epas_status["EPAS3S_eacErrorCode"]), None)
ret.steeringDisengage = self.hands_on_level >= 3 or (eac_status == "EAC_INHIBITED" and
eac_error_code == "EAC_ERROR_HIGH_ANGLE_RATE_SAFETY")
# Cruise state
cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp_party.vl["DI_state"]["DI_cruiseState"]), None)
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp_party.vl["DI_state"]["DI_speedUnits"]), None)
acc_state = cp_ap_party.vl["DAS_control"]["DAS_accState"]
# Respect all stock DAS cancel states, not just ACC_CANCEL_GENERIC_SILENT(13).
# ELDA/ELK triggers ACC_CANCEL_GENERIC(0) which must also be forwarded.
self.das_accCancel = acc_state in (0, 1, 2, 12, 13, 14, 15)
summon_state = self.can_define.dv["DI_state"]["DI_autoparkState"].get(int(cp_party.vl["DI_state"]["DI_autoparkState"]), None)
cruise_enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
self.cruise_override = cruise_state in ("OVERRIDE")
self.update_summon_state(summon_state, cruise_enabled)
# Match panda safety cruise engaged logic
ret.cruiseState.enabled = cruise_enabled and not self.summon
if speed_units == "KPH":
ret.cruiseState.speedCluster = cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
elif speed_units == "MPH":
ret.cruiseState.speedCluster = cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
ret.cruiseState.speed = max(ret.cruiseState.speedCluster / scale_speed, 1e-3)
ret.cruiseState.available = cruise_state == "STANDBY" or ret.cruiseState.enabled
ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special
ret.standstill = cp_party.vl["ESP_B"]["ESP_vehicleStandstillSts"] == 1
ret.accFaulted = cruise_state == "FAULT"
ret.buttonEvents = [*create_button_events(acc_state, self.acc_state_last, {0: ButtonType.cancel, 13: ButtonType.cancel})]
self.acc_state_last = acc_state
# Gear
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp_party.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")]
# Doors
ret.doorOpen = cp_party.vl["UI_warning"]["anyDoorOpen"] == 1
# Blinkers
ret.leftBlinker = cp_party.vl["UI_warning"]["leftBlinkerBlinking"] in (1, 2)
ret.rightBlinker = cp_party.vl["UI_warning"]["rightBlinkerBlinking"] in (1, 2)
# Seatbelt
ret.seatbeltUnlatched = cp_party.vl["UI_warning"]["buckleStatus"] != 1
# Blindspot
ret.leftBlindspot = cp_ap_party.vl["DAS_status"]["DAS_blindSpotRearLeft"] != 0
ret.rightBlindspot = cp_ap_party.vl["DAS_status"]["DAS_blindSpotRearRight"] != 0
# AEB
ret.stockAeb = cp_ap_party.vl["DAS_control"]["DAS_aebEvent"] == 1
# LKAS
steer_control_type = int(cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"])
if self.CP.flags & TeslaFlags.LEGACY_DAS_STEERING:
steer_control_type >>= 1 # legacy firmware uses a 2-bit field, one bit up from the 3-bit signal
ret.stockLkas = steer_control_type == 2 # LANE_KEEP_ASSIST
# Stock Autosteer should be disengaged (includes FSD)
# TODO: find for TESLA_MODEL_X and HW2.5 vehicles
if not (self.CP.flags & TeslaFlags.MISSING_DAS_SETTINGS):
ret.invalidLkasSetting = cp_ap_party.vl["DAS_status"]["DAS_autopilotState"] not in (0, 1, 2) # DISABLED, UNAVAILABLE, AVAILABLE
# Buttons # ToDo: add Gap adjust button
# Messages needed by carcontroller
self.das_control = copy.copy(cp_ap_party.vl["DAS_control"])
# Raw stock DAS_bodyControls bytes (bus 2), used to ride the blinker on the vehicle bus.
# FIXME: gate by FingerPrint
if TESLA_BLINKERS and Bus.cam in can_parsers:
self.das_body_controls_dat = bytes(can_parsers[Bus.cam].dat.get(0x3E9, b""))
CarStateExt.update(self, ret, ret_iq, can_parsers)
return ret, ret_iq
@staticmethod
def get_can_parsers(CP, CP_IQ):
parsers = {
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
**CarStateExt.get_parser(CP, CP_IQ),
}
# Stock DAS_bodyControls from the AP bus (bus 2) for the nav blinker.
if TESLA_BLINKERS and CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS and Bus.adas in DBC[CP.carFingerprint]:
parsers[Bus.cam] = CANParser(DBC[CP.carFingerprint][Bus.adas], [("DAS_bodyControls", 2)], CANBUS.autopilot_party)
return parsers

55
iqdbc_repo/iqdbc/car/tesla/fingerprints.py Normal file
View File

@@ -0,0 +1,55 @@
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from iqdbc.car.structs import CarParams
from iqdbc.car.tesla.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.TESLA_MODEL_3: {
(Ecu.eps, 0x730, None): [
b'TeM3_E014p10_0.0.0 (16),E014.17.00',
b'TeM3_E014p10_0.0.0 (16),EL014.17.00',
b'TeM3_ES014p11_0.0.0 (25),ES014.19.0',
b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1',
b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),E4015.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4015.03.2',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4L015.03.2',
b'TeMYG4_Main_0.0.0 (59),E4H014.29.0',
b'TeMYG4_Main_0.0.0 (65),E4H015.01.0',
b'TeMYG4_Main_0.0.0 (67),E4H015.02.1',
b'TeMYG4_Main_0.0.0 (77),E4H015.04.5',
b'TeMYG4_Main_0.0.0 (77),E4HP015.04.5',
b'TeMYG4_Main_0.0.0 (78),E4H015.05.0',
b'TeMYG4_Main_0.0.0 (78),E4HP015.05.0',
b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
],
},
CAR.TESLA_MODEL_Y: {
(Ecu.eps, 0x730, None): [
b'TeM3_E014p10_0.0.0 (16),Y002.18.00',
b'TeM3_E014p10_0.0.0 (16),YP002.18.00',
b'TeM3_E014p10_0.0.0 (24),Y002.21.2',
b'TeM3_ES014p11_0.0.0 (16),YS002.17',
b'TeM3_ES014p11_0.0.0 (25),YS002.19.0',
b'TeMYG4_DCS_Update_0.0.0 (13),Y4002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),Y4003.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),Y4P003.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (5),Y4003.03.2',
b'TeMYG4_Legacy3Y_0.0.0 (5),Y4P003.03.2',
b'TeMYG4_SingleECU_0.0.0 (28),Y4S002.23.0',
b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
b'TeMYG4_Legacy3Y_0.0.0 (6),Y4003.04.0',
b'TeMYG4_Main_0.0.0 (77),Y4003.05.4',
b'TeMYG4_Main_0.0.0 (78),Y4003.06.0',
],
},
CAR.TESLA_MODEL_X: {
(Ecu.eps, 0x730, None): [
b'TeM3_SP_XP002p2_0.0.0 (23),XPR003.6.0',
b'TeM3_SP_XP002p2_0.0.0 (36),XPR003.10.0',
],
},
}

66
iqdbc_repo/iqdbc/car/tesla/interface.py Normal file
View File

@@ -0,0 +1,66 @@
from iqdbc.car import Bus, get_safety_config, structs
from iqdbc.car.interfaces import CarInterfaceBase
from iqdbc.car.tesla.carcontroller import CarController
from iqdbc.car.tesla.carstate import CarState
from iqdbc.car.tesla.values import TeslaSafetyFlags, TeslaFlags, CANBUS, CAR, DBC, LEGACY_DAS_STEERING_FW, Ecu
from iqdbc.car.tesla.radar_interface import RadarInterface, RADAR_START_ADDR
from iqdbc.iqpilot.car.tesla.values import TeslaFlagsIQ, TeslaSafetyFlagsIQ
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "tesla"
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla)]
ret.steerLimitTimer = 0.4
ret.steerActuatorDelay = 0.1
ret.steerAtStandstill = True
ret.steerControlType = structs.CarParams.SteerControlType.angle
# Model X and HW 2.5 vehicles are missing DAS_settings
if 0x293 not in fingerprint[CANBUS.autopilot_party]:
ret.flags |= TeslaFlags.MISSING_DAS_SETTINGS.value
# Radar support is intended to work for:
# - Tesla Model 3 vehicles built approximately mid-2017 through early-2021
# - Tesla Model Y vehicles built approximately mid-2020 through early-2021
# - Vehicles equipped with the Continental ARS4-B radar (used on HW2 / HW2.5 / early HW3)
# - Radar CAN lines must be tapped and connected to CAN bus 1 (normally not used for tesla vehicles)
ret.radarUnavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in DBC[candidate]
ret.alphaLongitudinalAvailable = True
if alpha_long:
ret.openpilotLongitudinalControl = True
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.LONG_CONTROL.value
legacy_das = any(fw.ecu == Ecu.eps and fw.fwVersion in LEGACY_DAS_STEERING_FW.get(candidate, []) for fw in car_fw)
if legacy_das:
ret.flags |= TeslaFlags.LEGACY_DAS_STEERING.value
ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.LEGACY_DAS_STEERING.value
ret.dashcamOnly = candidate in (CAR.TESLA_MODEL_X,) # dashcam only, pending find invalidLkasSetting signal
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:
stock_cp.enableBsm = True
if candidate == CAR.TESLA_MODEL_X:
stock_cp.dashcamOnly = False
if 0x3DF in fingerprint[1]:
ret.flags |= TeslaFlagsIQ.HAS_VEHICLE_BUS.value
ret.iqSafetyFlags |= TeslaSafetyFlagsIQ.HAS_VEHICLE_BUS
return ret

89
iqdbc_repo/iqdbc/car/tesla/radar_interface.py Normal file
View File

@@ -0,0 +1,89 @@
from iqdbc.can import CANParser
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.tesla.values import DBC
RADAR_START_ADDR = 0x410
RADAR_MSG_COUNT = 80 # 40 points * 2 messages each
def get_radar_can_parser(CP):
if Bus.radar not in DBC[CP.carFingerprint]:
return None
messages = [('RadarStatus', 16)]
for i in range(RADAR_MSG_COUNT // 2):
messages.extend([
(f'RadarPoint{i}_A', 16),
(f'RadarPoint{i}_B', 16),
])
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.updated_messages = set()
self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
self.track_id = 0
self.radar_off_can = CP.radarUnavailable
self.rcp = get_radar_can_parser(CP)
def update(self, can_strings):
if self.radar_off_can or self.rcp is None:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = structs.RadarData()
if self.rcp is None:
return ret
if not self.rcp.can_valid:
ret.errors.canError = True
radar_status = self.rcp.vl['RadarStatus']
if radar_status['shortTermUnavailable']:
ret.errors.radarUnavailableTemporary = True
if radar_status['sensorBlocked'] or radar_status['vehDynamicsError']:
ret.errors.radarFault = True
for i in range(RADAR_MSG_COUNT // 2):
msg_a = self.rcp.vl[f'RadarPoint{i}_A']
msg_b = self.rcp.vl[f'RadarPoint{i}_B']
# Make sure msg A and B are together
if msg_a['Index'] != msg_b['Index2']:
continue
if not msg_a['Tracked']:
if i in self.pts:
del self.pts[i]
continue
if i not in self.pts:
self.pts[i] = structs.RadarData.RadarPoint()
self.pts[i].trackId = self.track_id
self.track_id += 1
self.pts[i].dRel = msg_a['LongDist']
self.pts[i].yRel = msg_a['LatDist']
self.pts[i].vRel = msg_a['LongSpeed']
self.pts[i].aRel = msg_a['LongAccel']
self.pts[i].yvRel = msg_b['LatSpeed']
self.pts[i].measured = bool(msg_a['Meas'])
ret.points = list(self.pts.values())
return ret

89
iqdbc_repo/iqdbc/car/tesla/teslacan.py Normal file
View File

@@ -0,0 +1,89 @@
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.tesla.values import CANBUS, CarControllerParams, TeslaFlags
from iqdbc.car import DT_CTRL
class TeslaCAN:
def __init__(self, CP, packer):
self.CP = CP
self.packer = packer
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 = 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
values = {
"DAS_steeringAngleRequest": -angle,
"DAS_steeringHapticRequest": 0,
"DAS_steeringControlType": control_type,
}
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active, cruise_override, set_speed_kph=None):
from iqdbc.car.interfaces import V_CRUISE_MAX
set_speed = max(v_ego * CV.MS_TO_KPH, 0)
self.l_jerk = 0.0
if active:
self.l_jerk = 0 if cruise_override else (self.l_jerk + CarControllerParams.JERK_UP * DT_CTRL * 4)
set_speed = 0 if accel < 0 else V_CRUISE_MAX
if set_speed_kph is not None and accel >= 0:
set_speed = max(0.0, min(V_CRUISE_MAX, float(set_speed_kph)))
values = {
"DAS_setSpeed": set_speed,
"DAS_accState": acc_state,
"DAS_aebEvent": 0,
"DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
"DAS_jerkMax": min(self.l_jerk, CarControllerParams.JERK_LIMIT_MAX),
"DAS_accelMin": accel,
"DAS_accelMax": max(accel, 0),
"DAS_controlCounter": counter,
}
return self.packer.make_can_msg("DAS_control", CANBUS.party, values)
def create_steering_allowed(self):
values = {
"APS_eacAllow": 1,
}
return self.packer.make_can_msg("APS_eacMonitor", CANBUS.party, values)
def create_body_controls(self, stock_dat, left_blinker, right_blinker, cancel=False):
# Ride alongside the car's native DAS_bodyControls: copy the raw frame, override only the
# turn-indicator bits, and stamp counter + 1 so our frame supersedes the stock one.
dat = bytearray(stock_dat)
if len(dat) < 8:
dat.extend(b"\x00" * (8 - len(dat)))
if left_blinker or right_blinker:
turn_req = 1 if left_blinker else 2 # DAS_TURN_INDICATOR_LEFT / _RIGHT
dat[1] = (dat[1] & ~0x07) | (turn_req & 0x07)
dat[2] = (dat[2] & ~0x3C) | (1 << 2) # DAS_ACTIVE_NAV_LANE_CHANGE
elif cancel:
dat[1] = (dat[1] & ~0x07) | 0x03 # DAS_TURN_INDICATOR_CANCEL
dat[2] = (dat[2] & ~0x3C) | (4 << 2) # DAS_CANCEL_LANE_CHANGE
counter = (((dat[6] >> 4) + 1) & 0x0F)
dat[6] = (dat[6] & ~0xF0) | (counter << 4)
addr = 0x3E9
checksum = (addr & 0xFF) + ((addr >> 8) & 0xFF)
for i in range(7):
checksum += dat[i]
dat[7] = checksum & 0xFF
return addr, bytes(dat), CANBUS.vehicle
def tesla_checksum(address: int, sig, d: bytearray) -> int:
checksum = (address & 0xFF) + ((address >> 8) & 0xFF)
checksum_byte = sig.start_bit // 8
for i in range(len(d)):
if i != checksum_byte:
checksum += d[i]
return checksum & 0xFF

0
iqdbc_repo/iqdbc/car/tesla/tests/__init__.py Normal file
View File

113
iqdbc_repo/iqdbc/car/tesla/tests/test_tesla.py Normal file
View File

@@ -0,0 +1,113 @@
from iqdbc.car import gen_empty_fingerprint, structs
from iqdbc.car.tesla.interface import CarInterface
from iqdbc.car.tesla.teslacan import TeslaCAN
from iqdbc.car.tesla.radar_interface import RADAR_START_ADDR
from iqdbc.car.tesla.carcontroller import CarController
from iqdbc.car.tesla.values import CAR
class TestTeslaFingerprint:
def test_radar_detection(self):
# Test radar availability detection for cars with radar DBC defined
for radar in (True, False):
fingerprint = gen_empty_fingerprint()
if radar:
fingerprint[1][RADAR_START_ADDR] = 8
CP = CarInterface.get_params(CAR.TESLA_MODEL_3, fingerprint, [], False, False, False)
assert CP.radarUnavailable != radar
def test_no_radar_car(self):
# Model X doesn't have radar DBC defined, should always be unavailable
for radar in (True, False):
fingerprint = gen_empty_fingerprint()
if radar:
fingerprint[1][RADAR_START_ADDR] = 8
CP = CarInterface.get_params(CAR.TESLA_MODEL_X, fingerprint, [], False, False, False)
assert CP.radarUnavailable # Always unavailable since no radar DBC
class TestTeslaCan:
class DummyPacker:
def make_can_msg(self, name, bus, values):
return name, bus, values
def test_longitudinal_command_does_not_reference_missing_jerk_attr(self):
CP = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_3)
tesla_can = TeslaCAN(CP, self.DummyPacker())
name, bus, values = tesla_can.create_longitudinal_command(4, 1.0, 0, 20.0, True, False)
assert name == "DAS_control"
assert bus == 0
assert values["DAS_jerkMax"] <= 4.9
assert values["DAS_jerkMax"] >= 0.0
def test_longitudinal_command_uses_explicit_set_speed(self):
CP = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_3)
tesla_can = TeslaCAN(CP, self.DummyPacker())
name, bus, values = tesla_can.create_longitudinal_command(4, 1.0, 0, 20.0, True, False, set_speed_kph=64.0)
assert name == "DAS_control"
assert bus == 0
assert values["DAS_setSpeed"] == 64.0
def test_longitudinal_command_preserves_decel_when_explicit_set_speed_present(self):
CP = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_3)
tesla_can = TeslaCAN(CP, self.DummyPacker())
name, bus, values = tesla_can.create_longitudinal_command(4, -0.5, 0, 20.0, True, False, set_speed_kph=64.0)
assert name == "DAS_control"
assert bus == 0
assert values["DAS_setSpeed"] == 0
assert values["DAS_accelMin"] < 0
class TestTeslaCarControllerIQParams:
def test_iq_params_override_set_speed(self):
CP = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_3)
CP.openpilotLongitudinalControl = True
controller = CarController(CAR.TESLA_MODEL_3.config.dbc_dict, CP, structs.IQCarParams())
class DummyCruise:
cancel = False
class DummyActuators:
steeringAngleDeg = 0.0
accel = 1.0
def as_builder(self):
return self
class DummyCarControl:
actuators = DummyActuators()
latActive = False
longActive = True
cruiseControl = DummyCruise()
class DummyCarState:
hands_on_level = 0
out = type("Out", (), {"vEgoRaw": 20.0, "steeringAngleDeg": 0.0, "steeringRateDeg": 0.0, "steeringTorque": 0.0, "vEgo": 20.0})()
das_accCancel = False
cruise_override = False
das_control = {"DAS_controlCounter": 0}
cc_iq = structs.IQCarControl(params=[
structs.IQCarControl.Param(
key="enhancedStockLongitudinalControl.setSpeedKph",
type="float",
value=b"64.0",
)
])
captured = {}
def fake_longitudinal_command(state, accel, cntr, v_ego, active, cruise_override, set_speed_kph=None):
captured["set_speed_kph"] = set_speed_kph
return ("DAS_control", 0, {"DAS_setSpeed": set_speed_kph})
controller.tesla_can.create_longitudinal_command = fake_longitudinal_command
controller.update(DummyCarControl(), cc_iq, DummyCarState(), 0)
assert captured["set_speed_kph"] == 64.0

178
iqdbc_repo/iqdbc/car/tesla/values.py Normal file
View File

@@ -0,0 +1,178 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
from iqdbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
from iqdbc.car.structs import CarParams, CarState
from iqdbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column, SupportType
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = CarParams.Ecu
class Footnote(Enum):
HW_TYPE = CarFootnote(
"Some 2023 model years have HW4. To check which hardware type your vehicle has, look for " +
"<b>Autopilot computer</b> under <b>Software -> Additional Vehicle Information</b> on your vehicle's touchscreen. </br></br>" +
"See <a href=\"https://www.notateslaapp.com/news/2173/how-to-check-if-your-tesla-has-hardware-4-ai4-or-hardware-3\">this page</a> for more information.",
Column.MODEL)
SETUP = CarFootnote(
"See more setup details for <a href=\"https://github.com/commaai/openpilot/wiki/tesla\" target=\"_blank\">Tesla</a>.",
Column.MAKE, setup_note=True)
@dataclass
class TeslaCarDocsHW3(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.tesla_a]))
footnotes: list[Enum] = field(default_factory=lambda: [Footnote.HW_TYPE, Footnote.SETUP])
@dataclass
class TeslaCarDocsHW4(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.tesla_b]))
footnotes: list[Enum] = field(default_factory=lambda: [Footnote.HW_TYPE, Footnote.SETUP])
@dataclass
class TeslaCarHW4ModelSXDocs(TeslaCarDocsHW4):
support_type: SupportType = SupportType.COMMUNITY
support_link: str = "community"
@dataclass
class TeslaPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.party: 'tesla_model3_party', Bus.adas: 'tesla_model3_vehicle'})
class CAR(Platforms):
TESLA_MODEL_3 = TeslaPlatformConfig(
[
# TODO: do we support 2017? It's HW3
TeslaCarDocsHW3("Tesla Model 3 (with HW3) 2019-23"),
TeslaCarDocsHW4("Tesla Model 3 (with HW4) 2024-25"),
],
CarSpecs(mass=1899., wheelbase=2.875, steerRatio=12.0),
{Bus.party: 'tesla_model3_party', Bus.radar: 'tesla_radar_continental_generated', Bus.adas: 'tesla_model3_vehicle'},
)
TESLA_MODEL_Y = TeslaPlatformConfig(
[
TeslaCarDocsHW3("Tesla Model Y (with HW3) 2020-23"),
TeslaCarDocsHW4("Tesla Model Y (with HW4) 2024-25"),
],
CarSpecs(mass=2072., wheelbase=2.890, steerRatio=12.0),
{Bus.party: 'tesla_model3_party', Bus.radar: 'tesla_radar_continental_generated', Bus.adas: 'tesla_model3_vehicle'},
)
TESLA_MODEL_X = TeslaPlatformConfig(
[TeslaCarHW4ModelSXDocs("Tesla Model X (with HW4) 2024")],
CarSpecs(mass=2495., wheelbase=2.960, steerRatio=12.0),
)
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
bus=0,
)
]
)
# Cars with this EPS FW have a 2-bit DAS_steeringControlType and use TeslaFlags.LEGACY_DAS_STEERING
LEGACY_DAS_STEERING_FW = {
CAR.TESLA_MODEL_3: [
b'TeM3_E014p10_0.0.0 (16),E014.17.00',
b'TeM3_E014p10_0.0.0 (16),EL014.17.00',
b'TeM3_ES014p11_0.0.0 (25),ES014.19.0',
b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1',
b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),E4015.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4015.03.2',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4L015.03.2',
b'TeMYG4_Main_0.0.0 (59),E4H014.29.0',
b'TeMYG4_Main_0.0.0 (65),E4H015.01.0',
b'TeMYG4_Main_0.0.0 (67),E4H015.02.1',
b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
],
CAR.TESLA_MODEL_Y: [
b'TeM3_E014p10_0.0.0 (16),Y002.18.00',
b'TeM3_E014p10_0.0.0 (16),YP002.18.00',
b'TeM3_ES014p11_0.0.0 (16),YS002.17',
b'TeM3_ES014p11_0.0.0 (25),YS002.19.0',
b'TeMYG4_DCS_Update_0.0.0 (13),Y4002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),Y4003.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),Y4P003.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (5),Y4003.03.2',
b'TeMYG4_Legacy3Y_0.0.0 (5),Y4P003.03.2',
b'TeMYG4_SingleECU_0.0.0 (28),Y4S002.23.0',
b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
],
CAR.TESLA_MODEL_X: [
b'TeM3_SP_XP002p2_0.0.0 (23),XPR003.6.0',
b'TeM3_SP_XP002p2_0.0.0 (36),XPR003.10.0',
],
}
class CANBUS:
party = 0
vehicle = 1
autopilot_party = 2
GEAR_MAP = {
"DI_GEAR_INVALID": CarState.GearShifter.unknown,
"DI_GEAR_P": CarState.GearShifter.park,
"DI_GEAR_R": CarState.GearShifter.reverse,
"DI_GEAR_N": CarState.GearShifter.neutral,
"DI_GEAR_D": CarState.GearShifter.drive,
"DI_GEAR_SNA": CarState.GearShifter.unknown,
}
# Add extra tolerance for average banked road since safety doesn't have the roll
AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. higher actual roll lowers lateral acceleration
class CarControllerParams:
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
# EPAS faults above this angle
360, # deg
# Tesla uses a vehicle model instead, check carcontroller.py for details
([], []),
([], []),
# Vehicle model angle limits
# Add extra tolerance for average banked road since safety doesn't have the roll
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^2
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^3
# limit angle rate to both prevent a fault and for low speed comfort (~12 mph rate down to 0 mph)
MAX_ANGLE_RATE=5, # deg/20ms frame, EPS faults at 12 at a standstill
)
STEER_STEP = 2 # Angle command is sent at 50 Hz
ACCEL_MAX = 2.0 # m/s^2
ACCEL_MIN = -3.48 # m/s^2
JERK_LIMIT_MAX = 4.9 # m/s^3, ACC faults at 5.0
JERK_LIMIT_MIN = -4.9 # m/s^3, ACC faults at 5.0
JERK_UP = 1.0 # m/s^3
class TeslaSafetyFlags(IntFlag):
LONG_CONTROL = 1
LEGACY_DAS_STEERING = 2
class TeslaFlags(IntFlag):
LONG_CONTROL = 1
LEGACY_DAS_STEERING = 2
MISSING_DAS_SETTINGS = 4
DBC = CAR.create_dbc_map()
STEER_THRESHOLD = 1