From 32a6af9ac8a9ee74ef60c8e8716bd8d2b1cf4bb4 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 21 Jun 2026 22:10:37 -0500 Subject: [PATCH] Truckothy --- selfdrive/controls/lib/longcontrol.py | 31 ++++++++++++++- selfdrive/controls/tests/test_longcontrol.py | 42 ++++++++++++++++++++ 2 files changed, 72 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7816a5a78..54e707d63 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -5,7 +5,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.common.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.common.filter_simple import FirstOrderFilter -from opendbc.car.gm.values import CarControllerParams, GMFlags +from opendbc.car.gm.values import CAR, CarControllerParams, GMFlags from openpilot.starpilot.common.testing_grounds import testing_ground CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -131,6 +131,11 @@ class LongControl: self.is_volt = bool( CP.brand == "gm" and str(CP.carFingerprint).startswith("CHEVROLET_VOLT") ) + self.is_gm_stock_truck = bool( + CP.brand == "gm" and + getattr(CP, "carFingerprint", None) in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and + not CP.enableGasInterceptorDEPRECATED + ) def update_mpc_mode(self, experimental_mode): new_mode = 'blended' if experimental_mode else 'acc' @@ -251,6 +256,29 @@ class LongControl: bleed = interp(abs(error), [0.25, 0.75, 1.5], [0.55, 0.25, 0.0]) self.pid.i *= bleed + def _trim_gm_truck_positive_hold_integrator(self, a_target, error, CS): + if not self.is_gm_stock_truck or self.pid.i <= 0.0: + return + if self.last_output_accel <= 0.10: + return + if a_target > 0.03: + return + if CS.vEgo <= NEGATIVE_TARGET_CREEP_GUARD_SPEED and a_target > -NEGATIVE_TARGET_CREEP_GUARD_DECEL: + return + + # Stock-ACC trucks are especially sensitive to hanging onto light positive + # torque after the planner has already crossed back to coast or mild decel. + # Bleed stale positive I faster in that narrow window so the command can + # settle instead of wobbling the converter lock state. + authority_mismatch = self.last_output_accel - max(a_target, 0.0) + if authority_mismatch <= 0.08 and error > -0.08: + return + + target_factor = float(interp(a_target, [-0.30, -0.10, -0.02, 0.03], [0.20, 0.35, 0.60, 0.85])) + if error < -0.20: + target_factor *= 0.75 + self.pid.i *= target_factor + def _apply_pedal_long_brake_bias(self, output_accel, a_target, CS): if not self.is_gm_pedal_long: return output_accel @@ -318,6 +346,7 @@ class LongControl: self.update_mpc_mode(self.experimental_mode) self._shape_volt_test_tune_integrator(error, CS.vEgo) self._trim_positive_overshoot_integrator(a_target, error, CS) + self._trim_gm_truck_positive_hold_integrator(a_target, error, CS) feedforward = a_target * self.feedforward_gain freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits) raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 94854f26c..95fb39c0e 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -427,3 +427,45 @@ def test_pedal_long_brake_bias_does_not_touch_non_pedal_or_mild_decel(): 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 + + +def test_gm_stock_truck_positive_i_bleeds_on_coast_request(): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.carFingerprint = "CHEVROLET_SILVERADO" + CP.enableGasInterceptorDEPRECATED = False + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.02] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.28] + + lc = LongControl(CP) + lc.pid.i = 0.25 + lc.last_output_accel = 0.20 + CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + lc._trim_gm_truck_positive_hold_integrator(-0.02, -0.02, CS) + + assert lc.pid.i < 0.25 + + +def test_gm_stock_truck_positive_i_trim_skips_when_planner_still_requests_accel(): + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.carFingerprint = "CHEVROLET_SILVERADO" + CP.enableGasInterceptorDEPRECATED = False + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.02] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.28] + + lc = LongControl(CP) + lc.pid.i = 0.25 + lc.last_output_accel = 0.20 + CS = car.CarState.new_message(vEgo=20.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + lc._trim_gm_truck_positive_hold_integrator(0.05, 0.05, CS) + + assert lc.pid.i == pytest.approx(0.25, abs=1e-9)