From f5c2ceb8e7dafebe6ff6e3d3d6cadd68bd5639b6 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Thu, 7 Nov 2024 22:57:45 +0100 Subject: [PATCH] more wip on carstate --- opendbc/car/tesla/carstate.py | 104 +++++++++++++++++++++++----------- opendbc/car/tesla/values.py | 11 ++-- 2 files changed, 78 insertions(+), 37 deletions(-) diff --git a/opendbc/car/tesla/carstate.py b/opendbc/car/tesla/carstate.py index 0aea2b3caa..411c28fff2 100644 --- a/opendbc/car/tesla/carstate.py +++ b/opendbc/car/tesla/carstate.py @@ -4,59 +4,70 @@ from opendbc.car import structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase -from opendbc.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP +from opendbc.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, PLATFORM_3Y ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) + self.can_define = CANDefine(DBC[CP.carFingerprint]['party']) self.hands_on_level = 0 self.das_control = None - def update(self, cp, cp_cam, *_) -> structs.CarState: + def update(self, cp, cp_cam, cp_pt, *_) -> structs.CarState: ret = structs.CarState() # Vehicle speed - if self.CP.carFingerprint == CAR.TESLA_MODEL_S_RAVEN: + if self.CP.carFingerprint in PLATFORM_3Y: ret.vEgoRaw = cp.vl["DI_speed"]["DI_vehicleSpeed"] * CV.KPH_TO_MS - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) else: - ret.vEgoRaw = cp.vl["DI_speed"]["DI_vehicleSpeed"] * CV.KPH_TO_MS - ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.vEgoRaw = cp.vl["ESP_private1"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Gas pedal - pedal_status = cp.vl["DI_systemStatus"]["DI_accelPedalPos"] + if self.CP.carFingerprint in PLATFORM_3Y: + pedal_status = cp.vl["DI_systemStatus"]["DI_accelPedalPos"] + else: + pedal_status = cp_pt.vl["DI_torque1"]["DI_pedalPos"] ret.gas = pedal_status / 100.0 ret.gasPressed = (pedal_status > 0) # Brake pedal ret.brake = 0 - ret.brakePressed = cp.vl["IBST_status"]["IBST_driverBrakeApply"] == 2 + if self.CP.carFingerprint in PLATFORM_3Y: + ret.brakePressed = cp.vl["IBST_status"]["IBST_driverBrakeApply"] == 2 + else: + ret.brakePressed = cp.vl["IBST_private2"]["IBST_brakePedalApplied"] == 1 # Steering wheel - epas_status = cp.vl["EPAS3S_sysStatus"] - self.hands_on_level = epas_status["EPAS3S_handsOnLevel"] - ret.steeringAngleDeg = -epas_status["EPAS3S_internalSAS"] - ret.steeringRateDeg = -cp_cam.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"] - ret.steeringTorque = -epas_status["EPAS3S_torsionBarTorque"] + epas_name = "EPAS3S" if self.CP.carFingerprint in PLATFORM_3Y else "EPAS" + epas_status = cp.vl[f"{epas_name}_sysStatus"] + self.hands_on_level = epas_status[f"{epas_name}_handsOnLevel"] + ret.steeringAngleDeg = -epas_status[f"{epas_name}_internalSAS"] + if self.CP.carFingerprint in PLATFORM_3Y: + ret.steeringRateDeg = -cp_cam.vl["SCCM_steeringAngleSensor"]["SCCM_steeringAngleSpeed"] + else: + ret.steeringRateDeg = -cp.vl["STW_ANGLHP_STAT"]["StW_AnglHP_Spd"] + ret.steeringTorque = -epas_status[f"{epas_name}_torsionBarTorque"] ret.steeringPressed = self.hands_on_level > 0 - eac_status = self.can_define.dv["EPAS3S_sysStatus"]["EPAS3S_eacStatus"].get(int(epas_status["EPAS3S_eacStatus"]), None) + eac_status = self.can_define.dv[f"{epas_name}_sysStatus"][f"{epas_name}_eacStatus"].get(int(epas_status[f"{epas_name}_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.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) + di_state = cp.vl["DI_state"] if self.CP.carFingerprint in PLATFORM_3Y else cp_pt.vl["DI_state"] + # TODO: this needs a different can_define + cruise_state = self.can_define.dv["DI_state"]["DI_cruiseState"].get(int(di_state["DI_cruiseState"]), None) + speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(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 = cp.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS + ret.cruiseState.speed = 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.speed = 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.standstill = cruise_state == "STANDSTILL" @@ -90,24 +101,53 @@ def update(self, cp, cp_cam, *_) -> structs.CarState: @staticmethod def get_can_parser(CP): - messages = [ - # sig_address, frequency - ("DI_speed", 50), - ("DI_systemStatus", 100), - ("IBST_status", 25), - ("DI_state", 10), - ("EPAS3S_sysStatus", 100), - ("UI_warning", 10) - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], messages, CANBUS.party) + messages = [] + if CP.carFingerprint in PLATFORM_3Y: + messages += [ + ("DI_speed", 50), + ("DI_systemStatus", 100), + ("IBST_status", 25), + ("DI_state", 10), + ("EPAS3S_sysStatus", 100), + ("UI_warning", 10) + ] + elif CP.carFingerprint == CAR.TESLA_MODEL_S_RAVEN: + # TODO: verify frequencies + messages += [ + ("STW_ANGLHP_STAT", 100), + ("EPAS_sysStatus", 100), + ("ESP_private1", 50), + ("IBST_private2", 50), + ] + + return CANParser(DBC[CP.carFingerprint]['party'], messages, CANBUS.party) @staticmethod def get_cam_can_parser(CP): messages = [ ("DAS_control", 25), ("DAS_status", 2), - ("SCCM_steeringAngleSensor", 100), ] - return CANParser(DBC[CP.carFingerprint]['pt'], messages, CANBUS.autopilot_party) + parser = None + if CP.carFingerprint in PLATFORM_3Y: + messages += [ + ("SCCM_steeringAngleSensor", 100), + ] + parser = CANParser(DBC[CP.carFingerprint]['party'], messages, CANBUS.autopilot_party) + elif CP.carFingerprint == CAR.TESLA_MODEL_S_RAVEN: + parser = CANParser(DBC[CP.carFingerprint]['powertrain'], messages, CANBUS.autopilot_powertrain) + + return parser + + @staticmethod + def get_adas_can_parser(CP): + # this is not ADAS, but that's the next in line... + if CP.carFingerprint != CAR.TESLA_MODEL_S_RAVEN: + return None + + messages = [ + ("DI_torque1", 100), + ("DI_state", 10), + ] + return CANParser(DBC[CP.carFingerprint]['powertrain'], messages, CANBUS.powertrain) diff --git a/opendbc/car/tesla/values.py b/opendbc/car/tesla/values.py index b5180c2bd3..4a8a2146e3 100644 --- a/opendbc/car/tesla/values.py +++ b/opendbc/car/tesla/values.py @@ -1,7 +1,7 @@ from enum import IntFlag from opendbc.car.structs import CarParams from opendbc.car import structs -from opendbc.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict +from opendbc.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms from opendbc.car.docs_definitions import CarDocs from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries @@ -11,24 +11,25 @@ class CAR(Platforms): TESLA_MODEL_3 = PlatformConfig( [CarDocs("Tesla Model 3", "All")], CarSpecs(mass=1899., wheelbase=2.875, steerRatio=12.0), - dbc_dict('tesla_model3_party', None) + {'party': 'tesla_model3_party'}, ) TESLA_MODEL_Y = PlatformConfig( [CarDocs("Tesla Model Y", "All")], CarSpecs(mass=2072., wheelbase=2.890, steerRatio=12.0), - dbc_dict('tesla_model3_party', None) + {'party': 'tesla_model3_party'}, ) TESLA_MODEL_S_RAVEN = PlatformConfig( [CarDocs("Tesla Model S Raven", "All")], CarSpecs(mass=2100., wheelbase=2.959, steerRatio=15.0), { - 'pt': 'tesla_powertrain', - 'party': 'tesla_raven_party', 'chassis': 'tesla_can', + 'party': 'tesla_raven_party', + 'powertrain': 'tesla_powertrain', 'radar': 'tesla_radar_continental_generated', }, ) +PLATFORM_3Y = (CAR.TESLA_MODEL_3, CAR.TESLA_MODEL_Y) FW_QUERY_CONFIG = FwQueryConfig( requests=[