diff --git a/opendbc_repo/opendbc/car/tesla/carcontroller.py b/opendbc_repo/opendbc/car/tesla/carcontroller.py index a882e50b94..4d82332936 100644 --- a/opendbc_repo/opendbc/car/tesla/carcontroller.py +++ b/opendbc_repo/opendbc/car/tesla/carcontroller.py @@ -169,8 +169,10 @@ class CarController(CarControllerBase): CS.engagement.pedal_speed_kph = 0.0 if self.frame % 2 == 0: + requested_angle = float(np.clip(actuators.steeringAngleDeg, + CS.out.steeringAngleDeg - 20., CS.out.steeringAngleDeg + 20.)) self.apply_angle_last = apply_steer_angle_limits_vm( - actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg, + requested_angle, self.apply_angle_last, CS.out.vEgoRaw, CS.out.steeringAngleDeg, lat_active, CarControllerParams, self.VM, ) cntr = (self.frame // 2) % 16 diff --git a/opendbc_repo/opendbc/car/tesla/preap/tests/__init__.py b/opendbc_repo/opendbc/car/tesla/preap/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/car/tesla/preap/tests/test_angle_tracking.py b/opendbc_repo/opendbc/car/tesla/preap/tests/test_angle_tracking.py new file mode 100644 index 0000000000..00a09b55fc --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/preap/tests/test_angle_tracking.py @@ -0,0 +1,36 @@ +from types import SimpleNamespace + +import pytest + +from opendbc.car import structs +from opendbc.car.tesla.carcontroller import CarController +from opendbc.car.tesla.interface import CarInterface +from opendbc.car.tesla.values import CAR, DBC + + +@pytest.mark.parametrize('direction', [-1., 1.]) +def test_preap_stalled_rack_request_stays_within_legacy_tracking_envelope(direction): + cp = CarInterface.get_non_essential_params(CAR.TESLA_MODEL_S_PREAP) + controller = CarController(DBC[cp.carFingerprint], cp) + controller.stock_cc = None + cs = SimpleNamespace(out=SimpleNamespace(vEgoRaw=3., steeringAngleDeg=0.), + hands_on_level=0, preap_lateral_authorized=True, cruiseEnabled=False) + cc = structs.CarControl.new_message() + cc.latActive = True + cc.actuators.steeringAngleDeg = direction * 100. + previous = 0. + for frame in range(100): + output, _ = controller.update(cc.as_reader(), cs, frame * 10000000, None) + assert abs(output.steeringAngleDeg) <= 20. + assert abs(output.steeringAngleDeg - previous) <= 5. + previous = output.steeringAngleDeg + assert previous == direction * 20. + + cs.out.steeringAngleDeg = -direction * 50. + output, _ = controller.update(cc.as_reader(), cs, 1000000000, None) + assert abs(output.steeringAngleDeg - previous) <= 5. + + cc.latActive = False + controller.frame = 102 + output, _ = controller.update(cc.as_reader(), cs, 1020000000, None) + assert output.steeringAngleDeg == cs.out.steeringAngleDeg