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

@@ -1 +1 @@
-Subproject commit fec966d6fd099dc140997978765733734fe6d1c3
+Subproject commit ddb3698f9bf03dc6096a754c01529cddd580a976

release/files_common

@@ -616,3 +616,4 @@ opendbc/vw_mqb_2010.dbc
 
 opendbc/tesla_can.dbc
 opendbc/tesla_radar.dbc
+opendbc/tesla_powertrain.dbc

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
 

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)

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

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

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

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)]]