Tesla stock AEB forwarding (#24503)

* keep long control in sync with the stock system

* panda changes needed

* add stock AEB state

* forgot to add the counter

* fix skipping some counter values if they overlap

panda

@@ -1 +1 @@
-Subproject commit 69215887dc5a28170b558f3f7d4f2d87eba42995
+Subproject commit 893a81aa823254cd72c9817886d4b4039ea04a7e

selfdrive/car/tesla/carcontroller.py

@@ -9,7 +9,6 @@ class CarController:
     self.CP = CP
     self.frame = 0
     self.last_angle = 0
-    self.long_control_counter = 0
     self.packer = CANPacker(dbc_name)
     self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt'])
     self.tesla_can = TeslaCAN(self.packer, self.pt_packer)
@@ -41,15 +40,15 @@ class CarController:
     self.last_angle = apply_angle
     can_sends.append(self.tesla_can.create_steering_control(apply_angle, lkas_enabled, self.frame))
 
-    # Longitudinal control (40Hz)
-    if self.CP.openpilotLongitudinalControl and self.frame % 5 in (0, 2):
+    # 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
 
-      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
+      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:

selfdrive/car/tesla/carstate.py

@@ -1,4 +1,5 @@
 import copy
+from collections import deque
 from cereal import car
 from common.conversions import Conversions as CV
 from selfdrive.car.tesla.values import DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS
@@ -17,6 +18,7 @@ class CarState(CarStateBase):
     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()
@@ -87,9 +89,13 @@ class CarState(CarStateBase):
 
     # 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
 
@@ -177,6 +183,8 @@ class CarState(CarStateBase):
     signals = [
       # sig_name, sig_address
       ("DAS_accState", "DAS_control"),
+      ("DAS_aebEvent", "DAS_control"),
+      ("DAS_controlCounter", "DAS_control"),
     ]
     checks = [
       # sig_address, frequency

selfdrive/car/tesla/teslacan.py

@@ -51,7 +51,7 @@ class TeslaCAN:
       "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
       "DAS_accelMin": min_accel,
       "DAS_accelMax": max_accel,
-      "DAS_controlCounter": (cnt % 8),
+      "DAS_controlCounter": cnt,
       "DAS_controlChecksum": 0,
     }