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

IQ.Pilot Prebuilt Release @ ab07000

Browse Source
This commit is contained in:
IQ.Lvbs history cleanup
2026-08-22 23:42:42 -05:00
commit 9f9c9a70cc
3729 changed files with 778697 additions and 0 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.lvbs.car.tesla.torque_blend import TorqueBlendController
from iqdbc.lvbs.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 = TorqueBlendController()
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.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
self.frame += 1
return new_actuators, can_sends

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

@@ -0,0 +1,189 @@
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.lvbs.car.tesla.iq_carstate import IQCarState
from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
from openpilot.common.params import Params
from openpilot.system.proprietary_runtime._verified_import import import_verified_module
vehicle_state = import_verified_module("iqpilot_alc_private", "iqpilot_private.konn3kt.iqlvbs.vehicle_state")
ButtonType = structs.CarState.ButtonEvent.Type
class CarState(CarStateBase, IQCarState):
def __init__(self, CP, CP_IQ):
CarStateBase.__init__(self, CP, CP_IQ)
IQCarState.__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_accCancel = False
self.das_cancel_last = True
self.das_control = None
self.das_body_controls_dat = b""
self._odometer_store = vehicle_state.VehicleOdometerStore(CP, Params())
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"]
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)
# 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.
# The stock AP is isolated from the party bus while the relay is closed, so its accState
# free-runs between ACC_ON and ACC_CANCEL_GENERIC. Only a rising edge while ACC is engaged
# is a real cancel; level-forwarding it pins DI_cruiseState to UNAVAILABLE and blocks engaging.
das_cancel = acc_state in (0, 1, 2, 12, 13, 14, 15)
if not cruise_enabled:
self.das_accCancel = False
elif das_cancel and not self.das_cancel_last:
self.das_accCancel = True
self.das_cancel_last = das_cancel
# 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""))
IQCarState.update(self, ret, ret_iq, can_parsers)
if ret.odometer > 0.0:
ret.odometer = self._odometer_store.record(ret.odometer) or 0.0
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),
**IQCarState.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

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

@@ -0,0 +1,56 @@
""" 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',
b'TeMYG4_Main_0.0.0 (87),Y4003.09.3',
],
},
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',
],
},
}

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

@@ -0,0 +1,69 @@
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, Ecu, is_legacy_das_steering
from iqdbc.car.tesla.radar_interface import RadarInterface, RADAR_START_ADDR
from iqdbc.lvbs.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 is_legacy_das_steering(candidate, fw.fwVersion) 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
# Vehicle-bus messages can be slow enough to miss the initial capture window.
# Accept either the established 0x3DF marker or the absolute odometer frame.
vehicle_bus_seen = any(0x3DF in bus or 0x3B6 in bus for bus in fingerprint.values())
if vehicle_bus_seen:
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 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
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

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

@@ -0,0 +1,226 @@
from collections import defaultdict
import pytest
from iqdbc.car import gen_empty_fingerprint, structs
from iqdbc.car.structs import CarParams
from iqdbc.car.fw_versions import match_fw_to_car
from iqdbc.car.tesla.fingerprints import FW_VERSIONS
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, FW_PATTERN, LEGACY_DAS_STEERING_FW, TeslaFlags, TeslaSafetyFlags,
get_platform_codes, is_legacy_das_steering)
from iqdbc.can import CANPacker, CANParser
Ecu = CarParams.Ecu
EPS_ADDR = 0x730
def fw_match(fw: bytes):
car_fw = [CarParams.CarFw(ecu=Ecu.eps, fwVersion=fw, address=EPS_ADDR, subAddress=0, brand='tesla')]
exact, matches = match_fw_to_car(car_fw, '0' * 17, log=False)
return exact, matches
class TestTeslaFwPattern:
def test_all_known_fw_parses(self):
for car, ecus in FW_VERSIONS.items():
for fws in ecus.values():
for fw in fws:
assert FW_PATTERN.match(fw) is not None, f'{car}: unparsed FW version: {fw}'
def test_model_code_identifies_one_platform(self):
# a new platform reusing an existing model code would silently misfingerprint
platforms = defaultdict(set)
for car, ecus in FW_VERSIONS.items():
for fws in ecus.values():
for model, _, _ in get_platform_codes(fws):
platforms[model].add(car)
for model, cars in platforms.items():
assert len(cars) == 1, f'model code {model} maps to multiple platforms: {cars}'
def test_exact_match_still_wins(self):
for car, ecus in FW_VERSIONS.items():
for fws in ecus.values():
for fw in fws:
exact, matches = fw_match(fw)
assert exact, f'{fw} fell back to fuzzy matching'
assert matches == {car}, f'{fw} matched {matches}, expected {car}'
@pytest.mark.parametrize("fw, expected", [
# a firmware bump within a known series, the case that used to fingerprint as MOCK
(b'TeMYG4_Main_0.0.0 (99),Y4003.14.0', CAR.TESLA_MODEL_Y),
(b'TeMYG4_Main_0.0.0 (99),E4H015.09.0', CAR.TESLA_MODEL_3),
(b'TeM3_SP_XP002p2_0.0.0 (40),XPR003.12.0', CAR.TESLA_MODEL_X),
# Tesla bumps the series within a platform (E4014 -> E4015, Y4002 -> Y4003)
(b'TeMYG4_Main_0.0.0 (12),Y4004.01.0', CAR.TESLA_MODEL_Y),
# an unknown model code is a car we don't support
(b'TeCT_Main_0.0.0 (1),CT001.01.0', None),
(b'garbage', None),
])
def test_unknown_fw_fuzzy_match(self, fw, expected):
exact, matches = fw_match(fw)
if expected is None:
assert matches == set(), f'{fw} unexpectedly matched {matches}'
else:
assert not exact
assert matches == {expected}
class TestTeslaLegacyDasSteering:
def test_reproduces_known_table(self):
for car, ecus in FW_VERSIONS.items():
for fws in ecus.values():
for fw in fws:
expected = fw in LEGACY_DAS_STEERING_FW.get(car, [])
assert is_legacy_das_steering(car, fw) == expected, f'{car}: wrong legacy DAS verdict for {fw}'
@pytest.mark.parametrize("car, fw, expected", [
# below a family's known modern cutoff (Y4/003 splits at 003.04.0)
(CAR.TESLA_MODEL_Y, b'TeMYG4_Legacy3Y_0.0.0 (5),Y4003.03.9', True),
(CAR.TESLA_MODEL_Y, b'TeMYG4_Main_0.0.0 (99),Y4003.14.0', False),
# E4/015 splits at 015.04.5
(CAR.TESLA_MODEL_3, b'TeMYG4_Main_0.0.0 (68),E4H015.03.9', True),
(CAR.TESLA_MODEL_3, b'TeMYG4_Main_0.0.0 (99),E4H015.09.0', False),
# families with no known modern FW: interpolate legacy, extrapolate modern
(CAR.TESLA_MODEL_3, b'TeM3_E014p10_0.0.0 (16),E014.18.00', True),
(CAR.TESLA_MODEL_3, b'TeM3_E014p10_0.0.0 (30),E014.22.0', False),
# numeric, not lexical, version compare (XPR003.10.0 > XPR003.6.0)
(CAR.TESLA_MODEL_X, b'TeM3_SP_XP002p2_0.0.0 (30),XPR003.9.0', True),
# an unknown series has no history to compare against
(CAR.TESLA_MODEL_Y, b'TeMYG4_Main_0.0.0 (12),Y4004.01.0', False),
(CAR.TESLA_MODEL_Y, b'garbage', False),
])
def test_unknown_fw(self, car, fw, expected):
assert is_legacy_das_steering(car, fw) == expected
def test_flag_set_from_fuzzy_match(self):
for fw, legacy in ((b'TeMYG4_Legacy3Y_0.0.0 (5),Y4003.03.9', True),
(b'TeMYG4_Main_0.0.0 (99),Y4003.14.0', False)):
car_fw = [CarParams.CarFw(ecu=Ecu.eps, fwVersion=fw, address=EPS_ADDR, subAddress=0, brand='tesla')]
CP = CarInterface.get_params(CAR.TESLA_MODEL_Y, gen_empty_fingerprint(), car_fw, False, False, False)
assert bool(CP.flags & TeslaFlags.LEGACY_DAS_STEERING) == legacy
assert bool(CP.safetyConfigs[0].safetyParam & TeslaSafetyFlags.LEGACY_DAS_STEERING) == legacy
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_vehicle_bus_odometer_decodes_kilometers(self):
packer = CANPacker("tesla_model3_vehicle")
parser = CANParser("tesla_model3_vehicle", [("ID3B6UI_odometer", 1)], 1)
message = packer.make_can_msg("ID3B6UI_odometer", 1, {
"UI_odometer": 29150.377,
"UI_odometerCounter": 1,
"UI_odometerChecksum": 0,
})
parser.update([1_000_000_000, [message]])
assert parser.vl["ID3B6UI_odometer"]["UI_odometer"] == 29150.377
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

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

@@ -0,0 +1,264 @@
import re
from collections import defaultdict
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from functools import cache
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, LiveFwVersions, OfflineFwVersions, 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),
)
# 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',
],
}
# e.g. TeMYG4_Main_0.0.0 (87),Y4003.09.3
# 11111_22222_______33____45__666666
# 1 = EPS firmware program, 2 = build lineage, 3 = build number, 4 = model code,
# 5 = trim/hardware variant, 6 = series and software version
#
# Only the model code identifies the vehicle: 1 and 2 are shared across models (Model 3 and
# Model Y both ship TeM3_ and TeMYG4_ firmware) and 3 is only monotone within one lineage.
FW_PATTERN = re.compile(rb'^Te[A-Z0-9]+_[A-Za-z0-9_]+_0\.0\.0 \(\d+\),' +
rb'(?P<model>E4|E|Y4|Y|XP)[A-Z]{0,2}(?P<series>\d{3})\.(?P<version>\d+(?:\.\d+)*)$')
def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes, tuple[int, ...]]]:
codes = set()
for fw in fw_versions:
match = FW_PATTERN.match(fw)
if match is not None:
codes.add((match.group('model'), match.group('series'),
tuple(int(v) for v in match.group('version').split(b'.'))))
return codes
@cache
def _das_steering_cutoffs() -> dict[tuple[str, bytes, bytes], tuple[tuple[int, ...] | None, tuple[int, ...] | None]]:
"""Per (platform, model code, series) family, the oldest known modern version and the newest
known legacy version. Tesla only ever moves a family forward, so these bound the split."""
# imported here because fingerprints.py imports this module
from iqdbc.car.tesla.fingerprints import FW_VERSIONS
legacy: defaultdict[tuple, set] = defaultdict(set)
modern: defaultdict[tuple, set] = defaultdict(set)
for platform, ecus in FW_VERSIONS.items():
known_legacy = LEGACY_DAS_STEERING_FW.get(platform, [])
for fws in ecus.values():
for fw in fws:
for model, series, version in get_platform_codes([fw]):
(legacy if fw in known_legacy else modern)[(platform, model, series)].add(version)
return {k: (min(modern[k]) if k in modern else None, max(legacy[k]) if k in legacy else None)
for k in set(legacy) | set(modern)}
def is_legacy_das_steering(candidate: str, fw: bytes) -> bool:
"""Whether an EPS FW uses the 2-bit DAS_steeringControlType. Unknown firmware newer than
anything in a family is treated as modern: cars only move forward, and someone left behind
on legacy software can force the platform with CarPlatformBundle."""
if fw in LEGACY_DAS_STEERING_FW.get(candidate, []):
return True
codes = get_platform_codes([fw])
if not len(codes):
return False
model, series, version = next(iter(codes))
first_modern, last_legacy = _das_steering_cutoffs().get((candidate, model, series), (None, None))
if first_modern is not None:
return version < first_modern
return last_legacy is not None and version <= last_legacy
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]:
# Tesla fingerprints on the EPS alone and Ecu.eps is in FUZZY_EXCLUDE_ECUS, so the generic fuzzy
# matcher can never match a Tesla. Match on the model code, which survives the EPS version bumps
# that ship with Tesla software updates. The series is deliberately not required to be known:
# Tesla bumps it within a platform (E4014 -> E4015, Y4002 -> Y4003).
offline_codes: defaultdict[bytes, set[str]] = defaultdict(set)
for candidate, ecus in offline_fw_versions.items():
for fws in ecus.values():
for model, _, _ in get_platform_codes(fws):
offline_codes[model].add(candidate)
candidates: set[str] = set()
for ecu, addr, sub_addr in {e for ecus in offline_fw_versions.values() for e in ecus}:
if ecu != Ecu.eps:
continue
for model, _, _ in get_platform_codes(live_fw_versions.get((addr, sub_addr), set())):
candidates |= offline_codes[model]
return candidates if len(candidates) == 1 else set()
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,
)
],
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
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