Release 260111

2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
--- a/opendbc_repo/opendbc/car/tesla/carstate.py
+++ b/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)
+    }