diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 0dd6ef222..2f8e7b8f6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -237,6 +237,23 @@ class LongControl: bleed = interp(abs(error), [0.25, 0.75, 1.5], [0.55, 0.25, 0.0]) self.pid.i *= bleed + def _apply_pedal_long_brake_bias(self, output_accel, a_target, CS): + if not self.is_gm_pedal_long: + return output_accel + if output_accel >= -0.05 or a_target >= -0.80: + return output_accel + if CS.vEgo <= 5.0: + return output_accel + + authority_gap = max(0.0, abs(a_target) - abs(output_accel)) + if authority_gap <= 0.40: + return output_accel + + speed_factor = interp(CS.vEgo, [5.0, 12.0, 25.0], [0.0, 0.7, 1.0]) + max_bias = interp(abs(a_target), [0.8, 2.0, 3.5], [0.0, 0.10, 0.20]) + bias = min(authority_gap * 0.12, max_bias) * speed_factor + return output_accel - float(bias) + @staticmethod def _cap_positive_output_on_negative_target(output_accel, a_target, error, CS): if output_accel <= 0.0: @@ -291,6 +308,7 @@ class LongControl: raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, freeze_integrator=freeze_integrator) raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS) + raw_output_accel = self._apply_pedal_long_brake_bias(raw_output_accel, a_target, CS) if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index de6789be3..d116a5f45 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1,6 +1,7 @@ from types import SimpleNamespace from cereal import car +import pytest import openpilot.selfdrive.controls.lib.longcontrol as longcontrol from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans @@ -289,3 +290,39 @@ def test_negative_target_unwinds_positive_accel_command_after_sign_flip(): assert lc.long_control_state == LongCtrlState.pid assert output_accel <= 0.01 + + +def test_pedal_long_brake_bias_adds_small_negative_nudge_for_strong_decel_request(): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.enableGasInterceptorDEPRECATED = True + CP.flags = 1 + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False) + + biased = lc._apply_pedal_long_brake_bias(-1.0, -3.0, CS) + + assert biased < -1.0 + assert biased == pytest.approx(-1.15, abs=0.03) + + +def test_pedal_long_brake_bias_does_not_touch_non_pedal_or_mild_decel(): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.enableGasInterceptorDEPRECATED = False + CP.flags = 0 + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False) + + assert lc._apply_pedal_long_brake_bias(-1.0, -3.0, CS) == -1.0 + assert lc._apply_pedal_long_brake_bias(-0.4, -0.6, CS) == -0.4