Tesla: smooth gas override jerk transition

Adapted from commaai/opendbc#3249 by @lukasloetkolben.

opendbc_repo/opendbc/car/tesla/carcontroller.py

@@ -68,13 +68,13 @@ class CarController(CarControllerBase):
         state = 13 if CC.cruiseControl.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, CS.out.vEgo, CC.longActive))
+        can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, self.frame, CS.out.vEgo, CS.out.gasPressed))
 
     else:
       # Increment counter so cancel is prioritized even without openpilot longitudinal
       if CC.cruiseControl.cancel:
         cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
-        can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False))
+        can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, self.frame, CS.out.vEgo, False))
 
     # TODO: HUD control
     new_actuators = actuators.as_builder()

opendbc_repo/opendbc/car/tesla/teslacan.py

@@ -1,3 +1,5 @@
+import numpy as np
+
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.tesla.values import CANBUS, CarControllerParams
 
@@ -5,6 +7,7 @@ from opendbc.car.tesla.values import CANBUS, CarControllerParams
 class TeslaCAN:
   def __init__(self, packer):
     self.packer = packer
+    self.gas_release_frame = 0
 
   def create_steering_control(self, angle, enabled):
     values = {
@@ -15,15 +18,20 @@ class TeslaCAN:
 
     return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
 
-  def create_longitudinal_command(self, acc_state, accel, counter, v_ego, active):
+  def create_longitudinal_command(self, acc_state, accel, counter, frame, v_ego, gas_pressed):
     set_speed = min(max(v_ego + accel, 0) * CV.MS_TO_KPH, 400)
 
+    if gas_pressed:
+      self.gas_release_frame = frame
+
+    jerk = float(np.interp(frame - self.gas_release_frame, [0, 100], [0.0, CarControllerParams.JERK_LIMIT_MAX]))
+
     values = {
       "DAS_setSpeed": set_speed,
       "DAS_accState": acc_state,
       "DAS_aebEvent": 0,
-      "DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
-      "DAS_jerkMax": CarControllerParams.JERK_LIMIT_MAX,
+      "DAS_jerkMin": -jerk,
+      "DAS_jerkMax": jerk,
       "DAS_accelMin": accel,
       "DAS_accelMax": max(accel, 0),
       "DAS_controlCounter": counter,

opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py

@@ -3,6 +3,7 @@ import pytest
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.tesla.carstate import update_tesla_gas_pressed
 from opendbc.car.tesla.teslacan import TeslaCAN
+from opendbc.car.tesla.values import CarControllerParams
 
 
 class RecordingPacker:
@@ -10,7 +11,6 @@ class RecordingPacker:
     return name, bus, values
 
 
-@pytest.mark.parametrize("active", [False, True])
 @pytest.mark.parametrize(
   ("v_ego", "accel", "expected_set_speed"),
   [
@@ -20,12 +20,37 @@ class RecordingPacker:
     (120.0, 2.0, 400.0),
   ],
 )
-def test_longitudinal_set_speed_tracks_accel_continuously(active, v_ego, accel, expected_set_speed):
-  _, _, values = TeslaCAN(RecordingPacker()).create_longitudinal_command(4, accel, 0, v_ego, active)
+def test_longitudinal_set_speed_tracks_accel_continuously(v_ego, accel, expected_set_speed):
+  _, _, values = TeslaCAN(RecordingPacker()).create_longitudinal_command(4, accel, 0, 200, v_ego, False)
 
   assert values["DAS_setSpeed"] == pytest.approx(expected_set_speed)
 
 
+def test_longitudinal_jerk_ramps_after_gas_release():
+  can = TeslaCAN(RecordingPacker())
+
+  _, _, pressed = can.create_longitudinal_command(4, 0, 0, 20, 20, True)
+  _, _, halfway = can.create_longitudinal_command(4, 0, 1, 70, 20, False)
+  _, _, complete = can.create_longitudinal_command(4, 0, 2, 120, 20, False)
+
+  assert pressed["DAS_jerkMin"] == pytest.approx(0.0)
+  assert pressed["DAS_jerkMax"] == pytest.approx(0.0)
+  assert halfway["DAS_jerkMin"] == pytest.approx(-CarControllerParams.JERK_LIMIT_MAX / 2)
+  assert halfway["DAS_jerkMax"] == pytest.approx(CarControllerParams.JERK_LIMIT_MAX / 2)
+  assert complete["DAS_jerkMin"] == pytest.approx(-CarControllerParams.JERK_LIMIT_MAX)
+  assert complete["DAS_jerkMax"] == pytest.approx(CarControllerParams.JERK_LIMIT_MAX)
+
+
+def test_longitudinal_jerk_release_timer_resets_while_gas_is_pressed():
+  can = TeslaCAN(RecordingPacker())
+
+  can.create_longitudinal_command(4, 0, 0, 20, 20, True)
+  _, _, values = can.create_longitudinal_command(4, 0, 1, 80, 20, True)
+
+  assert values["DAS_jerkMin"] == pytest.approx(0.0)
+  assert values["DAS_jerkMax"] == pytest.approx(0.0)
+
+
 def test_tesla_gas_pressed_hysteresis_prevents_release_chatter():
   assert update_tesla_gas_pressed(False, 0.4) is False
   assert update_tesla_gas_pressed(False, 0.8) is False

opendbc_repo/opendbc/car/tesla/values.py

@@ -125,7 +125,6 @@ class CarControllerParams:
   ACCEL_MAX = 2.0    # m/s^2
   ACCEL_MIN = -3.48  # m/s^2
   JERK_LIMIT_MAX = 4.9  # m/s^3, ACC faults at 5.0
-  JERK_LIMIT_MIN = -4.9  # m/s^3, ACC faults at 5.0
 
 
 class TeslaSafetyFlags(IntFlag):