Stop LongPitch from stacking hill throttle on a positive planner command.

Honor the toggle on pedal-long cars and cap leftover uphill grade feedforward so a 10-speed does not skip-shift 10-8.

Co-authored-by: Cursor <cursoragent@cursor.com>

opendbc_repo/opendbc/car/gm/carcontroller.py

@@ -256,6 +256,19 @@ def shape_truck_pitch_accel(pitch_accel: float, v_ego: float, enabled: bool) ->
   return pitch_accel * scale
 
 
+MAX_UPHILL_GRADE_FF = 0.20
+
+
+def limit_grade_feedforward(planner_accel: float, pitch_accel: float) -> float:
+  # Planner already closed-loops hill sag. Stacking full g*sin(pitch) on a
+  # positive command is what kick-downs 10-speed trucks.
+  if pitch_accel > 0.0 and planner_accel > 0.0:
+    return 0.0
+  if pitch_accel > MAX_UPHILL_GRADE_FF:
+    return MAX_UPHILL_GRADE_FF
+  return pitch_accel
+
+
 def shape_truck_friction_brake(apply_brake: int, accel_cmd: float, stopping: bool, active: bool) -> tuple[int, bool]:
   if apply_brake <= 0:
     return 0, False
@@ -1003,14 +1016,13 @@ class CarController(CarControllerBase):
           self.truck_follow_accel = 0.0
         else:
           long_pitch_enabled = bool(getattr(starpilot_toggles, "long_pitch", True))
-          pedal_long_path = bool(self.CP.enableGasInterceptorDEPRECATED and (self.CP.flags & GMFlags.PEDAL_LONG.value))
-          long_pitch_for_powertrain = long_pitch_enabled or pedal_long_path
 
           if self.is_volt:
-            if long_pitch_for_powertrain and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
+            if long_pitch_enabled and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
               volt_pitch_accel = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
             else:
               volt_pitch_accel = 0.0
+            volt_pitch_accel = limit_grade_feedforward(accel, volt_pitch_accel)
 
             aero_drag_accel = (0.5 * self.coeffDrag * self.frontalArea * self.airDensity * CS.out.vEgo ** 2) / self.mass
             accel_cmd = float(np.clip(accel + aero_drag_accel + volt_pitch_accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
@@ -1031,7 +1043,7 @@ class CarController(CarControllerBase):
             if self.apply_brake > 0:
               self.apply_gas = self.params.INACTIVE_REGEN
           else:
-            if long_pitch_for_powertrain and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
+            if long_pitch_enabled and len(CC.orientationNED) == 3 and CS.out.vEgo > self.CP.vEgoStopping:
               accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
             else:
               accel_due_to_pitch = 0.0
@@ -1048,6 +1060,7 @@ class CarController(CarControllerBase):
               not self.CP.enableGasInterceptorDEPRECATED
             )
             accel_due_to_pitch = shape_truck_pitch_accel(accel_due_to_pitch, CS.out.vEgo, truck_long_smoothing)
+            accel_due_to_pitch = limit_grade_feedforward(actuators.accel, accel_due_to_pitch)
             accel_input = actuators.accel + accel_due_to_pitch
             if truck_long_smoothing:
               accel_input = shape_truck_positive_accel(

opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py

@@ -53,6 +53,7 @@ from opendbc.car.gm.carcontroller import (
   get_testing_ground_1_brake_switch_bias,
   get_acc_dashboard_status_active,
   get_stock_cc_active_for_cancel,
+  limit_grade_feedforward,
   shape_bolt_acc_pedal_low_speed_friction,
   shape_truck_friction_brake,
   shape_truck_pitch_accel,
@@ -895,6 +896,20 @@ def test_shape_truck_pitch_accel_is_inactive_without_truck_tuning():
   assert shape_truck_pitch_accel(-0.30, 30.0, False) == pytest.approx(-0.30)
 
 
+def test_limit_grade_feedforward_does_not_stack_on_positive_planner():
+  assert limit_grade_feedforward(0.40, 0.50) == 0.0
+
+
+def test_limit_grade_feedforward_caps_uphill_hold():
+  assert limit_grade_feedforward(0.0, 0.50) == pytest.approx(0.20)
+  assert limit_grade_feedforward(-0.10, 0.50) == pytest.approx(0.20)
+
+
+def test_limit_grade_feedforward_keeps_downhill_help():
+  assert limit_grade_feedforward(0.40, -0.30) == pytest.approx(-0.30)
+  assert limit_grade_feedforward(-0.20, -0.30) == pytest.approx(-0.30)
+
+
 def test_shape_truck_friction_brake_suppresses_boundary_chatter():
   assert shape_truck_friction_brake(14, -0.3, False, False) == (0, False)