Tesla: set enableBsm flag for 3 and Y (#403)

Set enableBsm flag for Tesla 3Y

selfdrive/car/fingerprints.py

@@ -276,9 +276,8 @@ MIGRATION = {
   "SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022,
   "SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023,
   "SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023,
-  'TESLA AP1 MODEL S': TESLA.TESLA_AP1_MODELS,
-  'TESLA AP2 MODEL S': TESLA.TESLA_AP2_MODELS,
-  'TESLA MODEL S RAVEN': TESLA.TESLA_MODELS_RAVEN,
+  'TESLA AP3 MODEL 3': TESLA.TESLA_AP3_MODEL3,
+  'TESLA AP3 MODEL Y': TESLA.TESLA_AP3_MODELY,
   "TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2,
   "TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON,
   "TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019,

selfdrive/car/sunnypilot_carname.json

@@ -224,6 +224,8 @@
   "Škoda Octavia Scout 2017-19": "SKODA_OCTAVIA_MK3",
   "Škoda Scala 2020-23": "SKODA_KAMIQ_MK1",
   "Škoda Superb 2015-22": "SKODA_SUPERB_MK3",
+  "Tesla AP3 Model 3": "TESLA_AP3_MODEL3",
+  "Tesla AP3 Model Y": "TESLA_AP3_MODELY",
   "Toyota Alphard 2019-20": "TOYOTA_ALPHARD_TSS2",
   "Toyota Alphard Hybrid 2021": "TOYOTA_ALPHARD_TSS2",
   "Toyota Avalon 2016": "TOYOTA_AVALON",

selfdrive/car/tesla/carcontroller.py

@@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker
 from openpilot.selfdrive.car import apply_std_steer_angle_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN
-from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
+from openpilot.selfdrive.car.tesla.values import DBC, CarControllerParams
 
 
 class CarController(CarControllerBase):
@@ -37,25 +37,26 @@ class CarController(CarControllerBase):
       self.apply_angle_last = apply_angle
       can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, (self.frame // 2) % 16))
 
-    # Longitudinal control (in sync with stock message, about 40Hz)
+    # Longitudinal control
     if self.CP.openpilotLongitudinalControl:
+      acc_state = CS.das_control["DAS_accState"]
       target_accel = actuators.accel
       target_speed = max(CS.out.vEgo + (target_accel * CarControllerParams.ACCEL_TO_SPEED_MULTIPLIER), 0)
       max_accel = 0 if target_accel < 0 else target_accel
       min_accel = 0 if target_accel > 0 else target_accel
 
-      while len(CS.das_control_counters) > 0:
-        can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, CS.das_control_counters.popleft()))
+      counter = CS.das_control["DAS_controlCounter"]
+      can_sends.append(self.tesla_can.create_longitudinal_commands(acc_state, target_speed, min_accel, max_accel, counter))
 
     # Cancel on user steering override, since there is no steering torque blending
     if hands_on_fault:
       pcm_cancel_cmd = True
 
-    if self.frame % 10 == 0 and pcm_cancel_cmd:
-      # Spam every possible counter value, otherwise it might not be accepted
-      for counter in range(16):
-        can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.chassis, counter))
-        can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, pcm_cancel_cmd, CANBUS.autopilot_chassis, counter))
+    # Sent cancel request only if ACC is enabled
+    if self.frame % 10 == 0 and pcm_cancel_cmd and CS.acc_enabled:
+      counter = int(CS.sccm_right_stalk_counter)
+      can_sends.append(self.tesla_can.right_stalk_press((counter + 1) % 16 , 1))  # half up (cancel acc)
+      can_sends.append(self.tesla_can.right_stalk_press((counter + 2) % 16, 0))  # to prevent neutral gear warning
 
     # TODO: HUD control
 

selfdrive/car/tesla/carstate.py

@@ -1,8 +1,7 @@
 import copy
-from collections import deque
 from cereal import car
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
+from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, BUTTONS
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -14,13 +13,13 @@ class CarState(CarStateBase):
     self.can_define = CANDefine(DBC[CP.carFingerprint]['chassis'])
 
     # Needed by carcontroller
-    self.msg_stw_actn_req = None
     self.hands_on_level = 0
     self.steer_warning = None
-    self.acc_state = 0
-    self.das_control_counters = deque(maxlen=32)
+    self.acc_enabled = None
+    self.sccm_right_stalk_counter = None
+    self.das_control = None
 
-  def update(self, cp, cp_cam):
+  def update(self, cp, cp_cam, cp_adas):
     ret = car.CarState.new_message()
 
     # Vehicle speed
@@ -29,78 +28,74 @@ class CarState(CarStateBase):
     ret.standstill = (ret.vEgo < 0.1)
 
     # Gas pedal
-    ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0
-    ret.gasPressed = (ret.gas > 0)
+    pedal_status = cp.vl["DI_systemStatus"]["DI_accelPedalPos"]
+    ret.gas = pedal_status / 100.0
+    ret.gasPressed = (pedal_status > 0)
 
     # Brake pedal
     ret.brake = 0
-    ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1)
+    ret.brakePressed = cp.vl["IBST_status"]["IBST_driverBrakeApply"] == 2
 
     # Steering wheel
-    epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN else cp.vl["EPAS_sysStatus"]
+    epas_status = cp.vl["EPAS3S_sysStatus"]
+    self.hands_on_level = epas_status["EPAS3S_handsOnLevel"]
+    self.steer_warning = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacErrorCode"].get(int(epas_status["EPAS3S_eacErrorCode"]), None)
+    ret.steeringAngleDeg = -epas_status["EPAS3S_internalSAS"]
+    ret.steeringRateDeg = -cp_adas.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"]
+    ret.steeringTorque = -epas_status["EPAS3S_torsionBarTorque"]
 
-    self.hands_on_level = epas_status["EPAS_handsOnLevel"]
-    self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(epas_status["EPAS_eacErrorCode"]), None)
-    steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(epas_status["EPAS_eacStatus"]), None)
-
-    ret.steeringAngleDeg = -epas_status["EPAS_internalSAS"]
-    ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] # This is from a different angle sensor, and at different rate
-    ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"]
     ret.steeringPressed = (self.hands_on_level > 0)
-    ret.steerFaultPermanent = steer_status == "EAC_FAULT"
+    ret.steerFaultPermanent = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacStatus"].get(int(epas_status["EPAS3S_eacStatus"]), None) in ["EAC_FAULT"]
     ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON"))
 
     # Cruise state
     cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(cp.vl["DI_state"]["DI_cruiseState"]), None)
     speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp.vl["DI_state"]["DI_speedUnits"]), None)
 
-    acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
-
-    ret.cruiseState.enabled = acc_enabled
+    self.acc_enabled = (cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
+    ret.cruiseState.enabled = self.acc_enabled
     if speed_units == "KPH":
       ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
     elif speed_units == "MPH":
       ret.cruiseState.speed = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
     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.cruiseState.standstill = False  # This needs to be false, since we can resume from stop without sending anything special
 
     # Gear
-    ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
+    ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_systemStatus"]["DI_gear"].get(int(cp.vl["DI_systemStatus"]["DI_gear"]), "DI_GEAR_INVALID")]
 
-    # Buttons
-    buttonEvents = []
+    button_events = []
     for button in BUTTONS:
-      state = (cp.vl[button.can_addr][button.can_msg] in button.values)
+      state = (cp_adas.vl[button.can_addr][button.can_msg] in button.values)
       if self.button_states[button.event_type] != state:
         event = car.CarState.ButtonEvent.new_message()
         event.type = button.event_type
         event.pressed = state
-        buttonEvents.append(event)
+        button_events.append(event)
       self.button_states[button.event_type] = state
-    ret.buttonEvents = buttonEvents
+    self.button_events = button_events
 
     # Doors
-    ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS)
+    ret.doorOpen = (cp.vl["UI_warning"]["anyDoorOpen"] == 1)
 
     # Blinkers
-    ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1)
-    ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1)
+    ret.leftBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorLeftRequest"] != 0)
+    ret.rightBlinker = (cp_adas.vl["ID3F5VCFRONT_lighting"]["VCFRONT_indicatorRightRequest"] != 0)
 
     # Seatbelt
-    if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1)
-    else:
-      ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
+    ret.seatbeltUnlatched = cp.vl["UI_warning"]["buckleStatus"] != 1
 
-    # TODO: blindspot
+    # Blindspot
+    if self.CP.enableBsm:
+      ret.leftBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearLeft"] != 0
+      ret.rightBlindspot = cp_cam.vl["DAS_status"]["DAS_blindSpotRearRight"] != 0
 
     # AEB
     ret.stockAeb = (cp_cam.vl["DAS_control"]["DAS_aebEvent"] == 1)
 
     # Messages needed by carcontroller
-    self.msg_stw_actn_req = copy.copy(cp.vl["STW_ACTN_RQ"])
-    self.acc_state = cp_cam.vl["DAS_control"]["DAS_accState"]
-    self.das_control_counters.extend(cp_cam.vl_all["DAS_control"]["DAS_controlCounter"])
+    self.sccm_right_stalk_counter = copy.copy(cp_adas.vl["SCCM_rightStalk"]["SCCM_rightStalkCounter"])
+    self.das_control = copy.copy(cp_cam.vl["DAS_control"])
 
     return ret
 
@@ -109,31 +104,33 @@ class CarState(CarStateBase):
     messages = [
       # sig_address, frequency
       ("ESP_B", 50),
-      ("DI_torque1", 100),
-      ("DI_torque2", 100),
-      ("STW_ANGLHP_STAT", 100),
-      ("EPAS_sysStatus", 25),
+      ("DI_systemStatus", 100),
+      ("IBST_status", 25),
       ("DI_state", 10),
-      ("STW_ACTN_RQ", 10),
-      ("GTW_carState", 10),
-      ("BrakeMessage", 50),
+      ("EPAS3S_sysStatus", 100),
+      ("UI_warning", 10)
     ]
 
-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages.append(("DriverSeat", 20))
-    else:
-      messages.append(("SDM1", 10))
-
-    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis)
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.party)
 
   @staticmethod
   def get_cam_can_parser(CP):
     messages = [
-      # sig_address, frequency
-      ("DAS_control", 40),
+      ("DAS_control", 25),
+      ("DAS_status", 2)
     ]
 
-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages.append(("EPAS3P_sysStatus", 100))
+    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_party)
 
-    return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis)
+  @staticmethod
+  def get_adas_can_parser(CP):  # Vehicle Can on Model 3
+    messages = [
+      ("VCLEFT_switchStatus", 20),
+      ("SCCM_leftStalk", 10),
+      ("SCCM_rightStalk", 10),
+      ("SCCM_steeringAngleSensor", 100),
+      ("DAS_bodyControls", 2),
+      ("ID3F5VCFRONT_lighting", 10),
+    ]
+
+    return CANParser(DBC[CP.carFingerprint]["pt"], messages, CANBUS.vehicle)

selfdrive/car/tesla/fingerprints.py

@@ -4,29 +4,50 @@ from openpilot.selfdrive.car.tesla.values import CAR
 Ecu = car.CarParams.Ecu
 
 FW_VERSIONS = {
-  CAR.TESLA_AP2_MODELS: {
-    (Ecu.adas, 0x649, None): [
-      b'\x01\x00\x8b\x07\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x11',
-    ],
-    (Ecu.electricBrakeBooster, 0x64d, None): [
-      b'1037123-00-A',
-    ],
-    (Ecu.fwdRadar, 0x671, None): [
-      b'\x01\x00W\x00\x00\x00\x07\x00\x00\x00\x00\x08\x01\x00\x00\x00\x07\xff\xfe',
-    ],
+  CAR.TESLA_AP3_MODEL3: {
     (Ecu.eps, 0x730, None): [
-      b'\x10#\x01',
+      b'TeM3_E014p10_0.0.0 (16),E014.17.00',
+      b'TeMYG4_DCS_Update_0.0.0 (13),E4014.28.1',
+      b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0',
+      b'TeMYG4_SingleECU_0.0.0 (33),E4S014.27',
+    ],
+    (Ecu.engine, 0x606, None): [
+      b'\x01\x00\x05\x18A\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x000\x91',
+      b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x11',
+      b'\x01\x00\x05\x1cF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\r',
+      b'\x01\x00\x05\x1eK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xcc',
+      b'\x01\x00\x05\x1eO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xe0',
+      b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x05',
+      b'\x01\x00\x05 O\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x99',
+      b'\x01\x00\x05 O\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xbd',
+      b'\x01\x00\x05 D\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xe1',
+      b'\x01\x00\x05 K\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xa4',
+      b'\x01\x00\x05 N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xa7',
+      b'\x01\x00\x05 N\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xf2',
+      b'\x01\x00\x05 [\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd8',
     ],
   },
-  CAR.TESLA_MODELS_RAVEN: {
-    (Ecu.electricBrakeBooster, 0x64d, None): [
-      b'1037123-00-A',
-    ],
-    (Ecu.fwdRadar, 0x671, None): [
-      b'\x01\x00\x99\x02\x01\x00\x10\x00\x00AP8.3.03\x00\x10',
-    ],
+  CAR.TESLA_AP3_MODELY: {
     (Ecu.eps, 0x730, None): [
-      b'SX_0.0.0 (99),SR013.7',
+      b'TeM3_E014p10_0.0.0 (16),Y002.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 (9),Y4P002.25.0',
+      b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1',
+      b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26'
+    ],
+    (Ecu.engine, 0x606, None): [
+      b'\x01\x00\x05\x1cO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\x12',
+      b'\x01\x00\x05\x1eO\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x002\xe2',
+      b'\x01\x00\x05\x1e[\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x06',
+      b'\x01\x00\x05 D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd3',
+      b'\x01\x00\x05 O\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\x9a',
+      b'\x01\x00\x05 m\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xd5',
+      b'\x01\x00\x05 [\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xd7',
+      b'\x01\x00\x05 ^\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00Z\xae',
+      b'\x01\x00\x05 \\\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x003\xe7',
+      b'\x01\x00\x055p\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00;\xc5',
     ],
   },
 }

selfdrive/car/tesla/interface.py

@@ -1,12 +1,17 @@
 #!/usr/bin/env python3
 from cereal import car
 from panda import Panda
-from openpilot.selfdrive.car.tesla.values import CANBUS, CAR
+from openpilot.selfdrive.car.tesla.values import CAR
 from openpilot.selfdrive.car import get_safety_config
 from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
+ButtonType = car.CarState.ButtonEvent.Type
+
 
 class CarInterface(CarInterfaceBase):
+  def __init__(self, CP, CarController, CarState):
+    super().__init__(CP, CarController, CarState)
+
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     ret.carName = "tesla"
@@ -14,34 +19,62 @@ class CarInterface(CarInterfaceBase):
     # There is no safe way to do steer blending with user torque,
     # so the steering behaves like autopilot. This is not
     # how openpilot should be, hence dashcamOnly
-    ret.dashcamOnly = True
+    # ret.dashcamOnly = True
 
     ret.steerControlType = car.CarParams.SteerControlType.angle
 
     ret.longitudinalActuatorDelay = 0.5 # s
-    ret.radarTimeStep = (1.0 / 8) # 8Hz
+    ret.radarUnavailable = True
 
-    # Check if we have messages on an auxiliary panda, and that 0x2bf (DAS_control) is present on the AP powertrain bus
-    # If so, we assume that it is connected to the longitudinal harness.
-    flags = (Panda.FLAG_TESLA_RAVEN if candidate == CAR.TESLA_MODELS_RAVEN else 0)
-    if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
-      ret.openpilotLongitudinalControl = True
+    if candidate in [CAR.TESLA_AP3_MODEL3, CAR.TESLA_AP3_MODELY]:
+      flags = Panda.FLAG_TESLA_MODEL3_Y
       flags |= Panda.FLAG_TESLA_LONG_CONTROL
-      ret.safetyConfigs = [
-        get_safety_config(car.CarParams.SafetyModel.tesla, flags),
-        get_safety_config(car.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN),
-      ]
-    else:
-      ret.openpilotLongitudinalControl = False
+      ret.openpilotLongitudinalControl = True
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)]
+      ret.enableBsm = True
 
     ret.steerLimitTimer = 1.0
     ret.steerActuatorDelay = 0.25
     return ret
 
   def _update(self, c):
-    ret = self.CS.update(self.cp, self.cp_cam)
+    ret = self.CS.update(self.cp, self.cp_cam, self.cp_adas)
 
-    ret.events = self.create_common_events(ret).to_msg()
+    if ret.cruiseState.available:
+      if self.enable_mads:
+        for b in self.CS.button_events:
+          if b.type == ButtonType.altButton1 and b.pressed:
+            self.CS.madsEnabled = True
+          elif b.type == ButtonType.altButton2 and b.pressed:
+            self.CS.madsEnabled = False
+        self.CS.madsEnabled = self.get_acc_mads(ret.cruiseState.enabled, self.CS.accEnabled, self.CS.madsEnabled)
+        self.CS.madsEnabled = False if self.CS.steer_warning == "EAC_ERROR_HANDS_ON" and self.CS.hands_on_level >= 3 else self.CS.madsEnabled
+    else:
+      self.CS.madsEnabled = False
+
+    if not self.CP.pcmCruise or (self.CP.pcmCruise and self.CP.minEnableSpeed > 0) or not self.CP.pcmCruiseSpeed:
+      if any(b.type == ButtonType.cancel for b in self.CS.button_events):
+        self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+    if self.get_sp_pedal_disengage(ret):
+      self.CS.madsEnabled, self.CS.accEnabled = self.get_sp_cancel_cruise_state(self.CS.madsEnabled)
+      ret.cruiseState.enabled = ret.cruiseState.enabled if not self.enable_mads else False if self.CP.pcmCruise else self.CS.accEnabled
+
+    if self.CP.pcmCruise and self.CP.minEnableSpeed > 0 and self.CP.pcmCruiseSpeed:
+      if ret.gasPressed and not ret.cruiseState.enabled:
+        self.CS.accEnabled = False
+      self.CS.accEnabled = ret.cruiseState.enabled or self.CS.accEnabled
+
+    ret, self.CS = self.get_sp_common_state(ret, self.CS)
+
+    ret.buttonEvents = [
+      *self.CS.button_events,
+      *self.button_events.create_mads_event(self.CS.madsEnabled, self.CS.out.madsEnabled)  # MADS BUTTON
+    ]
+
+    events = self.create_common_events(ret, c, pcm_enable=False)
+
+    events, ret = self.create_sp_events(self.CS, ret, events)
+
+    ret.events = events.to_msg()
 
     return ret

selfdrive/car/tesla/radar_interface.py

@@ -1,90 +1,4 @@
-#!/usr/bin/env python3
-from cereal import car
-from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
-
 class RadarInterface(RadarInterfaceBase):
-  def __init__(self, CP):
-    super().__init__(CP)
-
-    if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      messages = [('RadarStatus', 16)]
-      self.num_points = 40
-      self.trigger_msg = 1119
-    else:
-      messages = [('TeslaRadarSguInfo', 10)]
-      self.num_points = 32
-      self.trigger_msg = 878
-
-    for i in range(self.num_points):
-      messages.extend([
-        (f'RadarPoint{i}_A', 16),
-        (f'RadarPoint{i}_B', 16),
-      ])
-
-    self.rcp = CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar)
-    self.updated_messages = set()
-    self.track_id = 0
-
-  def update(self, can_strings):
-    if self.rcp is None:
-      return super().update(None)
-
-    values = self.rcp.update_strings(can_strings)
-    self.updated_messages.update(values)
-
-    if self.trigger_msg not in self.updated_messages:
-      return None
-
-    ret = car.RadarData.new_message()
-
-    # Errors
-    errors = []
-    if not self.rcp.can_valid:
-      errors.append('canError')
-
-    if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN:
-      radar_status = self.rcp.vl['RadarStatus']
-      if radar_status['sensorBlocked'] or radar_status['shortTermUnavailable'] or radar_status['vehDynamicsError']:
-        errors.append('fault')
-    else:
-      radar_status = self.rcp.vl['TeslaRadarSguInfo']
-      if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail'] or radar_status['RADC_SensorDirty']:
-        errors.append('fault')
-
-    ret.errors = errors
-
-    # Radar tracks
-    for i in range(self.num_points):
-      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
-
-      # Check if it's a valid track
-      if not msg_a['Tracked']:
-        if i in self.pts:
-          del self.pts[i]
-        continue
-
-      # New track!
-      if i not in self.pts:
-        self.pts[i] = car.RadarData.RadarPoint.new_message()
-        self.pts[i].trackId = self.track_id
-        self.track_id += 1
-
-      # Parse track data
-      self.pts[i].dRel = msg_a['LongDist']
-      self.pts[i].yRel = msg_a['LatDist']
-      self.pts[i].vRel = msg_a['LongSpeed']
-      self.pts[i].aRel = msg_a['LongAccel']
-      self.pts[i].yvRel = msg_b['LatSpeed']
-      self.pts[i].measured = bool(msg_a['Meas'])
-
-    ret.points = list(self.pts.values())
-    self.updated_messages.clear()
-    return ret
+  pass

selfdrive/car/tesla/teslacan.py

@@ -17,6 +17,15 @@ class TeslaCAN:
     ret += sum(dat)
     return ret & 0xFF
 
+  @staticmethod
+  def right_stalk_crc(dat):
+    right_stalk_val = [0x7C, 0xB6, 0xF0, 0x2F, 0x69, 0xA3, 0xDD, 0x1C, 0x56, 0x90, 0xCA, 0x09, 0x43, 0x7D, 0xB7, 0xF1]
+    cntr = dat[0] & 0xF
+    crc1_func = crcmod.mkCrcFun(0x12F, initCrc=0x00, xorOut=0xFF, rev=False)
+    crc1 = crc1_func(dat) & 0xFF
+    crc2_func = crcmod.mkCrcFun(0x12F, initCrc=crc1, xorOut=0xFF, rev=False)
+    return crc2_func(bytes([right_stalk_val[cntr]])) & 0xFF
+
   def create_steering_control(self, angle, enabled, counter):
     values = {
       "DAS_steeringAngleRequest": -angle,
@@ -25,56 +34,11 @@ class TeslaCAN:
       "DAS_steeringControlCounter": counter,
     }
 
-    data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, values)[1]
+    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.chassis, values)
-
-  def create_action_request(self, msg_stw_actn_req, cancel, bus, counter):
-    # We copy this whole message when spamming cancel
-    values = {s: msg_stw_actn_req[s] for s in [
-      "SpdCtrlLvr_Stat",
-      "VSL_Enbl_Rq",
-      "SpdCtrlLvrStat_Inv",
-      "DTR_Dist_Rq",
-      "TurnIndLvr_Stat",
-      "HiBmLvr_Stat",
-      "WprWashSw_Psd",
-      "WprWash_R_Sw_Posn_V2",
-      "StW_Lvr_Stat",
-      "StW_Cond_Flt",
-      "StW_Cond_Psd",
-      "HrnSw_Psd",
-      "StW_Sw00_Psd",
-      "StW_Sw01_Psd",
-      "StW_Sw02_Psd",
-      "StW_Sw03_Psd",
-      "StW_Sw04_Psd",
-      "StW_Sw05_Psd",
-      "StW_Sw06_Psd",
-      "StW_Sw07_Psd",
-      "StW_Sw08_Psd",
-      "StW_Sw09_Psd",
-      "StW_Sw10_Psd",
-      "StW_Sw11_Psd",
-      "StW_Sw12_Psd",
-      "StW_Sw13_Psd",
-      "StW_Sw14_Psd",
-      "StW_Sw15_Psd",
-      "WprSw6Posn",
-      "MC_STW_ACTN_RQ",
-      "CRC_STW_ACTN_RQ",
-    ]}
-
-    if cancel:
-      values["SpdCtrlLvr_Stat"] = 1
-      values["MC_STW_ACTN_RQ"] = counter
-
-    data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[1]
-    values["CRC_STW_ACTN_RQ"] = self.crc(data[:7])
-    return self.packer.make_can_msg("STW_ACTN_RQ", bus, values)
+    return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
 
   def create_longitudinal_commands(self, acc_state, speed, min_accel, max_accel, cnt):
-    messages = []
     values = {
       "DAS_setSpeed": speed * CV.MS_TO_KPH,
       "DAS_accState": acc_state,
@@ -86,9 +50,20 @@ class TeslaCAN:
       "DAS_controlCounter": cnt,
       "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)
 
-    for packer, bus in [(self.packer, CANBUS.chassis), (self.pt_packer, CANBUS.powertrain)]:
-      data = packer.make_can_msg("DAS_control", bus, values)[1]
-      values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
-      messages.append(packer.make_can_msg("DAS_control", bus, values))
-    return messages
+  def right_stalk_press(self, counter, position):
+    values = {
+      "SCCM_rightStalkCrc": 0,
+      "SCCM_rightStalkCounter": counter,
+      "SCCM_rightStalkStatus": position,
+      "SCCM_rightStalkReserved1": 0,
+      "SCCM_parkButtonStatus": 0,
+      "SCCM_rightStalkReserved2": 0,
+    }
+
+    data = self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)[1]
+    values["SCCM_rightStalkCrc"] = self.right_stalk_crc(data[1:])
+    return self.pt_packer.make_can_msg("SCCM_rightStalk", CANBUS.vehicle, values)

selfdrive/car/tesla/values.py

@@ -9,32 +9,22 @@ Ecu = car.CarParams.Ecu
 
 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 
+
 class CAR(Platforms):
-  TESLA_AP1_MODELS = PlatformConfig(
-    [CarDocs("Tesla AP1 Model S", "All")],
-    CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0),
-    dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can')
+  TESLA_AP3_MODEL3 = PlatformConfig(
+    [CarDocs("Tesla AP3 Model 3", "All")],
+    CarSpecs(mass=1899., wheelbase=2.875, steerRatio=12.0),
+    dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party')
   )
-  TESLA_AP2_MODELS = PlatformConfig(
-    [CarDocs("Tesla AP2 Model S", "All")],
-    TESLA_AP1_MODELS.specs,
-    TESLA_AP1_MODELS.dbc_dict
-  )
-  TESLA_MODELS_RAVEN = PlatformConfig(
-    [CarDocs("Tesla Model S Raven", "All")],
-    TESLA_AP1_MODELS.specs,
-    dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can')
+  TESLA_AP3_MODELY = PlatformConfig(
+    [CarDocs("Tesla AP3 Model Y", "All")],
+    CarSpecs(mass=2072., wheelbase=2.890, steerRatio=12.0),
+    dbc_dict('tesla_model3_vehicle', None, chassis_dbc='tesla_model3_party')
   )
 
+
 FW_QUERY_CONFIG = FwQueryConfig(
   requests=[
-    Request(
-      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
-      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
-      whitelist_ecus=[Ecu.eps],
-      rx_offset=0x08,
-      bus=0,
-    ),
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
@@ -45,23 +35,20 @@ FW_QUERY_CONFIG = FwQueryConfig(
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
-      whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar],
+      whitelist_ecus=[Ecu.engine],
       rx_offset=0x10,
-      bus=0,
+      bus=1,
+      obd_multiplexing=False,
     ),
   ]
 )
 
-class CANBUS:
-  # Lateral harness
-  chassis = 0
-  radar = 1
-  autopilot_chassis = 2
 
-  # Longitudinal harness
-  powertrain = 4
-  private = 5
-  autopilot_powertrain = 6
+class CANBUS:
+  party = 0
+  vehicle = 1
+  autopilot_party = 2
+
 
 GEAR_MAP = {
   "DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
@@ -72,18 +59,18 @@ GEAR_MAP = {
   "DI_GEAR_SNA": car.CarState.GearShifter.unknown,
 }
 
-DOORS = ["DOOR_STATE_FL", "DOOR_STATE_FR", "DOOR_STATE_RL", "DOOR_STATE_RR", "DOOR_STATE_FrontTrunk", "BOOT_STATE"]
-
-# Make sure the message and addr is also in the CAN parser!
 BUTTONS = [
-  Button(car.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]),
-  Button(car.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]),
-  Button(car.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]),
-  Button(car.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]),
-  Button(car.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]),
-  Button(car.CarState.ButtonEvent.Type.resumeCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [2]),
+  Button(car.CarState.ButtonEvent.Type.leftBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [3, 4]),
+  Button(car.CarState.ButtonEvent.Type.rightBlinker, "SCCM_leftStalk", "SCCM_turnIndicatorStalkStatus", [1, 2]),
+  Button(car.CarState.ButtonEvent.Type.accelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(1, 10))),
+  Button(car.CarState.ButtonEvent.Type.decelCruise, "VCLEFT_switchStatus", "VCLEFT_swcRightScrollTicks", list(range(-9, 0))),
+  Button(car.CarState.ButtonEvent.Type.cancel, "SCCM_rightStalk", "SCCM_rightStalkStatus", [1]),
+  Button(car.CarState.ButtonEvent.Type.resumeCruise, "SCCM_rightStalk", "SCCM_rightStalkStatus", [4]),
+  Button(car.CarState.ButtonEvent.Type.altButton1, "SCCM_rightStalk", "SCCM_rightStalkStatus", [3]),  # MADS engaged
+  Button(car.CarState.ButtonEvent.Type.altButton2, "SCCM_rightStalk", "SCCM_rightStalkStatus", [2]),  # MADS disengaged
 ]
 
+
 class CarControllerParams:
   ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 1.6, .3])
   ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[10., 7.0, 0.8])

selfdrive/car/torque_data/override.toml

@@ -16,9 +16,8 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
 "TOYOTA_RAV4_TSS2_2023" = [nan, 3.0, nan]
 
 # Tesla has high torque
-"TESLA_AP1_MODELS" = [nan, 2.5, nan]
-"TESLA_AP2_MODELS" = [nan, 2.5, nan]
-"TESLA_MODELS_RAVEN" = [nan, 2.5, nan]
+"TESLA_AP3_MODEL3" = [nan, 2.5, nan]
+"TESLA_AP3_MODELY" = [nan, 2.5, nan]
 
 # Guess
 "FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]