diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py
index a5560211a55..ca573fadf04 100644
--- a/opendbc_repo/opendbc/car/tesla/carcontroller.py
+++ b/opendbc_repo/opendbc/car/tesla/carcontroller.py
@@ -3,7 +3,7 @@
from opendbc.car import Bus, apply_steer_angle_limits_vm, structs
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.tesla.teslacan import TeslaCAN
-from opendbc.car.tesla.values import CarControllerParams
+from opendbc.car.tesla.values import CarControllerParams, TeslaFlags
from opendbc.car.tesla.coop_steering import CoopSteeringCarController
from opendbc.car.vehicle_model import VehicleModel
@@ -27,6 +27,12 @@ def __init__(self, dbc_names, CP):
# Vehicle model used for lateral limiting
self.VM = VehicleModel(get_safety_CP())
+ # Blinker MITM state
+ self.has_vehicle_bus = bool(CP.flags & TeslaFlags.HAS_VEHICLE_BUS)
+ self.body_controls_counter_last = -1
+ self.blinker_request_prev = False
+ self.blinker_cancel_frame = 0
+
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
can_sends = []
@@ -55,19 +61,46 @@ def update(self, CC, CS, now_nanos):
# Longitudinal control
if self.CP.openpilotLongitudinalControl:
if self.frame % 4 == 0:
- state = 13 if CC.cruiseControl.cancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
+ 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
- can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CS.out.vEgo, CC.longActive))
+ set_speed_kph = None # TODO: pass from params when available
+ 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))
+ 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"")
+ if 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
self.frame += 1
- return new_actuators, can_sends
+ return new_actuators, can_sends
\ No newline at end of file
diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py
index e0f1302435c..81ce6aaee21 100644
--- a/opendbc_repo/opendbc/car/tesla/carstate.py
+++ b/opendbc_repo/opendbc/car/tesla/carstate.py
@@ -1,9 +1,10 @@
import copy
from opendbc.can import CANDefine, CANParser
-from opendbc.car import Bus, structs
+from opendbc.car import Bus, create_button_events, structs
+from opendbc.car.carlog import carlog
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import CarStateBase
-from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD
+from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD, TeslaFlags
ButtonType = structs.CarState.ButtonEvent.Type
@@ -14,9 +15,30 @@ def __init__(self, CP):
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.fsd14_error_logged = False
+ self.suspected_fsd14 = False
+ self.suspected_fsd14_clear_frames = 0
+
self.hands_on_level = 0
+ self.acc_cancel_last = 0
self.das_control = None
+ self.das_body_controls_dat = b""
+ self.das_accCancel = False
+ self.cruise_override = False
self.coop_steering = True
+ self.infotainment_3_finger_press = 0
+
+ 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) -> structs.CarState:
cp_party = can_parsers[Bus.party]
@@ -89,13 +111,27 @@ def update(self, can_parsers) -> structs.CarState:
ret.steerFaultTemporary = eac_status == "EAC_INHIBITED"
ret.vehicleSensorsInvalid = cp_ap_party.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleValidity"] != 1
+ # FSD disengages on strong user override (handsOnLevel >= 3) or high angle rate faults (fast override, high speed)
+ eac_error_code = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacErrorCode"].get(int(epas_status["EPAS3S_eacErrorCode"]), None)
+ self.steering_disengage = 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_raw = int(cp_party.vl["DI_state"]["DI_speedUnits"])
speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(speed_units_raw, speed_units_raw)
-
- ret.cruiseState.enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
+ 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 == "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 in ("KPH", "DI_SPEED_KPH"):
cruise_is_kph = True
elif speed_units in ("MPH", "DI_SPEED_MPH"):
@@ -111,6 +147,13 @@ def update(self, can_parsers) -> structs.CarState:
ret.standstill = cruise_state == "STANDSTILL"
ret.accFaulted = cruise_state == "FAULT"
+ # Emit a single cancel button event on the rising edge of any stock DAS cancel state.
+ # Feeding the raw DAS_accState enum would emit spurious "unknown" events for normal
+ # states and miss cancel codes other than 0/13 that das_accCancel already covers.
+ acc_cancel = 1 if self.das_accCancel else 0
+ ret.buttonEvents = [*create_button_events(acc_cancel, self.acc_cancel_last, {1: ButtonType.cancel})]
+ self.acc_cancel_last = acc_cancel
+
# DAS_fusedSpeedLimit from DBC is always in kph (scale=5). Do NOT apply ui_is_kph conversion.
speed_limit = cp_ap_party.vl["DAS_status"]["DAS_fusedSpeedLimit"]
if 0 < speed_limit <= 150:
@@ -141,22 +184,75 @@ def update(self, can_parsers) -> structs.CarState:
ret.leftBlindspot = cp_ap_party.vl["DAS_status"]["DAS_blindSpotRearLeft"] != 0
ret.rightBlindspot = cp_ap_party.vl["DAS_status"]["DAS_blindSpotRearRight"] != 0
- # AEB
+ # Stock AEB from DAS — the only reliable collision avoidance signal.
+ # DAS_steeringControlType (EMERGENCY_LANE_KEEP) also triggers on ELDA
+ # (normal lane correction), so it's NOT used for disengagement.
+ # The Panda safety layer handles emergency steering forwarding at
+ # the physical level (safety_tesla.h).
ret.stockAeb = cp_ap_party.vl["DAS_control"]["DAS_aebEvent"] == 1
ret.stockFcw = cp_ap_party.vl["DAS_status"]["DAS_forwardCollisionWarning"] != 0
- # Model X and HW 2.5 vehicles are missing DAS_settings
+ # LKAS
+ # On FSD 14+, ANGLE_CONTROL behavior changed to allow user winddown while actuating.
+ # Stock Autosteer should be off (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
+
+ # Because we don't have FSD 14 detection outside of a set of FW, we should check if this FW is accidentally missing from FSD_14_FW
+ # 1. If in Autosteer or FSD, already caught by invalidLkasSetting
+ # 2. If in TACC and DAS ever sends ANGLE_CONTROL (1), we can infer it's trying to do LKAS on FSD 14+
+ # NOTE: Tesla's latest firmware changed ELDA (Emergency Lane Departure Assist) to use ANGLE_CONTROL (1)
+ # instead of EMERGENCY_LANE_KEEP (3). Exclude ELDA by checking eac_status so it doesn't latch suspected_fsd14.
+ eac_is_emergency = eac_status == "EMERGENCY_LANE_KEEP"
+ angle_control = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == 1 and not eac_is_emergency # ANGLE_CONTROL, excluding ELDA
+ if not ret.invalidLkasSetting and angle_control and not self.CP.flags & TeslaFlags.FSD_14:
+ self.suspected_fsd14 = True
+ self.suspected_fsd14_clear_frames = 0
+
+ if self.suspected_fsd14:
+ ret.invalidLkasSetting = True
+ if not self.fsd14_error_logged:
+ carlog.error("FSD 14 detected, but FW not in FSD_14_FW set")
+ self.fsd14_error_logged = True
+ # Un-latch if ANGLE_CONTROL has been absent for ~3 s (100 frames @ ~33 Hz).
+ # This allows re-engagement after transient triggers (e.g. if ELDA slips through on new FW variants).
+ if not angle_control:
+ self.suspected_fsd14_clear_frames += 1
+ if self.suspected_fsd14_clear_frames >= 100:
+ self.suspected_fsd14 = False
+ self.suspected_fsd14_clear_frames = 0
+ else:
+ self.suspected_fsd14_clear_frames = 0
# 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.
+ if Bus.cam in can_parsers:
+ self.das_body_controls_dat = bytes(can_parsers[Bus.cam].dat.get(0x3E9, b""))
+
+ # 3-finger infotainment press detection (vehicle bus)
+ if Bus.adas in can_parsers:
+ cp_adas = can_parsers[Bus.adas]
+ prev_infotainment = self.infotainment_3_finger_press
+ self.infotainment_3_finger_press = int(cp_adas.vl["UI_status2"]["UI_activeTouchPoints"])
+ ret.buttonEvents = [*ret.buttonEvents, *create_button_events(
+ self.infotainment_3_finger_press, prev_infotainment,
+ {3: ButtonType.lkas})]
+
return ret
@staticmethod
def get_can_parsers(CP):
- return {
+ parsers = {
Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
- Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
+ Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party),
}
+ if CP.flags & TeslaFlags.HAS_VEHICLE_BUS:
+ parsers[Bus.adas] = CANParser("tesla_model3_vehicle", [("UI_status2", 2)], CANBUS.vehicle)
+ if CP.flags & TeslaFlags.HAS_DAS_BODY_CONTROLS:
+ parsers[Bus.cam] = CANParser("tesla_model3_vehicle", [("DAS_bodyControls", 2)], CANBUS.autopilot_party)
+ return parsers
\ No newline at end of file
diff --git a/opendbc_repo/opendbc/car/tesla/fingerprints.py b/opendbc_repo/opendbc/car/tesla/fingerprints.py
index b03dc390278..551fa01ea07 100644
--- a/opendbc_repo/opendbc/car/tesla/fingerprints.py
+++ b/opendbc_repo/opendbc/car/tesla/fingerprints.py
@@ -1,49 +1,55 @@
-from opendbc.car.structs import CarParams
-from opendbc.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'TeM3_E014p10_0.0.0 (24),E014.20.2',
- 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),E4HP015.05.0',
- b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
- b'TeMYG4_Main_0.0.0 (78),E4H015.05.0',
- ],
- },
- 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),YP002.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_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_SingleECU_0.0.0 (28),Y4S002.23.0',
- b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
- ],
- },
-}
+from opendbc.car.structs import CarParams
+from opendbc.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'TeM3_E014p10_0.0.0 (24),E014.20.2',
+ 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),E4HP015.05.0',
+ b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
+ b'TeMYG4_Main_0.0.0 (78),E4H015.05.0',
+ ],
+ },
+ 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),YP002.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_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_SingleECU_0.0.0 (28),Y4S002.23.0',
+ b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
+ ],
+ },
+ 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',
+ ],
+ },
+}
diff --git a/opendbc_repo/opendbc/car/tesla/interface.py b/opendbc_repo/opendbc/car/tesla/interface.py
index e7450c16257..c23a8d4623f 100644
--- a/opendbc_repo/opendbc/car/tesla/interface.py
+++ b/opendbc_repo/opendbc/car/tesla/interface.py
@@ -1,46 +1,61 @@
-from opendbc.car import Bus, get_safety_config, structs
-from opendbc.car.interfaces import CarInterfaceBase
-from opendbc.car.tesla.carcontroller import CarController
-from opendbc.car.tesla.carstate import CarState
-from opendbc.car.tesla.values import TeslaSafetyFlags, TeslaFlags, CANBUS, DBC
-from opendbc.car.tesla.radar_interface import RadarInterface, RADAR_START_ADDR
-
-
-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
-
- ret.vEgoStopping = 0.1
- ret.vEgoStarting = 0.1
- ret.stoppingDecelRate = 0.3
-
- return ret
+from opendbc.car import Bus, get_safety_config, structs
+from opendbc.car.interfaces import CarInterfaceBase
+from opendbc.car.tesla.carcontroller import CarController
+from opendbc.car.tesla.carstate import CarState
+from opendbc.car.tesla.values import TeslaSafetyFlags, TeslaFlags, CANBUS, CAR, DBC, FSD_14_FW, Ecu
+from opendbc.car.tesla.radar_interface import RadarInterface, RADAR_START_ADDR
+
+
+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
+
+ ret.vEgoStopping = 0.1
+ ret.vEgoStarting = 0.1
+ ret.stoppingDecelRate = 0.3
+
+ fsd_14 = any(fw.ecu == Ecu.eps and fw.fwVersion in FSD_14_FW.get(candidate, []) for fw in car_fw)
+ if fsd_14:
+ ret.flags |= TeslaFlags.FSD_14.value
+ ret.safetyConfigs[0].safetyParam |= TeslaSafetyFlags.FSD_14.value
+
+ ret.dashcamOnly = candidate in (CAR.TESLA_MODEL_X,) # dashcam only, pending find invalidLkasSetting signal
+
+ # Vehicle bus detection: 0x3DF (infotainment 3-finger press) on bus 1
+ if 0x3DF in fingerprint[CANBUS.vehicle]:
+ ret.flags |= TeslaFlags.HAS_VEHICLE_BUS.value
+
+ # DAS_bodyControls detection: 0x3E9 on bus 2 (autopilot party)
+ if 0x3E9 in fingerprint[CANBUS.autopilot_party]:
+ ret.flags |= TeslaFlags.HAS_DAS_BODY_CONTROLS.value
+
+ return ret
diff --git a/opendbc_repo/opendbc/car/tesla/teslacan.py b/opendbc_repo/opendbc/car/tesla/teslacan.py
index 8180aeb025a..53564334b3d 100644
--- a/opendbc_repo/opendbc/car/tesla/teslacan.py
+++ b/opendbc_repo/opendbc/car/tesla/teslacan.py
@@ -1,63 +1,114 @@
-from opendbc.car.common.conversions import Conversions as CV
-from opendbc.car.tesla.values import CANBUS, CarControllerParams
-
-
-class TeslaCAN:
- def __init__(self, CP, packer):
- self.CP = CP
- self.packer = packer
-
- @staticmethod
- def checksum(msg_id, dat):
- ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF)
- ret += sum(dat)
- return ret & 0xFF
-
- def create_steering_control(self, angle, enabled, counter):
- values = {
- "DAS_steeringAngleRequest": -angle,
- "DAS_steeringHapticRequest": 0,
- "DAS_steeringControlType": 1 if enabled else 0,
- "DAS_steeringControlCounter": counter,
- }
-
- data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)[1]
- values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
- return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
-
- def create_longitudinal_command(self, acc_state, accel, cntr, v_ego, active):
- set_speed = min(max(v_ego + accel, 0) * CV.MS_TO_KPH, 400)
-
- values = {
- "DAS_setSpeed": set_speed,
- "DAS_accState": acc_state,
- "DAS_aebEvent": 0,
- "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
- "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
- "DAS_accelMin": accel,
- "DAS_accelMax": max(accel, 0),
- "DAS_controlCounter": cntr,
- "DAS_controlChecksum": 0,
- }
- data = self.packer.make_can_msg("DAS_control", CANBUS.party, values)[1]
- values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
- return self.packer.make_can_msg("DAS_control", CANBUS.party, values)
-
- def create_steering_allowed(self, counter):
- values = {
- "APS_eacAllow": 1,
- "APS_eacMonitorCounter": counter,
- }
-
- data = self.packer.make_can_msg("APS_eacMonitor", CANBUS.party, values)[1]
- values["APS_eacMonitorChecksum"] = self.checksum(0x27d, data[:2])
- return self.packer.make_can_msg("APS_eacMonitor", CANBUS.party, values)
-
-
-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
+from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.tesla.values import CANBUS, CarControllerParams, TeslaFlags
+from opendbc.car import DT_CTRL
+
+
+def get_steer_ctrl_type(flags: int, ctrl_type: int) -> int:
+ # Returns the flipped signal value for DAS_steeringControlType on FSD 14
+ if flags & TeslaFlags.FSD_14:
+ return {1: 2, 2: 1}.get(ctrl_type, ctrl_type)
+ else:
+ return ctrl_type
+
+
+class TeslaCAN:
+ def __init__(self, CP, packer):
+ self.CP = CP
+ self.packer = packer
+ self.l_jerk = 0.0
+
+ @staticmethod
+ def checksum(msg_id, dat):
+ ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF)
+ ret += sum(dat)
+ return ret & 0xFF
+
+ def create_steering_control(self, angle, enabled, counter):
+ values = {
+ "DAS_steeringAngleRequest": -angle,
+ "DAS_steeringHapticRequest": 0,
+ "DAS_steeringControlType": get_steer_ctrl_type(self.CP.flags, 1 if enabled else 0),
+ "DAS_steeringControlCounter": counter,
+ }
+
+ data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)[1]
+ values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
+ return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
+
+ def create_longitudinal_command(self, acc_state, accel, cntr, v_ego, active, cruise_override=False, set_speed_kph=None):
+ from opendbc.car.interfaces import V_CRUISE_MAX
+
+ set_speed = max(v_ego * CV.MS_TO_KPH, 0)
+ if active:
+ self.l_jerk = 0 if cruise_override else (self.l_jerk + CarControllerParams.JERK_UP * DT_CTRL * 4)
+ # Ramp set_speed smoothly: follow current speed plus a safety margin,
+ # so DAS doesn't see an abrupt jump to 0 or V_CRUISE_MAX
+ set_speed = min(max(v_ego + accel, 0) * CV.MS_TO_KPH, 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)))
+ else:
+ self.l_jerk = 0.0
+
+ 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": cntr,
+ "DAS_controlChecksum": 0,
+ }
+ data = self.packer.make_can_msg("DAS_control", CANBUS.party, values)[1]
+ values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
+ return self.packer.make_can_msg("DAS_control", CANBUS.party, values)
+
+ def create_steering_allowed(self, counter):
+ values = {
+ "APS_eacAllow": 1,
+ "APS_eacMonitorCounter": counter,
+ }
+
+ data = self.packer.make_can_msg("APS_eacMonitor", CANBUS.party, values)[1]
+ values["APS_eacMonitorChecksum"] = self.checksum(0x27d, data[:2])
+ 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
+ # DAS_turnIndicatorRequest: bits 8-9 = byte1 bits 0-1
+ dat[1] = (dat[1] & ~0x03) | (turn_req & 0x03)
+ # DAS_turnIndicatorRequestReason: bits 17-20 = byte2 bits 1-4, DAS_ACTIVE_NAV_LANE_CHANGE = 1
+ dat[2] = (dat[2] & ~0x1E) | (1 << 1)
+ elif cancel:
+ # DAS_TURN_INDICATOR_CANCEL = 3
+ dat[1] = (dat[1] & ~0x03) | 0x03
+ # DAS_CANCEL_LANE_CHANGE = 4
+ dat[2] = (dat[2] & ~0x1E) | (4 << 1)
+
+ counter = (((dat[6] >> 4) + 1) & 0x0F)
+ dat[6] = (dat[6] & ~0xF0) | (counter << 4)
+
+ addr = 0x3E9
+ checksum_val = (addr & 0xFF) + ((addr >> 8) & 0xFF)
+ for i in range(7):
+ checksum_val += dat[i]
+ dat[7] = checksum_val & 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
\ No newline at end of file
diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py
index 7de101197ad..0641a2d0152 100644
--- a/opendbc_repo/opendbc/car/tesla/values.py
+++ b/opendbc_repo/opendbc/car/tesla/values.py
@@ -1,138 +1,164 @@
-from dataclasses import dataclass, field
-from enum import Enum, IntFlag
-from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleSteeringLimits, ISO_LATERAL_ACCEL
-from opendbc.car.structs import CarParams, CarState
-from opendbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column
-from opendbc.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 " +
- "Autopilot computer under Software -> Additional Vehicle Information on your vehicle's touchscreen. " +
- "See this page for more information.",
- Column.MODEL)
-
- SETUP = CarFootnote(
- "See more setup details for Tesla.",
- 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 TeslaPlatformConfig(PlatformConfig):
- dbc_dict: DbcDict = field(default_factory=lambda: {Bus.party: 'tesla_model3_party'})
-
-
-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'},
- )
- 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'},
- )
-
-
-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,
- )
- ]
-)
-
-
-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
-
- # Speed-based rate limits (primary limiter at low speed, VM handles high speed)
- ([0., 5., 25.], [2.5, 1.5, 0.2]),
- ([0., 5., 25.], [5., 2.0, 0.3]),
-
- # Vehicle model angle limits (secondary limiter — max lateral accel/jerk)
- # 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
-
- # Absolute cap: prevent fault even at standstill (EPS faults at 12)
- MAX_ANGLE_RATE=5, # deg/20ms frame
- )
-
- 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
-
-
-class TeslaSafetyFlags(IntFlag):
- LONG_CONTROL = 1
-
-
-class TeslaFlags(IntFlag):
- LONG_CONTROL = 1
- MISSING_DAS_SETTINGS = 4
-
-
-DBC = CAR.create_dbc_map()
-
-STEER_THRESHOLD = 1
-
-if __name__ == "__main__":
- cars = []
- for platform in CAR:
- for doc in platform.config.car_docs:
- cars.append(doc.name)
- cars.sort()
- for c in cars:
- print(c)
+from dataclasses import dataclass, field
+from enum import Enum, IntFlag
+from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleSteeringLimits, ISO_LATERAL_ACCEL
+from opendbc.car.structs import CarParams, CarState
+from opendbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column, SupportType
+from opendbc.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 " +
+ "Autopilot computer under Software -> Additional Vehicle Information on your vehicle's touchscreen. " +
+ "See this page for more information.",
+ Column.MODEL)
+
+ SETUP = CarFootnote(
+ "See more setup details for Tesla.",
+ 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'})
+
+
+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'},
+ )
+ 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'},
+ )
+ 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 FSD 14 and use TeslaFlags.FSD_14
+FSD_14_FW = {
+ CAR.TESLA_MODEL_3: [
+ b'TeMYG4_Main_0.0.0 (77),E4HP015.04.5',
+ b'TeMYG4_Main_0.0.0 (78),E4HP015.05.0',
+ ],
+ CAR.TESLA_MODEL_Y: [
+ b'TeMYG4_Legacy3Y_0.0.0 (6),Y4003.04.0',
+ b'TeMYG4_Main_0.0.0 (77),Y4003.05.4',
+ ]
+}
+
+
+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
+
+ # Speed-based rate limits (primary limiter at low speed, VM handles high speed)
+ ([0., 5., 25.], [2.5, 1.5, 0.2]),
+ ([0., 5., 25.], [5., 2.0, 0.3]),
+
+ # Vehicle model angle limits (secondary limiter — max lateral accel/jerk)
+ # 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
+
+ # Absolute cap: prevent fault even at standstill (EPS faults at 12)
+ MAX_ANGLE_RATE=5, # deg/20ms frame
+ )
+
+ 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
+ FSD_14 = 2
+
+
+class TeslaFlags(IntFlag):
+ LONG_CONTROL = 1
+ FSD_14 = 2
+ MISSING_DAS_SETTINGS = 4
+ HAS_VEHICLE_BUS = 8
+ HAS_DAS_BODY_CONTROLS = 16
+
+
+DBC = CAR.create_dbc_map()
+
+STEER_THRESHOLD = 1
+
+if __name__ == "__main__":
+ cars = []
+ for platform in CAR:
+ for doc in platform.config.car_docs:
+ cars.append(doc.name)
+ cars.sort()
+ for c in cars:
+ print(c)
\ No newline at end of file
diff --git a/opendbc_repo/opendbc/safety/safety/safety_tesla.h b/opendbc_repo/opendbc/safety/safety/safety_tesla.h
index 60b8337f218..9170ce18cf7 100644
--- a/opendbc_repo/opendbc/safety/safety/safety_tesla.h
+++ b/opendbc_repo/opendbc/safety/safety/safety_tesla.h
@@ -5,6 +5,30 @@
static bool tesla_longitudinal = false;
static bool tesla_stock_aeb = false;
+// FSD 14 support: flips ANGLE_CONTROL and LANE_KEEP_ASSIST control types
+static bool tesla_fsd_14 = false;
+
+// Stock steering control (LDA/ELDA/Autopark) conflict detection
+static bool tesla_stock_steering_control = false;
+static bool tesla_stock_steering_control_prev = false;
+
+// Summon/Autopark state: block openpilot messages when active
+static bool tesla_summon = false;
+static bool tesla_summon_prev = false;
+
+static int tesla_get_steer_ctrl_type(const int ctrl_type) {
+ // Returns the flipped control type for FSD 14 vehicles
+ int steer_ctrl_type = ctrl_type;
+ if (tesla_fsd_14) {
+ if (ctrl_type == 1) {
+ steer_ctrl_type = 2;
+ } else if (ctrl_type == 2) {
+ steer_ctrl_type = 1;
+ }
+ }
+ return steer_ctrl_type;
+}
+
static void tesla_rx_hook(const CANPacket_t *to_push) {
int bus = GET_BUS(to_push);
int addr = GET_ADDR(to_push);
@@ -36,6 +60,21 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
// Cruise state
if (addr == 0x286) {
+ // Autopark/Summon state
+ int autopark_state = (GET_BYTE(to_push, 3) >> 1) & 0x0FU; // DI_autoparkState
+ bool tesla_summon_now = (autopark_state == 3) || // ACTIVE
+ (autopark_state == 4) || // COMPLETE
+ (autopark_state == 9); // SELFPARK_STARTED
+
+ // Only consider rising edges while controls are not allowed
+ if (tesla_summon_now && !tesla_summon_prev && !cruise_engaged_prev) {
+ tesla_summon = true;
+ }
+ if (!tesla_summon_now) {
+ tesla_summon = false;
+ }
+ tesla_summon_prev = tesla_summon_now;
+
int cruise_state = (GET_BYTE(to_push, 1) >> 4) & 0x07U;
bool cruise_engaged = (cruise_state == 2) || // ENABLED
(cruise_state == 3) || // STANDSTILL
@@ -43,6 +82,7 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
(cruise_state == 6) || // PRE_FAULT
(cruise_state == 7); // PRE_CANCEL
+ cruise_engaged = cruise_engaged && !tesla_summon;
vehicle_moving = cruise_state != 3; // STANDSTILL
pcm_cruise_check(cruise_engaged);
}
@@ -53,6 +93,22 @@ static void tesla_rx_hook(const CANPacket_t *to_push) {
// "AEB_ACTIVE"
tesla_stock_aeb = (GET_BYTE(to_push, 2) & 0x03U) == 1U;
}
+
+ // DAS_steeringControl: detect stock steering control (LDA, ELDA, Autopark)
+ if (addr == 0x488) {
+ int steering_control_type = GET_BYTE(to_push, 2) >> 6;
+ bool tesla_stock_steering_control_now = steering_control_type != 0; // any non-NONE
+
+ // Always respect the DAS's steering commands, including emergency evasive steering.
+ // Without this, openpilot would block the emergency steering from reaching the EPAS.
+ if (tesla_stock_steering_control_now && !tesla_stock_steering_control_prev) {
+ tesla_stock_steering_control = true;
+ }
+ if (!tesla_stock_steering_control_now) {
+ tesla_stock_steering_control = false;
+ }
+ tesla_stock_steering_control_prev = tesla_stock_steering_control_now;
+ }
}
generic_rx_checks((addr == 0x488) && (bus == 0)); // DAS_steeringControl
@@ -88,18 +144,37 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
int addr = GET_ADDR(to_send);
bool violation = false;
+ // Don't send any messages when Summon is active
+ if (tesla_summon) {
+ violation = true;
+ }
+
// Steering control: (0.1 * val) - 1638.35 in deg.
if (addr == 0x488) {
// We use 1/10 deg as a unit here
int raw_angle_can = ((GET_BYTE(to_send, 0) & 0x7FU) << 8) | GET_BYTE(to_send, 1);
int desired_angle = raw_angle_can - 16384;
int steer_control_type = GET_BYTE(to_send, 2) >> 6;
- bool steer_control_enabled = (steer_control_type != 0) && // NONE
- (steer_control_type != 3); // DISABLED
+ const int angle_ctrl_type = tesla_get_steer_ctrl_type(1);
+ const int lkas_ctrl_type = tesla_get_steer_ctrl_type(2);
+ bool steer_control_enabled = (steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
+ (steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
if (steer_angle_cmd_checks(desired_angle, steer_control_enabled, TESLA_STEERING_LIMITS)) {
violation = true;
}
+
+ bool valid_steer_control_type = (steer_control_type == 0) || // NONE
+ (steer_control_type == angle_ctrl_type) || // ANGLE_CONTROL
+ (steer_control_type == lkas_ctrl_type); // LANE_KEEP_ASSIST
+ if (!valid_steer_control_type) {
+ violation = true;
+ }
+
+ if (tesla_stock_steering_control) {
+ // Don't allow any steering commands when stock steering control is active (LDA, ELDA, Autopark)
+ violation = true;
+ }
}
// DAS_control: longitudinal control message
@@ -110,16 +185,16 @@ static bool tesla_tx_hook(const CANPacket_t *to_send) {
violation = true;
}
+ // Don't send long/cancel messages when the stock AEB system is active
+ if (tesla_stock_aeb) {
+ violation = true;
+ }
+
int raw_accel_max = ((GET_BYTE(to_send, 6) & 0x1FU) << 4) | (GET_BYTE(to_send, 5) >> 4);
int raw_accel_min = ((GET_BYTE(to_send, 5) & 0x0FU) << 5) | (GET_BYTE(to_send, 4) >> 3);
int acc_state = GET_BYTE(to_send, 1) >> 4;
if (tesla_longitudinal) {
- // Don't send messages when the stock AEB system is active
- if (tesla_stock_aeb) {
- violation = true;
- }
-
// Prevent both acceleration from being negative, as this could cause the car to reverse after coming to standstill
if ((raw_accel_max < TESLA_LONG_LIMITS.inactive_accel) && (raw_accel_min < TESLA_LONG_LIMITS.inactive_accel)) {
violation = true;
@@ -162,14 +237,22 @@ static int tesla_fwd_hook(CANPacket_t* to_send) {
if (bus_num == 2) {
bool block_msg = false;
- // DAS_steeringControl, APS_eacMonitor
- if ((addr == 0x488) || (addr == 0x27d)) {
- block_msg = true;
- }
- // DAS_control
- if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
- block_msg = true;
+ if (!tesla_summon) {
+ // APS_eacMonitor
+ if (addr == 0x27d) {
+ block_msg = true;
+ }
+
+ // DAS_steeringControl: block when no stock steering conflict
+ if ((addr == 0x488) && !tesla_stock_steering_control) {
+ block_msg = true;
+ }
+
+ // DAS_control
+ if (tesla_longitudinal && (addr == 0x2b9) && !tesla_stock_aeb) {
+ block_msg = true;
+ }
}
if (!block_msg) {
@@ -186,15 +269,25 @@ static safety_config tesla_init(uint16_t param) {
{0x488, 0, 4}, // DAS_steeringControl
{0x2b9, 0, 8}, // DAS_control
{0x27D, 0, 3}, // APS_eacMonitor
+ {0x3E9, 1, 8}, // DAS_bodyControls (blinker on vehicle bus)
};
- UNUSED(param);
+ const uint16_t TESLA_FLAG_FSD_14 = 2;
+ tesla_fsd_14 = GET_FLAG(param, TESLA_FLAG_FSD_14);
+
#ifdef ALLOW_DEBUG
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
#endif
tesla_stock_aeb = false;
+ tesla_stock_steering_control = false;
+ tesla_stock_steering_control_prev = false;
+ // we used to assume Summon on startup; instead we tolerate a short
+ // window without TX (DI_state comes at 10 Hz). Panda safety will
+ // reject TX until the first DI_state message clears tesla_summon.
+ tesla_summon = false;
+ tesla_summon_prev = false;
static RxCheck tesla_model3_y_rx_checks[] = {
{.msg = {{0x2b9, 2, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
@@ -214,4 +307,4 @@ const safety_hooks tesla_hooks = {
.rx = tesla_rx_hook,
.tx = tesla_tx_hook,
.fwd = tesla_fwd_hook,
-};
+};
\ No newline at end of file
diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py
index 940151bc4ea..a16cb0e833e 100644
--- a/selfdrive/car/car_specific.py
+++ b/selfdrive/car/car_specific.py
@@ -50,12 +50,13 @@ def __init__(self, CP: structs.CarParams):
self.mute_seatbelt = False
self.vCruise_prev = 250
self.carrotCruise_prev = False
+ self.tesla_lkas_button_prev = False
def update_params(self):
if self.frame % 100 == 0:
self.mute_seatbelt = self.params.get_bool("MuteSeatbelt")
self.mute_door = self.params.get_bool("MuteDoor")
-
+
def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
self.frame += 1
self.update_params()
@@ -172,6 +173,15 @@ def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
else:
events = self.create_common_events(CS, CS_prev)
+ # Tesla 3-finger infotainment press: toggle ExperimentalMode
+ if self.CP.brand == 'tesla':
+ lkas_pressed = any(b.type == ButtonType.lkas and b.pressed for b in CS.buttonEvents)
+ # Only allow toggling once the user has acknowledged the experimental mode warning (matches UI gate)
+ if lkas_pressed and not self.tesla_lkas_button_prev and self.params.get_bool("ExperimentalModeConfirmed"):
+ new_val = not self.params.get_bool("ExperimentalMode")
+ self.params.put_bool("ExperimentalMode", new_val)
+ self.tesla_lkas_button_prev = lkas_pressed
+
if CC.enabled:
if self.vCruise_prev == 0 and CS.vCruise > 0:
events.add(EventName.audioPrompt)
@@ -187,7 +197,7 @@ def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
allow_enable=True, allow_button_cancel=True):
events = Events()
-
+
if CS.doorOpen and not self.mute_door:
events.add(EventName.doorOpen)
if CS.seatbeltUnlatched and not self.mute_seatbelt:
@@ -282,4 +292,4 @@ def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extr
if CS.softHoldActive > 0:
events.add(EventName.softHold)
- return events
+ return events
\ No newline at end of file