Skip to content

Commit

Permalink
more wip on carstate
Browse files Browse the repository at this point in the history
  • Loading branch information
@robbederks
robbederks committed Nov 7, 2024
1 parent b3dd21a commit f5c2ceb
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 37 deletions.
104 changes: 72 additions & 32 deletions opendbc/car/tesla/carstate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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)
11 changes: 6 additions & 5 deletions opendbc/car/tesla/values.py
View file
Original file line number Diff line number Diff line change
@@ -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

Expand All @@ -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=[
Expand Down

0 comments on commit f5c2ceb

Please sign in to comment.