From 8a2766a86b1426387f63a4a545f2a23828c05b5b Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Wed, 6 Mar 2024 21:14:48 +0100 Subject: [PATCH] Tesla Raven (#29947) * fingerprinting * wip * bug * fix another bug * fix rebase * clean up raven * forgot to save * one more rename * one more rename * radar fixes * AP1 also has bosch radar * put back dashcamOnly * small fixes * raven flag * fix bug * fix raven flag * bump opendbc * fix radar trigger for non-raven * fix tests? * bump panda * more test fixes * tesla fingerprinting is a bit slower now * fix tests * bump opendbc * bump submodules to master --------- Co-authored-by: Comma Device old-commit-hash: 7177ec06312c574aeb21dd261004be44bda5ba29 --- opendbc | 2 +- panda | 2 +- release/files_common | 3 +- selfdrive/car/fw_query_definitions.py | 5 ++ selfdrive/car/tesla/carstate.py | 30 ++++++++--- selfdrive/car/tesla/fingerprints.py | 11 ++++ selfdrive/car/tesla/interface.py | 11 ++-- selfdrive/car/tesla/radar_interface.py | 62 +++++++++++----------- selfdrive/car/tesla/values.py | 34 +++++++----- selfdrive/car/tests/routes.py | 1 + selfdrive/car/tests/test_fw_fingerprint.py | 5 +- selfdrive/car/torque_data/override.toml | 1 + 12 files changed, 105 insertions(+), 62 deletions(-) diff --git a/opendbc b/opendbc index 1745ab518..ff1f1ff33 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 1745ab51825055cd18748013c4a5e3377319e390 +Subproject commit ff1f1ff335261c469635c57c81817afd04663eab diff --git a/panda b/panda index ea156f7c6..41e9610ff 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ea156f7c628a371bea9a15a29f9068d5392534ba +Subproject commit 41e9610ff841e4cf62051c6df09c1870f5d12477 diff --git a/release/files_common b/release/files_common index c0be9d3ca..4286c46c9 100644 --- a/release/files_common +++ b/release/files_common @@ -542,7 +542,8 @@ opendbc/vw_golf_mk4.dbc opendbc/vw_mqb_2010.dbc opendbc/tesla_can.dbc -opendbc/tesla_radar.dbc +opendbc/tesla_radar_bosch_generated.dbc +opendbc/tesla_radar_continental_generated.dbc opendbc/tesla_powertrain.dbc tinygrad_repo/openpilot/compile2.py diff --git a/selfdrive/car/fw_query_definitions.py b/selfdrive/car/fw_query_definitions.py index 80492b417..236ade49b 100755 --- a/selfdrive/car/fw_query_definitions.py +++ b/selfdrive/car/fw_query_definitions.py @@ -47,6 +47,11 @@ class StdQueries: MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER) + SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ + p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER) + UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \ p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \ diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 2cb4f09d7..bee652ff3 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/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) diff --git a/selfdrive/car/tesla/fingerprints.py b/selfdrive/car/tesla/fingerprints.py index 772ca59ef..9b6f3865b 100644 --- a/selfdrive/car/tesla/fingerprints.py +++ b/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', + ], + }, } diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index 537433a35..f98988673 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from openpilot.selfdrive.car.tesla.values import CANBUS +from openpilot.selfdrive.car.tesla.values import CANBUS, CAR from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -28,19 +28,20 @@ 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 + 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 - return ret def _update(self, c): diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index b3e7c7fcb..599ab3105 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/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']: diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index f33e62618..43132970b 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -1,8 +1,7 @@ from collections import namedtuple -from dataclasses import dataclass, field from cereal import car -from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict from openpilot.selfdrive.car.docs_definitions import CarInfo from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries @@ -10,24 +9,25 @@ Ecu = car.CarParams.Ecu Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) - -@dataclass -class TeslaPlatformConfig(PlatformConfig): - dbc_dict: DbcDict = field(default_factory=lambda: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can')) - - class CAR(Platforms): - AP1_MODELS = TeslaPlatformConfig( + AP1_MODELS = PlatformConfig( 'TESLA AP1 MODEL S', CarInfo("Tesla AP1 Model S", "All"), - CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0) + CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0), + dbc_dict('tesla_powertrain', 'tesla_radar_bosch_generated', chassis_dbc='tesla_can') ) - AP2_MODELS = TeslaPlatformConfig( + AP2_MODELS = PlatformConfig( 'TESLA AP2 MODEL S', CarInfo("Tesla AP2 Model S", "All"), - AP1_MODELS.specs + AP1_MODELS.specs, + AP1_MODELS.dbc_dict + ) + MODELS_RAVEN = PlatformConfig( + 'TESLA MODEL S RAVEN', + CarInfo("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=[ @@ -38,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], @@ -48,7 +55,6 @@ FW_QUERY_CONFIG = FwQueryConfig( ] ) - class CANBUS: # Lateral harness chassis = 0 diff --git a/selfdrive/car/tests/routes.py b/selfdrive/car/tests/routes.py index ff747b593..92ee7fa92 100755 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -289,6 +289,7 @@ routes = [ CarTestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS), CarTestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS), + CarTestRoute("66c1699b7697267d/2024-03-03--13-09-53", TESLA.MODELS_RAVEN), # Segments that test specific issues # Controls mismatch due to interceptor threshold diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index a8bae2939..b9eadc8cd 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -263,7 +263,7 @@ class TestFwFingerprintTiming(unittest.TestCase): print(f'get_vin {name} case, query time={self.total_time / self.N} seconds') def test_fw_query_timing(self): - total_ref_time = {1: 8.3, 2: 9.2} + total_ref_time = {1: 8.4, 2: 9.3} brand_ref_times = { 1: { 'gm': 1.0, @@ -275,13 +275,14 @@ class TestFwFingerprintTiming(unittest.TestCase): 'mazda': 0.1, 'nissan': 0.8, 'subaru': 0.45, - 'tesla': 0.2, + 'tesla': 0.3, 'toyota': 1.6, 'volkswagen': 0.65, }, 2: { 'ford': 1.6, 'hyundai': 1.85, + 'tesla': 0.3, } } diff --git a/selfdrive/car/torque_data/override.toml b/selfdrive/car/torque_data/override.toml index 3cf17f317..3de33b88d 100644 --- a/selfdrive/car/torque_data/override.toml +++ b/selfdrive/car/torque_data/override.toml @@ -18,6 +18,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"] # Tesla has high torque "TESLA AP1 MODEL S" = [nan, 2.5, nan] "TESLA AP2 MODEL S" = [nan, 2.5, nan] +"TESLA MODEL S RAVEN" = [nan, 2.5, nan] # Guess "FORD BRONCO SPORT 1ST GEN" = [nan, 1.5, nan]