diff --git a/opendbc_repo/opendbc/car/gm/carcontroller.py b/opendbc_repo/opendbc/car/gm/carcontroller.py index b8152bd92a..a8b99ac864 100644 --- a/opendbc_repo/opendbc/car/gm/carcontroller.py +++ b/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( diff --git a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py b/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py index 2761fdc793..4b91ebb923 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_carcontroller.py +++ b/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)