rm tesla (#33300)
* rm tesla
* more rm
* ugh we should remove dynamic imports soon
old-commit-hash: b9dec5e3b5
pull/33314/head
Shane Smiskol
9 months ago
committed by
GitHub
parent
4ece8b6bb1
commit
f8ef09fcb2
17 changed files with 5 additions and 587 deletions
@ -1,66 +0,0 @@ |
|||||||
from opendbc.can.packer import CANPacker |
|
||||||
from openpilot.selfdrive.car import apply_std_steer_angle_limits |
|
||||||
from openpilot.selfdrive.car.helpers import clip |
|
||||||
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(CarControllerBase): |
|
||||||
def __init__(self, dbc_name, CP): |
|
||||||
super().__init__(dbc_name, CP) |
|
||||||
self.apply_angle_last = 0 |
|
||||||
self.packer = CANPacker(dbc_name) |
|
||||||
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) |
|
||||||
self.tesla_can = TeslaCAN(self.packer, self.pt_packer) |
|
||||||
|
|
||||||
def update(self, CC, CS, now_nanos): |
|
||||||
actuators = CC.actuators |
|
||||||
pcm_cancel_cmd = CC.cruiseControl.cancel |
|
||||||
|
|
||||||
can_sends = [] |
|
||||||
|
|
||||||
# Temp disable steering on a hands_on_fault, and allow for user override |
|
||||||
hands_on_fault = CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3 |
|
||||||
lkas_enabled = CC.latActive and not hands_on_fault |
|
||||||
|
|
||||||
if self.frame % 2 == 0: |
|
||||||
if lkas_enabled: |
|
||||||
# Angular rate limit based on speed |
|
||||||
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) |
|
||||||
|
|
||||||
# To not fault the EPS |
|
||||||
apply_angle = clip(apply_angle, CS.out.steeringAngleDeg - 20, CS.out.steeringAngleDeg + 20) |
|
||||||
else: |
|
||||||
apply_angle = CS.out.steeringAngleDeg |
|
||||||
|
|
||||||
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) |
|
||||||
if self.CP.openpilotLongitudinalControl: |
|
||||||
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())) |
|
||||||
|
|
||||||
# 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)) |
|
||||||
|
|
||||||
# TODO: HUD control |
|
||||||
|
|
||||||
new_actuators = actuators.as_builder() |
|
||||||
new_actuators.steeringAngleDeg = self.apply_angle_last |
|
||||||
|
|
||||||
self.frame += 1 |
|
||||||
return new_actuators, can_sends |
|
||||||
@ -1,140 +0,0 @@ |
|||||||
import copy |
|
||||||
from collections import deque |
|
||||||
from cereal import car |
|
||||||
from opendbc.can.parser import CANParser |
|
||||||
from opendbc.can.can_define import CANDefine |
|
||||||
from openpilot.selfdrive.car.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS |
|
||||||
from openpilot.selfdrive.car.interfaces import CarStateBase |
|
||||||
|
|
||||||
|
|
||||||
class CarState(CarStateBase): |
|
||||||
def __init__(self, CP): |
|
||||||
super().__init__(CP) |
|
||||||
self.button_states = {button.event_type: False for button in BUTTONS} |
|
||||||
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) |
|
||||||
|
|
||||||
def update(self, cp, cp_cam, *_): |
|
||||||
ret = car.CarState.new_message() |
|
||||||
|
|
||||||
# Vehicle speed |
|
||||||
ret.vEgoRaw = cp.vl["ESP_B"]["ESP_vehicleSpeed"] * CV.KPH_TO_MS |
|
||||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
|
||||||
ret.standstill = (ret.vEgo < 0.1) |
|
||||||
|
|
||||||
# Gas pedal |
|
||||||
ret.gas = cp.vl["DI_torque1"]["DI_pedalPos"] / 100.0 |
|
||||||
ret.gasPressed = (ret.gas > 0) |
|
||||||
|
|
||||||
# Brake pedal |
|
||||||
ret.brake = 0 |
|
||||||
ret.brakePressed = bool(cp.vl["BrakeMessage"]["driverBrakeStatus"] != 1) |
|
||||||
|
|
||||||
# Steering wheel |
|
||||||
epas_status = cp_cam.vl["EPAS3P_sysStatus"] if self.CP.carFingerprint == CAR.TESLA_MODELS_RAVEN else cp.vl["EPAS_sysStatus"] |
|
||||||
|
|
||||||
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.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 |
|
||||||
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 |
|
||||||
|
|
||||||
# Gear |
|
||||||
ret.gearShifter = GEAR_MAP[self.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")] |
|
||||||
|
|
||||||
# Buttons |
|
||||||
buttonEvents = [] |
|
||||||
for button in BUTTONS: |
|
||||||
state = (cp.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) |
|
||||||
self.button_states[button.event_type] = state |
|
||||||
ret.buttonEvents = buttonEvents |
|
||||||
|
|
||||||
# Doors |
|
||||||
ret.doorOpen = any((self.can_define.dv["GTW_carState"][door].get(int(cp.vl["GTW_carState"][door]), "OPEN") == "OPEN") for door in DOORS) |
|
||||||
|
|
||||||
# Blinkers |
|
||||||
ret.leftBlinker = (cp.vl["GTW_carState"]["BC_indicatorLStatus"] == 1) |
|
||||||
ret.rightBlinker = (cp.vl["GTW_carState"]["BC_indicatorRStatus"] == 1) |
|
||||||
|
|
||||||
# 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) |
|
||||||
|
|
||||||
# TODO: blindspot |
|
||||||
|
|
||||||
# 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"]) |
|
||||||
|
|
||||||
return ret |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_can_parser(CP): |
|
||||||
messages = [ |
|
||||||
# sig_address, frequency |
|
||||||
("ESP_B", 50), |
|
||||||
("DI_torque1", 100), |
|
||||||
("DI_torque2", 100), |
|
||||||
("STW_ANGLHP_STAT", 100), |
|
||||||
("EPAS_sysStatus", 25), |
|
||||||
("DI_state", 10), |
|
||||||
("STW_ACTN_RQ", 10), |
|
||||||
("GTW_carState", 10), |
|
||||||
("BrakeMessage", 50), |
|
||||||
] |
|
||||||
|
|
||||||
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) |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def get_cam_can_parser(CP): |
|
||||||
messages = [ |
|
||||||
# sig_address, frequency |
|
||||||
("DAS_control", 40), |
|
||||||
] |
|
||||||
|
|
||||||
if CP.carFingerprint == CAR.TESLA_MODELS_RAVEN: |
|
||||||
messages.append(("EPAS3P_sysStatus", 100)) |
|
||||||
|
|
||||||
return CANParser(DBC[CP.carFingerprint]['chassis'], messages, CANBUS.autopilot_chassis) |
|
||||||
@ -1,32 +0,0 @@ |
|||||||
from cereal import car |
|
||||||
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', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x730, None): [ |
|
||||||
b'\x10#\x01', |
|
||||||
], |
|
||||||
}, |
|
||||||
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', |
|
||||||
], |
|
||||||
(Ecu.eps, 0x730, None): [ |
|
||||||
b'SX_0.0.0 (99),SR013.7', |
|
||||||
], |
|
||||||
}, |
|
||||||
} |
|
||||||
@ -1,40 +0,0 @@ |
|||||||
#!/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 import get_safety_config |
|
||||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
|
||||||
|
|
||||||
|
|
||||||
class CarInterface(CarInterfaceBase): |
|
||||||
@staticmethod |
|
||||||
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): |
|
||||||
ret.carName = "tesla" |
|
||||||
|
|
||||||
# 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.steerControlType = car.CarParams.SteerControlType.angle |
|
||||||
|
|
||||||
ret.longitudinalActuatorDelay = 0.5 # s |
|
||||||
ret.radarTimeStep = (1.0 / 8) # 8Hz |
|
||||||
|
|
||||||
# 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 |
|
||||||
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.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, flags)] |
|
||||||
|
|
||||||
ret.steerLimitTimer = 1.0 |
|
||||||
ret.steerActuatorDelay = 0.25 |
|
||||||
return ret |
|
||||||
@ -1,90 +0,0 @@ |
|||||||
#!/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 |
|
||||||
@ -1,94 +0,0 @@ |
|||||||
import crcmod |
|
||||||
|
|
||||||
from openpilot.selfdrive.car.conversions import Conversions as CV |
|
||||||
from openpilot.selfdrive.car.tesla.values import CANBUS, CarControllerParams |
|
||||||
|
|
||||||
|
|
||||||
class TeslaCAN: |
|
||||||
def __init__(self, packer, pt_packer): |
|
||||||
self.packer = packer |
|
||||||
self.pt_packer = pt_packer |
|
||||||
self.crc = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff) |
|
||||||
|
|
||||||
@staticmethod |
|
||||||
def checksum(msg_id, dat): |
|
||||||
# TODO: get message ID from name instead |
|
||||||
ret = (msg_id & 0xFF) + ((msg_id >> 8) & 0xFF) |
|
||||||
ret += sum(dat) |
|
||||||
return ret & 0xFF |
|
||||||
|
|
||||||
def create_steering_control(self, angle, enabled, counter): |
|
||||||
values = { |
|
||||||
"DAS_steeringAngleRequest": -angle, |
|
||||||
"DAS_steeringHapticRequest": 0, |
|
||||||
"DAS_steeringControlType": 1 if enabled else 0, |
|
||||||
"DAS_steeringControlCounter": counter, |
|
||||||
} |
|
||||||
|
|
||||||
data = self.packer.make_can_msg("DAS_steeringControl", CANBUS.chassis, 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) |
|
||||||
|
|
||||||
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, |
|
||||||
"DAS_aebEvent": 0, |
|
||||||
"DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN, |
|
||||||
"DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX, |
|
||||||
"DAS_accelMin": min_accel, |
|
||||||
"DAS_accelMax": max_accel, |
|
||||||
"DAS_controlCounter": cnt, |
|
||||||
"DAS_controlChecksum": 0, |
|
||||||
} |
|
||||||
|
|
||||||
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 |
|
||||||
@ -1,98 +0,0 @@ |
|||||||
from collections import namedtuple |
|
||||||
|
|
||||||
from cereal import car |
|
||||||
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(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_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') |
|
||||||
) |
|
||||||
|
|
||||||
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], |
|
||||||
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], |
|
||||||
whitelist_ecus=[Ecu.adas, Ecu.electricBrakeBooster, Ecu.fwdRadar], |
|
||||||
rx_offset=0x10, |
|
||||||
bus=0, |
|
||||||
), |
|
||||||
] |
|
||||||
) |
|
||||||
|
|
||||||
class CANBUS: |
|
||||||
# Lateral harness |
|
||||||
chassis = 0 |
|
||||||
radar = 1 |
|
||||||
autopilot_chassis = 2 |
|
||||||
|
|
||||||
# Longitudinal harness |
|
||||||
powertrain = 4 |
|
||||||
private = 5 |
|
||||||
autopilot_powertrain = 6 |
|
||||||
|
|
||||||
GEAR_MAP = { |
|
||||||
"DI_GEAR_INVALID": car.CarState.GearShifter.unknown, |
|
||||||
"DI_GEAR_P": car.CarState.GearShifter.park, |
|
||||||
"DI_GEAR_R": car.CarState.GearShifter.reverse, |
|
||||||
"DI_GEAR_N": car.CarState.GearShifter.neutral, |
|
||||||
"DI_GEAR_D": car.CarState.GearShifter.drive, |
|
||||||
"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]), |
|
||||||
] |
|
||||||
|
|
||||||
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]) |
|
||||||
JERK_LIMIT_MAX = 8 |
|
||||||
JERK_LIMIT_MIN = -8 |
|
||||||
ACCEL_TO_SPEED_MULTIPLIER = 3 |
|
||||||
|
|
||||||
def __init__(self, CP): |
|
||||||
pass |
|
||||||
|
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map() |
|
||||||
Loading…
Reference in new issue