diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index a7098163b..f54e33b5c 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -32,7 +32,7 @@ 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 TOYOTA_NO_LEAD_COAST_BRAKE_ACCEL = -0.30 # m/s^2 -TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL = 1.0 # m/s^2 +TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL = 2.0 # m/s^2 # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -91,7 +91,22 @@ def update_permit_braking(current: bool, net_acceleration_request_min: float, st def limit_interceptor_pcm_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float) -> float: - if stopping or abs(target_accel) > TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL: + if stopping: + return pcm_accel_cmd + + # The Toyota long path should not cross the accel sign against a strong planner + # request on pedal/SDSU cars during ordinary driving. + if target_accel > 0.15 and pcm_accel_cmd < 0.0: + positive_floor = float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.12, 0.08, 0.0])) + pcm_accel_cmd = max(pcm_accel_cmd, positive_floor) + elif target_accel < -0.15 and pcm_accel_cmd > 0.0: + negative_floor = float(np.interp(v_ego, [0.0, 5.0, 15.0], [-0.12, -0.08, 0.0])) + pcm_accel_cmd = min(pcm_accel_cmd, negative_floor) + + if target_accel <= -1.6: + return pcm_accel_cmd + + if abs(target_accel) > TOYOTA_INTERCEPTOR_COMFORT_TARGET_ACCEL: return pcm_accel_cmd lower_delta = float(np.interp(v_ego, [0.0, 5.0, 15.0], [0.20, 0.25, 0.35])) diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 7c30138fa..423128b77 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -156,6 +156,10 @@ class CarInterface(CarInterfaceBase): if ret.flags & ToyotaFlags.HYBRID.value: ret.longitudinalActuatorDelay = 0.05 + if ret.enableGasInterceptorDEPRECATED: + # Pedal/SDSU Toyotas feel best with a softer final stop clamp. + ret.stopAccel = -1.5 + return ret @staticmethod diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index a13965512..bf718113d 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -338,10 +338,20 @@ class TestToyotaCarController: def test_interceptor_comfort_limit_bypasses_harder_braking(self): original = -1.80 - limited = limit_interceptor_pcm_accel(original, -1.20, False, 8.5) + limited = limit_interceptor_pcm_accel(original, -1.80, False, 8.5) assert limited == original + def test_interceptor_comfort_limit_prevents_positive_target_from_crossing_negative(self): + limited = limit_interceptor_pcm_accel(-2.0, 1.7, False, 7.5) + + assert limited >= 0.0 + + def test_interceptor_comfort_limit_prevents_negative_target_from_crossing_positive(self): + limited = limit_interceptor_pcm_accel(0.8, -0.7, False, 7.5) + + assert limited <= 0.0 + class TestToyotaCarState: def test_interceptor_gas_pressed_threshold(self):