diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index d7128674d..614f0a14d 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -28,6 +28,9 @@ PRIUS_INTEGRAL_MISMATCH_UNWIND = 8.0 PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.5 MAX_PITCH_COMPENSATION = 1.5 # m/s^2 +TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s +TOYOTA_COAST_BRAKE_ENABLE_ACCEL = -0.10 # m/s^2 +TOYOTA_COAST_BRAKE_DISABLE_ACCEL = -0.06 # m/s^2 # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -60,6 +63,28 @@ def get_long_tune(CP, params): rate=1 / (DT_CTRL * 3)) +def update_permit_braking(current: bool, net_acceleration_request_min: float, stopping: bool, + long_active: bool, v_ego: float) -> bool: + if stopping or not long_active: + return True + + # At cruising speeds, some Toyota platforms turn tiny negative accel corrections + # into noticeable brake taps. Keep a small coast-only band so mild follow/cruise + # trims stay off the brakes while still allowing real negative requests through. + if v_ego >= TOYOTA_COAST_BRAKE_MIN_SPEED: + if net_acceleration_request_min <= TOYOTA_COAST_BRAKE_ENABLE_ACCEL: + return True + if net_acceleration_request_min >= TOYOTA_COAST_BRAKE_DISABLE_ACCEL: + return False + return current + + if net_acceleration_request_min < 0.2: + return True + if net_acceleration_request_min > 0.3: + return False + return current + + class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) @@ -287,10 +312,11 @@ class CarController(CarControllerBase): # Along with rate limiting positive jerk above, this greatly improves gas response time # Consider the net acceleration request that the PCM should be applying (pitch included) net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request) - if net_acceleration_request_min < 0.2 or stopping or not CC.longActive: - self.permit_braking = True - elif net_acceleration_request_min > 0.3: - self.permit_braking = False + self.permit_braking = update_permit_braking(self.permit_braking, + net_acceleration_request_min, + stopping, + CC.longActive, + CS.out.vEgo) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 14db5a13e..7a0b80339 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -5,7 +5,7 @@ from hypothesis import given, settings, strategies as st from opendbc.car import Bus from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict -from opendbc.car.toyota.carcontroller import CarController +from opendbc.car.toyota.carcontroller import CarController, update_permit_braking from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ @@ -221,6 +221,20 @@ class TestToyotaCarController: assert controller.standstill_req is False + def test_permit_braking_high_speed_coasts_for_tiny_decel(self): + assert update_permit_braking(True, -0.05, False, True, 25.0) is False + assert update_permit_braking(False, -0.05, False, True, 25.0) is False + + def test_permit_braking_high_speed_brakes_for_meaningful_decel(self): + assert update_permit_braking(False, -0.15, False, True, 25.0) is True + + def test_permit_braking_low_speed_keeps_legacy_behavior(self): + assert update_permit_braking(False, -0.05, False, True, 10.0) is True + + def test_permit_braking_forces_on_when_stopping_or_inactive(self): + assert update_permit_braking(False, 0.10, True, True, 25.0) is True + assert update_permit_braking(False, 0.10, False, False, 25.0) is True + def test_sng_hack_clears_existing_standstill_latch(self): controller = self._make_controller(standstill_req=True, last_standstill=True)