Shane Smiskol
1 year ago
commit
cfb8453acc
63 changed files with 117 additions and 736 deletions
@ -0,0 +1,54 @@ |
||||
import numpy as np |
||||
|
||||
|
||||
def get_kalman_gain(dt, A, C, Q, R, iterations=100): |
||||
P = np.zeros_like(Q) |
||||
for _ in range(iterations): |
||||
P = A.dot(P).dot(A.T) + dt * Q |
||||
S = C.dot(P).dot(C.T) + R |
||||
K = P.dot(C.T).dot(np.linalg.inv(S)) |
||||
P = (np.eye(len(P)) - K.dot(C)).dot(P) |
||||
return K |
||||
|
||||
|
||||
class KF1D: |
||||
# this EKF assumes constant covariance matrix, so calculations are much simpler |
||||
# the Kalman gain also needs to be precomputed using the control module |
||||
|
||||
def __init__(self, x0, A, C, K): |
||||
self.x0_0 = x0[0][0] |
||||
self.x1_0 = x0[1][0] |
||||
self.A0_0 = A[0][0] |
||||
self.A0_1 = A[0][1] |
||||
self.A1_0 = A[1][0] |
||||
self.A1_1 = A[1][1] |
||||
self.C0_0 = C[0] |
||||
self.C0_1 = C[1] |
||||
self.K0_0 = K[0][0] |
||||
self.K1_0 = K[1][0] |
||||
|
||||
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0 |
||||
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1 |
||||
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0 |
||||
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1 |
||||
|
||||
# K matrix needs to be pre-computed as follow: |
||||
# import control |
||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R) |
||||
# self.K = np.transpose(K) |
||||
|
||||
def update(self, meas): |
||||
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas) |
||||
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas |
||||
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas |
||||
self.x0_0 = x0_0 |
||||
self.x1_0 = x1_0 |
||||
return [self.x0_0, self.x1_0] |
||||
|
||||
@property |
||||
def x(self): |
||||
return [[self.x0_0], [self.x1_0]] |
||||
|
||||
def set_x(self, x): |
||||
self.x0_0 = x[0][0] |
||||
self.x1_0 = x[1][0] |
||||
@ -1,67 +0,0 @@ |
||||
import copy |
||||
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 = copy.deepcopy(actuators) |
||||
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 opendbc.can.parser import CANParser |
||||
from opendbc.can.can_define import CANDefine |
||||
from openpilot.selfdrive.car import structs |
||||
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, *_) -> structs.CarState: |
||||
ret = structs.CarState() |
||||
|
||||
# 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 = structs.CarState.ButtonEvent() |
||||
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 openpilot.selfdrive.car.structs import CarParams |
||||
from openpilot.selfdrive.car.tesla.values import CAR |
||||
|
||||
Ecu = 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,42 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
from panda import Panda |
||||
from openpilot.selfdrive.car import get_safety_config, structs |
||||
from openpilot.selfdrive.car.tesla.carstate import CarState |
||||
from openpilot.selfdrive.car.tesla.values import CANBUS, CAR |
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
CS: CarState |
||||
|
||||
@staticmethod |
||||
def _get_params(ret: structs.CarParams, 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 = structs.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(structs.CarParams.SafetyModel.tesla, flags), |
||||
get_safety_config(structs.CarParams.SafetyModel.tesla, flags | Panda.FLAG_TESLA_POWERTRAIN), |
||||
] |
||||
else: |
||||
ret.openpilotLongitudinalControl = False |
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.tesla, flags)] |
||||
|
||||
ret.steerLimitTimer = 1.0 |
||||
ret.steerActuatorDelay = 0.25 |
||||
return ret |
||||
@ -1,90 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
from opendbc.can.parser import CANParser |
||||
from openpilot.selfdrive.car import structs |
||||
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 = structs.RadarData() |
||||
|
||||
# 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] = structs.RadarData.RadarPoint() |
||||
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 openpilot.selfdrive.car import AngleRateLimit, CarSpecs, PlatformConfig, Platforms, dbc_dict |
||||
from openpilot.selfdrive.car import structs |
||||
from openpilot.selfdrive.car.docs_definitions import CarDocs |
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries |
||||
|
||||
Ecu = structs.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": structs.CarState.GearShifter.unknown, |
||||
"DI_GEAR_P": structs.CarState.GearShifter.park, |
||||
"DI_GEAR_R": structs.CarState.GearShifter.reverse, |
||||
"DI_GEAR_N": structs.CarState.GearShifter.neutral, |
||||
"DI_GEAR_D": structs.CarState.GearShifter.drive, |
||||
"DI_GEAR_SNA": structs.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(structs.CarState.ButtonEvent.Type.leftBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [1]), |
||||
Button(structs.CarState.ButtonEvent.Type.rightBlinker, "STW_ACTN_RQ", "TurnIndLvr_Stat", [2]), |
||||
Button(structs.CarState.ButtonEvent.Type.accelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [4, 16]), |
||||
Button(structs.CarState.ButtonEvent.Type.decelCruise, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [8, 32]), |
||||
Button(structs.CarState.ButtonEvent.Type.cancel, "STW_ACTN_RQ", "SpdCtrlLvr_Stat", [1]), |
||||
Button(structs.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