wip

selfdrive/car/tesla/carcontroller.py

@@ -1,11 +1,12 @@
 from openpilot.common.numpy_fast import clip
 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
 
 
-class CarController:
+class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.frame = 0

selfdrive/car/tesla/carstate.py

@@ -2,7 +2,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 DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
+from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
 from openpilot.selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
@@ -37,13 +37,15 @@ class CarState(CarStateBase):
     ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1)
 
     # Steering wheel
-    self.hands_on_level = cp.vl["EPAS_sysStatus"]["EPAS_handsOnLevel"]
-    self.steer_warning = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacErrorCode"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacErrorCode"]), None)
-    steer_status = self.can_define.dv["EPAS_sysStatus"]["EPAS_eacStatus"].get(int(cp.vl["EPAS_sysStatus"]["EPAS_eacStatus"]), None)
+    epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.MODELS_RAVEN else cp.vl["EPAS_sysStatus"]
 
-    ret.steeringAngleDeg = -cp.vl["EPAS_sysStatus"]["EPAS_internalSAS"]
+    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 = -cp.vl["EPAS_sysStatus"]["EPAS_torsionBarTorque"]
+    ret.steeringTorque = -epas_status["EPAS_torsionBarTorque"]
     ret.steeringPressed = (self.hands_on_level > 0)
     ret.steerFaultPermanent = steer_status == "EAC_FAULT"
     ret.steerFaultTemporary = (self.steer_warning not in ("EAC_ERROR_IDLE", "EAC_ERROR_HANDS_ON"))
@@ -85,7 +87,10 @@ class CarState(CarStateBase):
     ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1)
 
     # Seatbelt
-    ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
+    if self.CP.carFingerprint == CAR.MODELS_RAVEN:
+      ret.seatbeltUnlatched = (cp.vl["DriverSeat"]["buckleStatus"] != 1)
+    else:
+      ret.seatbeltUnlatched = (cp.vl["SDM1"]["SDM_bcklDrivStatus"] != 1)
 
     # TODO: blindspot
 
@@ -111,9 +116,14 @@ class CarState(CarStateBase):
       ("DI_state", 10),
       ("STW_ACTN_RQ", 10),
       ("GTW_carState", 10),
-      ("SDM1", 10),
       ("BrakeMessage", 50),
     ]
+
+    if CP.carFingerprint == CAR.MODELS_RAVEN:
+      messages.append(("DriverSeat", 20))
+    else:
+      messages.append(("SDM1", 10))
+
     return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.chassis)
 
   @staticmethod
@@ -122,4 +132,8 @@ class CarState(CarStateBase):
       # sig_address, frequency
       ("DAS_control", 40),
     ]
+
+    if CP.carFingerprint == CAR.MODELS_RAVEN:
+      messages.append(("EPAS3P_sysStatus", 100))
+
     return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis)

selfdrive/car/tesla/fingerprints.py

@@ -25,4 +25,15 @@ FW_VERSIONS = {
       b'\x10#\x01',
     ],
   },
+  CAR.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',
+    ],
+    (Ecu.eps, 0x730, None): [
+      b'SX_0.0.0 (99),SR013.7',
+    ],
+  },
 }

selfdrive/car/tesla/interface.py

@@ -8,7 +8,7 @@ from openpilot.selfdrive.car.interfaces import CarInterfaceBase
 
 class CarInterface(CarInterfaceBase):
   @staticmethod
-  def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs):
+  def _get_params(ret, params, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs):
     ret.carName = "tesla"
 
     # There is no safe way to do steer blending with user torque,
@@ -28,33 +28,26 @@ class CarInterface(CarInterfaceBase):
 
     # 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.MODELS_RAVEN else 0)
     if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
-      ret.openpilotLongitudinalControl = True and not params.get_bool("DisableOpenpilotLongitudinal")
+      ret.openpilotLongitudinalControl = not disable_openpilot_long
+      flags |= Panda.FLAG_TESLA_LONG_CONTROL
       ret.safetyConfigs = [
-        get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL),
-        get_safety_config(car.CarParams.SafetyModel.tesla, Panda.FLAG_TESLA_LONG_CONTROL | Panda.FLAG_TESLA_POWERTRAIN),
+        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.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)]
 
     ret.steerLimitTimer = 1.0
     ret.steerActuatorDelay = 0.25
-
-    if candidate in (CAR.AP2_MODELS, CAR.AP1_MODELS):
-      ret.mass = 2100.
-      ret.wheelbase = 2.959
-      ret.centerToFront = ret.wheelbase * 0.5
-      ret.steerRatio = 15.0
-    else:
-      raise ValueError(f"Unsupported car: {candidate}")
-
     return ret
 
   def _update(self, c, frogpilot_variables):
     ret = self.CS.update(self.cp, self.cp_cam, frogpilot_variables)
 
-    ret.events = self.create_common_events(ret, frogpilot_variables).to_msg()
+    ret.events = self.create_common_events(ret).to_msg()
 
     return ret
 

selfdrive/car/tesla/radar_interface.py

@@ -1,38 +1,33 @@
 #!/usr/bin/env python3
 from cereal import car
 from opendbc.can.parser import CANParser
-from openpilot.selfdrive.car.tesla.values import DBC, CANBUS
+from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS
 from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
 
-RADAR_MSGS_A = list(range(0x310, 0x36E, 3))
-RADAR_MSGS_B = list(range(0x311, 0x36F, 3))
-NUM_POINTS = len(RADAR_MSGS_A)
-
-def get_radar_can_parser(CP):
-  # Status messages
-  messages = [
-    ('TeslaRadarSguInfo', 10),
-  ]
-
-  # Radar tracks. There are also raw point clouds available,
-  # we don't use those.
-  for i in range(NUM_POINTS):
-    msg_id_a = RADAR_MSGS_A[i]
-    msg_id_b = RADAR_MSGS_B[i]
-    messages.extend([
-      (msg_id_a, 8),
-      (msg_id_b, 8),
-    ])
-
-  return CANParser(DBC[CP.carFingerprint]['radar'], messages, CANBUS.radar)
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
-    self.rcp = get_radar_can_parser(CP)
+    self.CP = CP
+
+    if CP.carFingerprint == CAR.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
-    self.trigger_msg = RADAR_MSGS_B[-1]
 
   def update(self, can_strings):
     if self.rcp is None:
@@ -48,17 +43,24 @@ class RadarInterface(RadarInterfaceBase):
 
     # Errors
     errors = []
-    sgu_info = self.rcp.vl['TeslaRadarSguInfo']
     if not self.rcp.can_valid:
       errors.append('canError')
-    if sgu_info['RADC_HWFail'] or sgu_info['RADC_SGUFail'] or sgu_info['RADC_SensorDirty']:
-      errors.append('fault')
+
+    if self.CP.carFingerprint == CAR.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(NUM_POINTS):
-      msg_a = self.rcp.vl[RADAR_MSGS_A[i]]
-      msg_b = self.rcp.vl[RADAR_MSGS_B[i]]
+    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']:

selfdrive/car/tesla/values.py

@@ -1,32 +1,33 @@
 from collections import namedtuple
-from enum import StrEnum
-from typing import Dict, List, Union
 
 from cereal import car
-from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
-from openpilot.selfdrive.car.docs_definitions import CarInfo
+from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict
+from openpilot.selfdrive.car.docs_definitions import CarDocs
 from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
 
 Ecu = car.CarParams.Ecu
 
 Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 
-
-class CAR(StrEnum):
-  AP1_MODELS = 'TESLA AP1 MODEL S'
-  AP2_MODELS = 'TESLA AP2 MODEL S'
-
-
-CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = {
-  CAR.AP1_MODELS: CarInfo("Tesla AP1 Model S", "All"),
-  CAR.AP2_MODELS: CarInfo("Tesla AP2 Model S", "All"),
-}
-
-
-DBC = {
-  CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
-  CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
-}
+class CAR(Platforms):
+  AP1_MODELS = PlatformConfig(
+    'TESLA AP1 MODEL S',
+    [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')
+  )
+  AP2_MODELS = PlatformConfig(
+    'TESLA AP2 MODEL S',
+    [CarDocs("Tesla AP2 Model S", "All")],
+    AP1_MODELS.specs,
+    AP1_MODELS.dbc_dict
+  )
+  MODELS_RAVEN = PlatformConfig(
+    'TESLA MODEL S RAVEN',
+    [CarDocs("Tesla Model S Raven", "All")],
+    AP1_MODELS.specs,
+    dbc_dict('tesla_powertrain', 'tesla_radar_continental_generated', chassis_dbc='tesla_can')
+  )
 
 FW_QUERY_CONFIG = FwQueryConfig(
   requests=[
@@ -37,6 +38,13 @@ FW_QUERY_CONFIG = FwQueryConfig(
       rx_offset=0x08,
       bus=0,
     ),
+    Request(
+      [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
+      [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
+      whitelist_ecus=[Ecu.eps],
+      rx_offset=0x08,
+      bus=0,
+    ),
     Request(
       [StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
       [StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
@@ -47,7 +55,6 @@ FW_QUERY_CONFIG = FwQueryConfig(
   ]
 )
 
-
 class CANBUS:
   # Lateral harness
   chassis = 0
@@ -89,3 +96,6 @@ class CarControllerParams:
 
   def __init__(self, CP):
     pass
+
+
+DBC = CAR.create_dbc_map()