diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index c5856998d..3320b9c5b 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/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() diff --git a/opendbc_repo/opendbc/car/tesla/teslacan.py b/opendbc_repo/opendbc/car/tesla/teslacan.py index e21246799..faa041796 100644 --- a/opendbc_repo/opendbc/car/tesla/teslacan.py +++ b/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, diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py b/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py index f26df4ba4..b2dcc2f75 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py index dbd045c43..92b35920b 100644 --- a/opendbc_repo/opendbc/car/tesla/values.py +++ b/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):