diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 26df34f7c8..f22216c66f 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -47,6 +47,7 @@ TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long MAX_STEER_RATE = 100 # deg/s MAX_STEER_RATE_FRAMES = 17 # tx control frames needed before torque can be cut +COROLLA_MAX_STEER_RATE = 80 # EPS allows user torque above threshold for 50 frames before permanently faulting MAX_USER_TORQUE = 500 @@ -81,6 +82,10 @@ def get_toyota_lat_active(requested_active: bool, steering_torque: float) -> boo return requested_active and abs(steering_torque) < MAX_USER_TORQUE +def get_toyota_steer_rate_limit(car_fingerprint) -> int: + return COROLLA_MAX_STEER_RATE if car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE + + def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool: return ( auto_hold_enabled and @@ -248,6 +253,7 @@ class CarController(CarControllerBase): self.standstill_req = False self.permit_braking = True self.steer_rate_counter = 0 + self.steer_rate_limit = get_toyota_steer_rate_limit(self.CP.carFingerprint) self.distance_button = 0 # *** start long control state *** @@ -366,7 +372,7 @@ class CarController(CarControllerBase): # >100 degree/sec steering fault prevention self.steer_rate_counter, apply_steer_req = common_fault_avoidance( - abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active, + abs(CS.out.steeringRateDeg) >= self.steer_rate_limit, lat_active, self.steer_rate_counter, MAX_STEER_RATE_FRAMES, ) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 04329b1bd6..1e637c549d 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -12,7 +12,7 @@ from opendbc.car.toyota import toyotacan from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \ get_prius_positive_feedforward_scale, \ get_rav4_interceptor_pedal_scale, \ - get_toyota_lat_active, \ + get_toyota_lat_active, get_toyota_steer_rate_limit, \ MAX_STEER_RATE, MAX_STEER_RATE_FRAMES, MAX_USER_TORQUE, \ limit_interceptor_pcm_accel, \ limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \ @@ -758,6 +758,23 @@ class TestToyotaCarController: requests.append(request) assert requests == ([True] * 17 + [False]) * 2 + @pytest.mark.parametrize("candidate", list(CAR)) + def test_steer_rate_margin_is_corolla_only(self, candidate): + expected = 80 if candidate == CAR.TOYOTA_COROLLA_TSS2 else MAX_STEER_RATE + assert get_toyota_steer_rate_limit(candidate) == expected + + @pytest.mark.parametrize("direction", [-1, 1]) + def test_corolla_rate_margin_preserves_request_spacing(self, direction): + counter = 0 + requests = [] + for rate in [0] * 30 + [90 * direction] * 36 + [0] * 30: + counter, request = common_fault_avoidance( + abs(rate) >= get_toyota_steer_rate_limit(CAR.TOYOTA_COROLLA_TSS2), + get_toyota_lat_active(True, 117 * direction), counter, MAX_STEER_RATE_FRAMES, + ) + requests.append(request) + assert requests == [True] * 30 + ([True] * 17 + [False]) * 2 + [True] * 30 + @staticmethod def _make_controller(*, standstill_req=False, last_standstill=False): controller = CarController.__new__(CarController)