Handle stock AEB and active steering avoidance by disengaging ope…

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

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

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',
+    ],
+  },
+}