pedal long brake bias

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':

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