From 4960578bdfae11689da81e80973b998b05e6fd96 Mon Sep 17 00:00:00 2001 From: Robbe Derks Date: Thu, 18 Nov 2021 13:57:12 +0100 Subject: [PATCH] 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 submodule --- panda | 2 +- release/files_common | 1 + selfdrive/car/tesla/carcontroller.py | 20 ++++++++++++++++---- selfdrive/car/tesla/carstate.py | 8 ++++++-- selfdrive/car/tesla/interface.py | 27 ++++++++++++++++++++++++--- selfdrive/car/tesla/teslacan.py | 28 ++++++++++++++++++++++++---- selfdrive/car/tesla/values.py | 16 ++++++++++++---- selfdrive/controls/radard.py | 16 +++++++++++----- 8 files changed, 95 insertions(+), 23 deletions(-) diff --git a/panda b/panda index fec966d6f..ddb3698f9 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit fec966d6fd099dc140997978765733734fe6d1c3 +Subproject commit ddb3698f9bf03dc6096a754c01529cddd580a976 diff --git a/release/files_common b/release/files_common index ceaef0b0b..aeae351df 100644 --- a/release/files_common +++ b/release/files_common @@ -616,3 +616,4 @@ opendbc/vw_mqb_2010.dbc opendbc/tesla_can.dbc opendbc/tesla_radar.dbc +opendbc/tesla_powertrain.dbc diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index df53b9dd9..7e6a2f2e9 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -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 diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 4bd1d1001..0a45b6f2b 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -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) diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index e26421545..45dc0a723 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -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 diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index 0dee8b182..130180286 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -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 diff --git a/selfdrive/car/tesla/values.py b/selfdrive/car/tesla/values.py index 2a7cf9e50..90bc45c7a 100644 --- a/selfdrive/car/tesla/values.py +++ b/selfdrive/car/tesla/values.py @@ -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 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 5d94804d7..88f39ce43 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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)]]