Release 260111

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -0,0 +1,55 @@
+import numpy as np
+from opendbc.can import CANPacker
+from opendbc.car import Bus, apply_std_steer_angle_limits
+from opendbc.car.interfaces import CarControllerBase
+from opendbc.car.tesla.teslacan import TeslaCAN
+from opendbc.car.tesla.values import CarControllerParams
+
+
+class CarController(CarControllerBase):
+  def __init__(self, dbc_names, CP):
+    super().__init__(dbc_names, CP)
+    self.apply_angle_last = 0
+    self.packer = CANPacker(dbc_names[Bus.party])
+    self.tesla_can = TeslaCAN(self.packer)
+
+  def update(self, CC, CS, now_nanos):
+    actuators = CC.actuators
+    can_sends = []
+
+    # Disengage and allow for user override on high torque inputs
+    # TODO: move this to a generic disengageRequested carState field and set CC.cruiseControl.cancel based on it
+    hands_on_fault = CS.hands_on_level >= 3
+    cruise_cancel = CC.cruiseControl.cancel or hands_on_fault
+    lat_active = CC.latActive and not hands_on_fault
+
+    if self.frame % 2 == 0:
+      # Angular rate limit based on speed
+      self.apply_angle_last = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo,
+                                                           CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS)
+
+      can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_last, lat_active, (self.frame // 2) % 16))
+
+    if self.frame % 10 == 0:
+      can_sends.append(self.tesla_can.create_steering_allowed((self.frame // 10) % 16))
+
+    # Longitudinal control
+    if self.CP.openpilotLongitudinalControl:
+      if self.frame % 4 == 0:
+        state = 13 if cruise_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 cruise_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

opendbc_repo/opendbc/car/tesla/carstate.py

@@ -0,0 +1,101 @@
+import copy
+from opendbc.can import CANDefine, CANParser
+from opendbc.car import Bus, structs
+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
+
+ButtonType = structs.CarState.ButtonEvent.Type
+
+
+class CarState(CarStateBase):
+  def __init__(self, CP):
+    super().__init__(CP)
+    self.can_define = CANDefine(DBC[CP.carFingerprint][Bus.party])
+    self.shifter_values = self.can_define.dv["DI_systemStatus"]["DI_gear"]
+
+    self.hands_on_level = 0
+    self.das_control = None
+
+  def update(self, can_parsers) -> structs.CarState:
+    cp_party = can_parsers[Bus.party]
+    cp_ap_party = can_parsers[Bus.ap_party]
+    ret = structs.CarState()
+
+    # 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
+    pedal_status = cp_party.vl["DI_systemStatus"]["DI_accelPedalPos"]
+    ret.gas = pedal_status / 100.0
+    ret.gasPressed = pedal_status > 0
+
+    # Brake pedal
+    ret.brake = 0
+    ret.brakePressed = cp_party.vl["IBST_status"]["IBST_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"]
+
+    # This matches stock logic, but with halved minimum frames (0.25-0.3s)
+    ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > STEER_THRESHOLD, 15)
+
+    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"
+
+    # 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)
+
+    ret.cruiseState.enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
+    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 = cruise_state == "STANDSTILL"
+    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
+
+    # Stock Autosteer should be off (includes FSD)
+    ret.invalidLkasSetting = cp_ap_party.vl["DAS_settings"]["DAS_autosteerEnabled"] != 0
+
+    # Buttons # ToDo: add Gap adjust button
+
+    # Messages needed by carcontroller
+    self.das_control = copy.copy(cp_ap_party.vl["DAS_control"])
+
+    return ret
+
+  @staticmethod
+  def get_can_parsers(CP):
+    return {
+      Bus.party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.party),
+      Bus.ap_party: CANParser(DBC[CP.carFingerprint][Bus.party], [], CANBUS.autopilot_party)
+    }

opendbc_repo/opendbc/car/tesla/fingerprints.py

@@ -0,0 +1,37 @@
+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'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_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: {
+    (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_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 (5),Y4003.03.2',
+      b'TeMYG4_Legacy3Y_0.0.0 (2),Y4P003.02.0',
+      b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
+    ],
+  },
+}

opendbc_repo/opendbc/car/tesla/interface.py

@@ -0,0 +1,34 @@
+from opendbc.car import 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
+
+
+class CarInterface(CarInterfaceBase):
+  CarState = CarState
+  CarController = CarController
+
+  @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 = 1.0
+    ret.steerActuatorDelay = 0.1
+    ret.steerAtStandstill = True
+
+    ret.steerControlType = structs.CarParams.SteerControlType.angle
+    ret.radarUnavailable = True
+
+    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

opendbc_repo/opendbc/car/tesla/teslacan.py

@@ -0,0 +1,66 @@
+from opendbc.car.common.conversions import Conversions as CV
+from opendbc.car.tesla.values import CANBUS, CarControllerParams
+
+
+class TeslaCAN:
+  def __init__(self, packer):
+    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):
+    from opendbc.car.interfaces import V_CRUISE_MAX
+    set_speed = max(v_ego * CV.MS_TO_KPH, 0)
+    if active:
+      # TODO: this causes jerking after gas override when above set speed
+      set_speed = 0 if accel < 0 else V_CRUISE_MAX
+
+    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

opendbc_repo/opendbc/car/tesla/values.py

@@ -0,0 +1,111 @@
+from dataclasses import dataclass, field
+from enum import Enum, IntFlag
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleSteeringLimits
+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 " +
+    "<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)
+
+
+@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])
+
+
+@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])
+
+
+@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),
+  )
+  TESLA_MODEL_Y = TeslaPlatformConfig(
+    [
+      TeslaCarDocsHW3("Tesla Model Y (with HW3) 2020-23"),
+      TeslaCarDocsHW4("Tesla Model Y (with HW4) 2024"),
+     ],
+    CarSpecs(mass=2072., wheelbase=2.890, 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,
+    )
+  ]
+)
+
+
+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: AngleSteeringLimits = AngleSteeringLimits(
+    # EPAS faults above this angle
+    360,  # deg
+    # Angle rate limits are set using the Tesla Model Y VehicleModel such that they maximally meet ISO 11270
+    # At 5 m/s, FSD has been seen hitting up to ~4 deg/frame with ~5 deg/frame at very low creeping speeds
+    # At 30 m/s, FSD has been seen hitting mostly 0.1 deg/frame, sometimes 0.2 deg/frame, and rarely 0.3 deg/frame
+    ([0., 5., 25.], [2.5, 1.5, 0.2]),
+    ([0., 5., 25.], [5., 2.0, 0.3]),
+  )
+
+  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
+
+
+DBC = CAR.create_dbc_map()
+
+STEER_THRESHOLD = 0.5