Tesla longitudinal control (#22561)
* tesla long squashed changes * fix no resume from stop * try setting accel limits to 0 to remove jerk with fast switchover * expand radard lookup tables * bump merged panda submodulepull/22966/head
Robbe Derks
GitHub
parent
6c909ba651
commit
4960578bdf
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit fec966d6fd099dc140997978765733734fe6d1c3
|
||||
Subproject commit ddb3698f9bf03dc6096a754c01529cddd580a976
|
||||
|
|
@ -616,3 +616,4 @@ opendbc/vw_mqb_2010.dbc
|
|||
|
||||
opendbc/tesla_can.dbc
|
||||
opendbc/tesla_radar.dbc
|
||||
opendbc/tesla_powertrain.dbc
|
||||
|
|
|
|||
|
|
@ -1,14 +1,16 @@
|
|||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.car.tesla.teslacan import TeslaCAN
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.tesla.values import CANBUS, CarControllerParams
|
||||
from selfdrive.car.tesla.teslacan import TeslaCAN
|
||||
from selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.last_angle = 0
|
||||
self.long_control_counter = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.tesla_can = TeslaCAN(dbc_name, self.packer)
|
||||
self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
||||
self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, cruise_cancel):
|
||||
can_sends = []
|
||||
|
|
@ -34,6 +36,16 @@ class CarController():
|
|||
self.last_angle = apply_angle
|
||||
can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, frame))
|
||||
|
||||
# Longitudinal control (40Hz)
|
||||
if self.CP.openpilotLongitudinalControl and ((frame % 5) in [0, 2]):
|
||||
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
|
||||
|
||||
can_sends.extend(self.tesla_can.create_longitudinal_commands(CS.acc_state, target_speed, min_accel, max_accel, self.long_control_counter))
|
||||
self.long_control_counter += 1
|
||||
|
||||
# Cancel on user steering override, since there is no steering torque blending
|
||||
if hands_on_fault:
|
||||
cruise_cancel = True
|
||||
|
|
@ -46,7 +58,7 @@ class CarController():
|
|||
# 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, cruise_cancel, CANBUS.chassis, counter))
|
||||
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot, counter))
|
||||
can_sends.append(self.tesla_can.create_action_request(CS.msg_stw_actn_req, cruise_cancel, CANBUS.autopilot_chassis, counter))
|
||||
|
||||
# TODO: HUD control
|
||||
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ class CarState(CarStateBase):
|
|||
self.msg_stw_actn_req = None
|
||||
self.hands_on_level = 0
|
||||
self.steer_warning = None
|
||||
self.acc_state = 0
|
||||
|
||||
def update(self, cp, cp_cam):
|
||||
ret = car.CarState.new_message()
|
||||
|
|
@ -57,7 +58,7 @@ class CarState(CarStateBase):
|
|||
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 = (cruise_state == "STANDSTILL")
|
||||
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")]
|
||||
|
|
@ -88,6 +89,7 @@ class CarState(CarStateBase):
|
|||
|
||||
# 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"]
|
||||
|
||||
return ret
|
||||
|
||||
|
|
@ -174,8 +176,10 @@ class CarState(CarStateBase):
|
|||
def get_cam_can_parser(CP):
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("DAS_accState", "DAS_control", 0),
|
||||
]
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("DAS_control", 40),
|
||||
]
|
||||
return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot)
|
||||
return CANParser(DBC[CP.carFingerprint]['chassis'], signals, checks, CANBUS.autopilot_chassis)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.car.tesla.values import CAR
|
||||
from panda import Panda
|
||||
from selfdrive.car.tesla.values import CANBUS, CAR
|
||||
from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
|
@ -10,7 +11,6 @@ class CarInterface(CarInterfaceBase):
|
|||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
|
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||
ret.carName = "tesla"
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla)]
|
||||
|
||||
# There is no safe way to do steer blending with user torque,
|
||||
# so the steering behaves like autopilot. This is not
|
||||
|
|
@ -18,7 +18,28 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.dashcamOnly = True
|
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||
ret.openpilotLongitudinalControl = False
|
||||
|
||||
# Set kP and kI to 0 over the whole speed range to have the planner accel as actuator command
|
||||
ret.longitudinalTuning.kpBP = [0]
|
||||
ret.longitudinalTuning.kpV = [0]
|
||||
ret.longitudinalTuning.kiBP = [0]
|
||||
ret.longitudinalTuning.kiV = [0]
|
||||
ret.stopAccel = 0.0
|
||||
ret.startAccel = 0.0
|
||||
ret.longitudinalActuatorDelayUpperBound = 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.
|
||||
if (CANBUS.autopilot_powertrain in fingerprint.keys()) and (0x2bf in fingerprint[CANBUS.autopilot_powertrain].keys()):
|
||||
ret.openpilotLongitudinalControl = True
|
||||
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),
|
||||
]
|
||||
else:
|
||||
ret.openpilotLongitudinalControl = False
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)]
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerRateCost = 0.5
|
||||
|
|
|
|||
|
|
@ -1,13 +1,13 @@
|
|||
import copy
|
||||
import crcmod
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from selfdrive.car.tesla.values import CANBUS
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.car.tesla.values import CANBUS, CarControllerParams
|
||||
|
||||
|
||||
class TeslaCAN:
|
||||
def __init__(self, dbc_name, packer):
|
||||
self.can_define = CANDefine(dbc_name)
|
||||
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
|
||||
|
|
@ -39,3 +39,23 @@ class TeslaCAN:
|
|||
data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[2]
|
||||
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 % 8),
|
||||
"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)[2]
|
||||
values["DAS_controlChecksum"] = self.checksum(0x2b9, data[:7])
|
||||
messages.append(packer.make_can_msg("DAS_control", bus, values))
|
||||
return messages
|
||||
|
|
|
|||
|
|
@ -25,14 +25,20 @@ FINGERPRINTS = {
|
|||
}
|
||||
|
||||
DBC = {
|
||||
CAR.AP2_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
|
||||
CAR.AP1_MODELS: dbc_dict(None, 'tesla_radar', chassis_dbc='tesla_can'),
|
||||
CAR.AP2_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
|
||||
CAR.AP1_MODELS: dbc_dict('tesla_powertrain', 'tesla_radar', chassis_dbc='tesla_can'),
|
||||
}
|
||||
|
||||
class CANBUS:
|
||||
# Lateral harness
|
||||
chassis = 0
|
||||
autopilot = 2
|
||||
radar = 1
|
||||
autopilot_chassis = 2
|
||||
|
||||
# Longitudinal harness
|
||||
powertrain = 4
|
||||
private = 5
|
||||
autopilot_powertrain = 6
|
||||
|
||||
GEAR_MAP = {
|
||||
"DI_GEAR_INVALID": car.CarState.GearShifter.unknown,
|
||||
|
|
@ -58,4 +64,6 @@ BUTTONS = [
|
|||
class CarControllerParams:
|
||||
RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15])
|
||||
RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4])
|
||||
|
||||
JERK_LIMIT_MAX = 8
|
||||
JERK_LIMIT_MIN = -8
|
||||
ACCEL_TO_SPEED_MULTIPLIER = 3
|
||||
|
|
|
|||
|
|
@ -18,16 +18,22 @@ from selfdrive.hardware import TICI
|
|||
class KalmanParams():
|
||||
def __init__(self, dt):
|
||||
# Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library.
|
||||
# hardcoding a lookup table to compute K for values of radar_ts between 0.1s and 1.0s
|
||||
assert dt > .01 and dt < .1, "Radar time step must be between .01s and 0.1s"
|
||||
# hardcoding a lookup table to compute K for values of radar_ts between 0.01s and 0.2s
|
||||
assert dt > .01 and dt < .2, "Radar time step must be between .01s and 0.2s"
|
||||
self.A = [[1.0, dt], [0.0, 1.0]]
|
||||
self.C = [1.0, 0.0]
|
||||
#Q = np.matrix([[10., 0.0], [0.0, 100.]])
|
||||
#R = 1e3
|
||||
#K = np.matrix([[ 0.05705578], [ 0.03073241]])
|
||||
dts = [dt * 0.01 for dt in range(1, 11)]
|
||||
K0 = [0.12288, 0.14557, 0.16523, 0.18282, 0.19887, 0.21372, 0.22761, 0.24069, 0.2531, 0.26491]
|
||||
K1 = [0.29666, 0.29331, 0.29043, 0.28787, 0.28555, 0.28342, 0.28144, 0.27958, 0.27783, 0.27617]
|
||||
dts = [dt * 0.01 for dt in range(1, 21)]
|
||||
K0 = [0.12287673, 0.14556536, 0.16522756, 0.18281627, 0.1988689, 0.21372394,
|
||||
0.22761098, 0.24069424, 0.253096, 0.26491023, 0.27621103, 0.28705801,
|
||||
0.29750003, 0.30757767, 0.31732515, 0.32677158, 0.33594201, 0.34485814,
|
||||
0.35353899, 0.36200124]
|
||||
K1 = [0.29666309, 0.29330885, 0.29042818, 0.28787125, 0.28555364, 0.28342219,
|
||||
0.28144091, 0.27958406, 0.27783249, 0.27617149, 0.27458948, 0.27307714,
|
||||
0.27162685, 0.27023228, 0.26888809, 0.26758976, 0.26633338, 0.26511557,
|
||||
0.26393339, 0.26278425]
|
||||
self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]]
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue