sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Whitespace-only changes.
@@ -0,0 +1,69 @@
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.lateral import apply_steer_angle_limits_vm
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.tesla.teslacan import TeslaCAN
|
||||
from opendbc.car.tesla.values import CarControllerParams
|
||||
from opendbc.car.vehicle_model import VehicleModel
|
||||
from opendbc.sunnypilot.car.tesla.coop_steering import CoopSteeringCarController
|
||||
|
||||
|
||||
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 opendbc.car.tesla.interface import CarInterface
|
||||
return CarInterface.get_non_essential_params("TESLA_MODEL_Y")
|
||||
|
||||
|
||||
class CarController(CarControllerBase, CoopSteeringCarController):
|
||||
def __init__(self, dbc_names, CP, CP_SP):
|
||||
CarControllerBase.__init__(self, dbc_names, CP, CP_SP)
|
||||
CoopSteeringCarController.__init__(self)
|
||||
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())
|
||||
|
||||
def update(self, CC, CC_SP, CS, now_nanos):
|
||||
CoopSteeringCarController.update(self, self.CP_SP)
|
||||
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 % 2 == 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.apply_angle_last, lat_active, self.coop_steering.control_type))
|
||||
|
||||
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 else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
|
||||
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
cntr = (self.frame // 4) % 8
|
||||
can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CS.out.vEgo, CC.longActive))
|
||||
|
||||
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))
|
||||
|
||||
# TODO: HUD control
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.steeringAngleDeg = self.apply_angle_last
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
@@ -0,0 +1,154 @@
|
||||
import copy
|
||||
from opendbc.can import CANDefine, CANParser
|
||||
from opendbc.car import Bus, 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.teslacan import get_steer_ctrl_type
|
||||
from opendbc.car.tesla.values import DBC, CANBUS, GEAR_MAP, STEER_THRESHOLD, TeslaFlags
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.carstate_ext import CarStateExt
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarState(CarStateBase, CarStateExt):
|
||||
def __init__(self, CP, CP_SP):
|
||||
CarStateBase.__init__(self, CP, CP_SP)
|
||||
CarStateExt.__init__(self, CP, CP_SP)
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
|
||||
self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"]
|
||||
|
||||
self.autopark = False
|
||||
self.autopark_prev = False
|
||||
self.cruise_enabled_prev = False
|
||||
self.fsd14_error_logged = False
|
||||
self.suspected_fsd14 = False
|
||||
|
||||
self.hands_on_level = 0
|
||||
self.das_control = None
|
||||
|
||||
def update_autopark_state(self, autopark_state: str, cruise_enabled: bool):
|
||||
autopark_now = autopark_state in ("ACTIVE", "COMPLETE", "SELFPARK_STARTED")
|
||||
if autopark_now and not self.autopark_prev and not self.cruise_enabled_prev:
|
||||
self.autopark = True
|
||||
if not autopark_now:
|
||||
self.autopark = False
|
||||
self.autopark_prev = autopark_now
|
||||
self.cruise_enabled_prev = cruise_enabled
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
cp_party = can_parsers[Bus.party]
|
||||
cp_ap_party = can_parsers[Bus.ap_party]
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
# 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)
|
||||
|
||||
# Gas pedal
|
||||
ret.gasPressed = cp_party.vl["DI_systemStatus"]["DI_accelPedalPos"] > 0
|
||||
|
||||
# Brake pedal
|
||||
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"]
|
||||
|
||||
# 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)
|
||||
|
||||
autopark_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.update_autopark_state(autopark_state, cruise_enabled)
|
||||
|
||||
# Match panda safety cruise engaged logic
|
||||
ret.cruiseState.enabled = cruise_enabled and not self.autopark
|
||||
if speed_units == "KPH":
|
||||
ret.cruiseState.speed = max(cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS, 1e-3)
|
||||
elif speed_units == "MPH":
|
||||
ret.cruiseState.speed = max(cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS, 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"
|
||||
|
||||
# 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
|
||||
# On FSD 14+, ANGLE_CONTROL behavior changed to allow user winddown while actuating.
|
||||
# FSD switched from using ANGLE_CONTROL to LANE_KEEP_ASSIST to likely keep the old steering override disengage logic.
|
||||
# LKAS switched from LANE_KEEP_ASSIST to ANGLE_CONTROL to likely allow overriding LKAS events smoothly
|
||||
lkas_ctrl_type = get_steer_ctrl_type(self.CP.flags, 2)
|
||||
ret.stockLkas = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == lkas_ctrl_type # LANE_KEEP_ASSIST
|
||||
|
||||
# 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_settings"]["DAS_autosteerEnabled"] != 0
|
||||
|
||||
# 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+
|
||||
angle_control = cp_ap_party.vl["DAS_steeringControl"]["DAS_steeringControlType"] == 1 # ANGLE_CONTROL
|
||||
if not ret.invalidLkasSetting and angle_control and not self.CP.flags & TeslaFlags.FSD_14:
|
||||
self.suspected_fsd14 = True
|
||||
|
||||
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
|
||||
|
||||
# Buttons # ToDo: add Gap adjust button
|
||||
|
||||
# Messages needed by carcontroller
|
||||
self.das_control = copy.copy(cp_ap_party.vl["DAS_control"])
|
||||
|
||||
CarStateExt.update(self, ret, ret_sp, can_parsers)
|
||||
|
||||
return ret, ret_sp
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_SP):
|
||||
return {
|
||||
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_SP),
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.values import CAR
|
||||
from opendbc.sunnypilot.car.fingerprints_ext import merge_fw_versions
|
||||
from opendbc.sunnypilot.car.tesla.fingerprints_ext import FW_VERSIONS_EXT
|
||||
|
||||
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',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
FW_VERSIONS = merge_fw_versions(FW_VERSIONS, FW_VERSIONS_EXT)
|
||||
@@ -0,0 +1,70 @@
|
||||
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
|
||||
|
||||
from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP, TeslaSafetyFlagsSP
|
||||
|
||||
|
||||
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
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_sp(stock_cp: structs.CarParams, ret: structs.CarParamsSP, candidate, fingerprint: dict[int, dict[int, int]],
|
||||
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_sp: bool, docs: bool) -> structs.CarParamsSP:
|
||||
|
||||
stock_cp.enableBsm = True
|
||||
|
||||
if candidate == CAR.TESLA_MODEL_X:
|
||||
stock_cp.dashcamOnly = False
|
||||
|
||||
if 0x3DF in fingerprint[1]:
|
||||
ret.flags |= TeslaFlagsSP.HAS_VEHICLE_BUS.value
|
||||
ret.safetyParam |= TeslaSafetyFlagsSP.HAS_VEHICLE_BUS
|
||||
|
||||
return ret
|
||||
@@ -0,0 +1,86 @@
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.interfaces import RadarInterfaceBase
|
||||
from opendbc.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_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
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']
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
@@ -0,0 +1,60 @@
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.tesla.values import CANBUS, CarControllerParams, TeslaFlags
|
||||
|
||||
|
||||
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
|
||||
|
||||
def create_steering_control(self, angle, enabled, control_type):
|
||||
# On FSD 14+, ANGLE_CONTROL behavior changed to allow user winddown while actuating.
|
||||
# with openpilot, after overriding w/ ANGLE_CONTROL the wheel snaps back to the original angle abruptly
|
||||
# so we now use LANE_KEEP_ASSIST to match stock FSD.
|
||||
# see carstate.py for more details
|
||||
values = {
|
||||
"DAS_steeringAngleRequest": -angle,
|
||||
"DAS_steeringHapticRequest": 0,
|
||||
"DAS_steeringControlType": get_steer_ctrl_type(self.CP.flags, control_type if enabled else 0),
|
||||
}
|
||||
|
||||
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
|
||||
|
||||
def create_longitudinal_command(self, acc_state, accel, counter, 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": 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 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
|
||||
Whitespace-only changes.
@@ -0,0 +1,104 @@
|
||||
import re
|
||||
import unittest
|
||||
|
||||
from opendbc.car import gen_empty_fingerprint
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.tesla.interface import CarInterface
|
||||
from opendbc.car.tesla.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.tesla.radar_interface import RADAR_START_ADDR
|
||||
from opendbc.car.tesla.values import CAR, FSD_14_FW
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# Fields prefixed unknown_* we observe structurally but don't know the meaning of.
|
||||
# Only `platform` has evidence-backed semantic meaning (matches car_model in FW_VERSIONS).
|
||||
#
|
||||
# unknown_prefix is everything before the comma; we don't split it because we don't know what its
|
||||
# parts mean, but observed shape is: <family>_<package>_<triplet> (<build>), e.g.
|
||||
# TeMYG4 _ Main _ 0.0.0 (78) or TeM3 _ SP_XP002p2 _ 0.0.0 (23)
|
||||
# family package triplet build family package triplet build
|
||||
#
|
||||
# After the comma, the version string decomposes into:
|
||||
# platform : E/Y/X = car model (Model 3 / Y / X). The only field with known meaning.
|
||||
# variant_code : differentiator WITHIN a platform — hardware/trim/calibration bits packed
|
||||
# into <digit?><letters?><3-digit series>, e.g. '4HP015', '4003', 'L014',
|
||||
# 'PR003'. We don't fully know what the parts mean individually, but the
|
||||
# whole string identifies a specific variant within the car model.
|
||||
# software_major/minor : numeric components after the first '.' — conventional release numbers.
|
||||
# minor is optional (e.g. 'E4S014.27' has no minor).
|
||||
#
|
||||
# Suspected (not confirmed): for M3/MY, `TeM3_*` outer + no-leading-digit variant_code == HW3, and
|
||||
# `TeMYG4_*` outer + leading-'4' variant_code == HW4 (the 'G4' in TeMYG4 likely denotes Gen 4).
|
||||
#
|
||||
# Example full parse of 'TeMYG4_Main_0.0.0 (78),E4HP015.05.0':
|
||||
# unknown_prefix='TeMYG4_Main_0.0.0 (78)'
|
||||
# platform=E variant_code=4HP015 software_major=05 software_minor=0
|
||||
FW_RE = re.compile(
|
||||
rb'^(?P<unknown_prefix>.+),' +
|
||||
rb'(?P<platform>[EYX])' +
|
||||
rb'(?P<variant_code>\d?[A-Z]*\d{3})' +
|
||||
rb'\.(?P<software_major>\d+)' +
|
||||
rb'(?:\.(?P<software_minor>\d+))?$'
|
||||
)
|
||||
|
||||
PLATFORM_TO_CAR = {
|
||||
b'E': CAR.TESLA_MODEL_3,
|
||||
b'Y': CAR.TESLA_MODEL_Y,
|
||||
b'X': CAR.TESLA_MODEL_X,
|
||||
}
|
||||
|
||||
# Hypothesized FSD 14 profile, in terms of variant_code bookends (given software_major >= 4):
|
||||
# M3: variant_code starts with '4H', ends with '015'
|
||||
# MY: variant_code starts with '4', ends with '003'
|
||||
# Older series (M3 '014', MY '002') are never FSD 14.
|
||||
FSD_14_FW_RULE = {
|
||||
CAR.TESLA_MODEL_3: (b'4H', b'015'),
|
||||
CAR.TESLA_MODEL_Y: (b'4', b'003'),
|
||||
}
|
||||
|
||||
|
||||
class TestTeslaFingerprint(unittest.TestCase):
|
||||
def test_fw_platform_code(self):
|
||||
# Every EPS FW must parse and its platform letter must match the car it's filed under.
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
for fw in ecus.get((Ecu.eps, 0x730, None), []):
|
||||
m = FW_RE.match(fw)
|
||||
|
||||
assert m is not None, f"Unparsable FW: {fw}"
|
||||
assert PLATFORM_TO_CAR[m['platform']] == car_model, f"Platform letter {m['platform']!r} != {car_model.value}: {fw}"
|
||||
|
||||
def test_fsd_14_fw(self):
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
if car_model not in FSD_14_FW_RULE:
|
||||
continue
|
||||
|
||||
variant_prefix, variant_suffix = FSD_14_FW_RULE[car_model]
|
||||
for fw in ecus.get((Ecu.eps, 0x730, None), []):
|
||||
m = FW_RE.match(fw)
|
||||
assert m is not None, f"Unparsable FW: {fw}"
|
||||
|
||||
is_fsd_14 = fw in FSD_14_FW.get(car_model, [])
|
||||
expected = (
|
||||
m['variant_code'].startswith(variant_prefix)
|
||||
and m['variant_code'].endswith(variant_suffix)
|
||||
and int(m['software_major']) >= 4
|
||||
)
|
||||
assert is_fsd_14 == expected, f"{fw}"
|
||||
|
||||
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
|
||||
@@ -0,0 +1,142 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
||||
from opendbc.car.lateral import AngleSteeringLimitsVM
|
||||
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 " +
|
||||
"<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 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',
|
||||
b'TeMYG4_Main_0.0.0 (77),E4H015.04.5',
|
||||
b'TeMYG4_Main_0.0.0 (78),E4H015.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',
|
||||
b'TeMYG4_Main_0.0.0 (78),Y4003.06.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,
|
||||
}
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ANGLE_LIMITS: AngleSteeringLimitsVM = AngleSteeringLimitsVM(
|
||||
# EPAS faults above this angle
|
||||
360, # deg
|
||||
# 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
|
||||
|
||||
|
||||
class TeslaSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
FSD_14 = 2
|
||||
|
||||
|
||||
class TeslaFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
FSD_14 = 2
|
||||
MISSING_DAS_SETTINGS = 4
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
|
||||
STEER_THRESHOLD = 1
|
||||
Reference in new issue
Block a user