Tesla: setSpeed is vEgo while disabled (#1923)

set set speed to vEgo while inactive

Author: sshane

opendbc/car/tesla/carcontroller.py

@@ -39,13 +39,13 @@ def update(self, CC, CS, now_nanos):
         state = 13 if cruise_cancel else 4  # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
         accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
         cntr = (self.frame // 4) % 8
-        can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CC.longActive))
+        can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CS.out.vEgo, CC.longActive))
 
     else:
       # Increment counter so cancel is prioritized even without openpilot longitudinal
       if cruise_cancel:
         cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
-        can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, False))
+        can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False))
 
     # TODO: HUD control
     new_actuators = actuators.as_builder()

opendbc/car/tesla/teslacan.py

@@ -1,3 +1,4 @@
+from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.interfaces import V_CRUISE_MAX
 from opendbc.car.tesla.values import CANBUS, CarControllerParams
 
@@ -24,10 +25,14 @@ def create_steering_control(self, angle, enabled, counter):
     values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
     return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
 
-  def create_longitudinal_command(self, acc_state, accel, cntr, active):
-    values = {
+  def create_longitudinal_command(self, acc_state, accel, cntr, v_ego, active):
+    set_speed = v_ego * CV.MS_TO_KPH
+    if active:
       # TODO: this causes jerking after gas override when above set speed
-      "DAS_setSpeed": 0 if (accel < 0 or not active) else V_CRUISE_MAX,
+      set_speed = 0 if accel < 0 else V_CRUISE_MAX
+
+    values = {
+      "DAS_setSpeed": set_speed,
       "DAS_accState": acc_state,
       "DAS_aebEvent": 0,
       "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,