From 066a0f25201e69fe12ee433a84831a6f8abd7f48 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 22 Jun 2026 23:37:15 -0500 Subject: [PATCH] Tibetan Singing Bowl --- opendbc_repo/opendbc/car/gm/interface.py | 6 +- opendbc_repo/opendbc/car/gm/tests/test_gm.py | 28 ++++++++- .../opendbc/car/hyundai/carcontroller.py | 17 +++++- .../opendbc/car/hyundai/tests/test_hyundai.py | 16 ++++- selfdrive/controls/lib/longcontrol.py | 28 ++++++++- selfdrive/controls/tests/test_longcontrol.py | 60 +++++++++++++++++++ 6 files changed, 149 insertions(+), 6 deletions(-) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 24583b992..974297a22 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -133,7 +133,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_pid_accel_limits(CP, current_speed, cruise_speed): if CP.enableGasInterceptorDEPRECATED and bool(CP.flags & GMFlags.PEDAL_LONG.value): - if CP.carFingerprint in BOLT_PEDAL_LONG_CARS: + if CP.carFingerprint == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL: + accel_min = CarControllerParams.ACCEL_MIN + accel_max = np.interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0], + [0.54, 0.74, 1.03, 1.46, CarControllerParams.ACCEL_MAX]) + elif CP.carFingerprint in BOLT_PEDAL_LONG_CARS: accel_min = np.interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95]) accel_max = np.interp(current_speed, [0.0, 1.5, 4.0, 8.0, 15.0], diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index 11a6789c7..1e9af8b93 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -1,4 +1,5 @@ import pytest +import numpy as np from types import SimpleNamespace from parameterized import parameterized @@ -18,7 +19,7 @@ from opendbc.car.gm.carcontroller import ( import opendbc.car.gm.interface as gm_interface from opendbc.car.common.conversions import Conversions as CV from opendbc.car.gm.fingerprints import FINGERPRINTS -from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, CruiseButtons, GMFlags, GMSafetyFlags +from opendbc.car.gm.values import CAMERA_ACC_CAR, CAR, CC_ONLY_CAR, DBC, GM_RX_OFFSET, CarControllerParams, CruiseButtons, GMFlags, GMSafetyFlags from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params @@ -63,6 +64,31 @@ class TestGMFingerprint: class TestGMInterface: + def test_bolt_acc_pedal_pid_accel_limits_keep_full_negative_authority(self): + cp = SimpleNamespace( + enableGasInterceptorDEPRECATED=True, + flags=GMFlags.PEDAL_LONG.value, + carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + ) + + accel_min, accel_max = gm_interface.CarInterface.get_pid_accel_limits(cp, 4.73, 0.0) + + assert accel_min == pytest.approx(CarControllerParams.ACCEL_MIN) + assert accel_max == pytest.approx(np.interp(4.73, [0.0, 1.5, 4.0, 8.0, 15.0], + [0.54, 0.74, 1.03, 1.46, CarControllerParams.ACCEL_MAX])) + + def test_bolt_cc_pedal_pid_accel_limits_remain_regen_limited(self): + cp = SimpleNamespace( + enableGasInterceptorDEPRECATED=True, + flags=GMFlags.PEDAL_LONG.value, + carFingerprint=CAR.CHEVROLET_BOLT_CC_2022_2023, + ) + + accel_min, _ = gm_interface.CarInterface.get_pid_accel_limits(cp, 4.73, 0.0) + + assert accel_min == pytest.approx(np.interp(4.73, [0.0, 1.5, 4.0, 8.0, 15.0, 30.0], + [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95])) + def test_missing_hard_cruise_signal_defaults_to_init(self): assert get_hard_cruise_buttons({"ACCButtons": CruiseButtons.RES_ACCEL}) == CruiseButtons.INIT assert get_hard_cruise_buttons({"ACCButtonsHard": CruiseButtons.DECEL_SET}) == CruiseButtons.DECEL_SET diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index a4f2a521e..12befdac3 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -71,6 +71,11 @@ def egmp_dynamic_longitudinal_tuning(CP) -> bool: kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, getattr(CP, "carVin", "")) +def should_reset_ev6_gt_line_longitudinal_tuning(CP, long_control_state: LongCtrlState) -> bool: + return kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, getattr(CP, "carVin", "")) and \ + long_control_state == LongCtrlState.off + + @dataclass class Ioniq6LongitudinalTuningState: desired_accel: float = 0.0 @@ -84,6 +89,13 @@ class Ioniq6LongitudinalTuningState: long_control_state_last: LongCtrlState = LongCtrlState.off +def reset_ev6_gt_line_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, CP, + long_control_state: LongCtrlState) -> Ioniq6LongitudinalTuningState: + if should_reset_ev6_gt_line_longitudinal_tuning(CP, long_control_state): + return Ioniq6LongitudinalTuningState(long_control_state_last=long_control_state) + return state + + @dataclass class GenesisG90LongitudinalTuningState: actual_accel: float = 0.0 @@ -434,7 +446,10 @@ class CarController(CarControllerBase): use_egmp_dynamic_long_tuning = egmp_dynamic_longitudinal_tuning(self.CP) and self.long_active_ecu and \ actuators.longControlState in (LongCtrlState.starting, LongCtrlState.pid, LongCtrlState.stopping) - if use_egmp_dynamic_long_tuning and self.frame % 5 == 0: + if should_reset_ev6_gt_line_longitudinal_tuning(self.CP, actuators.longControlState): + self._ioniq_6_long_tuning = reset_ev6_gt_line_longitudinal_tuning(self._ioniq_6_long_tuning, self.CP, + actuators.longControlState) + elif use_egmp_dynamic_long_tuning and self.frame % 5 == 0: self._ioniq_6_long_tuning = update_ioniq_6_longitudinal_tuning(self._ioniq_6_long_tuning, accel_cmd, CS.out.vEgo, CS.out.aEgo, actuators.longControlState, self.long_active_ecu) diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index bbdcbdf69..c5a0f5599 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -9,7 +9,8 @@ from opendbc.car.structs import CarControl, CarParams from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car from opendbc.car.hyundai.carcontroller import CarController, Ioniq6LongitudinalTuningState, GenesisG90LongitudinalTuningState, \ update_ioniq_6_longitudinal_tuning, \ - update_genesis_g90_longitudinal_tuning, egmp_dynamic_longitudinal_tuning + update_genesis_g90_longitudinal_tuning, egmp_dynamic_longitudinal_tuning, \ + should_reset_ev6_gt_line_longitudinal_tuning, reset_ev6_gt_line_longitudinal_tuning from opendbc.car.hyundai.carstate import CarState, decode_canfd_camera_lead, decode_ioniq_6_blindspot_radar_state from opendbc.car.hyundai.interface import CarInterface from opendbc.car.hyundai import hyundaican, hyundaicanfd @@ -541,6 +542,16 @@ class TestHyundaiFingerprint: assert CP.stoppingDecelRate == pytest.approx(0.4) assert kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, CP.carVin) assert egmp_dynamic_longitudinal_tuning(CP) + assert should_reset_ev6_gt_line_longitudinal_tuning(CP, LongCtrlState.off) + assert not should_reset_ev6_gt_line_longitudinal_tuning(CP, LongCtrlState.pid) + + stale_state = Ioniq6LongitudinalTuningState(desired_accel=-2.2, actual_accel=-2.2, accel_last=-2.2, + jerk_upper=1.0, jerk_lower=5.0, + long_control_state_last=LongCtrlState.stopping) + reset_state = reset_ev6_gt_line_longitudinal_tuning(stale_state, CP, LongCtrlState.off) + assert reset_state.actual_accel == pytest.approx(0.0) + assert reset_state.accel_last == pytest.approx(0.0) + assert reset_state.long_control_state_last == LongCtrlState.off def test_kia_ev6_non_gt_line_keeps_family_longitudinal_params(self): toggles = get_test_toggles() @@ -554,6 +565,9 @@ class TestHyundaiFingerprint: assert CP.longitudinalActuatorDelay == pytest.approx(0.5) assert not kia_ev6_gt_line_longitudinal_tuning(CP.carFingerprint, CP.carVin) assert not egmp_dynamic_longitudinal_tuning(CP) + assert not should_reset_ev6_gt_line_longitudinal_tuning(CP, LongCtrlState.off) + stale_state = Ioniq6LongitudinalTuningState(actual_accel=-2.2, accel_last=-2.2) + assert reset_ev6_gt_line_longitudinal_tuning(stale_state, CP, LongCtrlState.off) is stale_state def test_genesis_g90_longitudinal_params_bias_toward_earlier_stop_handoff(self): toggles = get_test_toggles() diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index c530789fe..4cee83a3c 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -17,6 +17,8 @@ STOPPING_RELEASE_STRONG_ACCEL = 0.45 MOVING_STOP_FOLLOW_MIN_GAP = 0.25 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 +BOLT_ACC_PEDAL_REGEN_LIMIT_BP = [0.0, 1.5, 4.0, 8.0, 15.0, 30.0] +BOLT_ACC_PEDAL_REGEN_LIMIT_V = [-0.93, -1.28, -1.98, -2.58, -2.86, -2.95] LongCtrlState = car.CarControl.Actuators.LongControlState @@ -137,6 +139,12 @@ class LongControl: getattr(CP, "carFingerprint", None) in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and not CP.enableGasInterceptorDEPRECATED ) + self.is_bolt_acc_pedal_friction_car = bool( + CP.brand == "gm" and + CP.enableGasInterceptorDEPRECATED and + getattr(CP, "carFingerprint", None) == CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL and + (CP.flags & GMFlags.PEDAL_LONG.value) + ) def update_mpc_mode(self, experimental_mode): new_mode = 'blended' if experimental_mode else 'acc' @@ -315,6 +323,22 @@ class LongControl: positive_cap = interp(a_target, [-1.5, -0.6, -0.1], [0.0, 0.0, 0.05]) return min(output_accel, float(positive_cap)) + def _get_longitudinal_feedforward(self, a_target, v_ego): + feedforward = a_target * self.feedforward_gain + if not self.is_bolt_acc_pedal_friction_car or a_target >= 0.0: + return feedforward + + pedal_regen_limit = float(interp(v_ego, BOLT_ACC_PEDAL_REGEN_LIMIT_BP, BOLT_ACC_PEDAL_REGEN_LIMIT_V)) + if a_target >= pedal_regen_limit: + return feedforward + + # Preserve the existing pedal/interceptor shaping up to the known regen + # envelope, then restore full-gain feedforward only for the extra decel + # that must be satisfied by friction blending. + pedal_component = pedal_regen_limit * self.feedforward_gain + friction_component = a_target - pedal_regen_limit + return pedal_component + friction_component + def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] @@ -352,7 +376,7 @@ class LongControl: 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 + feedforward = self._get_longitudinal_feedforward(a_target, CS.vEgo) 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, freeze_integrator=freeze_integrator) @@ -438,7 +462,7 @@ class LongControl: error = self.v_pid - CS.vEgo error_deadzone = apply_deadzone(error, deadzone) freeze_integrator = prevent_overshoot or self._get_pedal_long_freeze(a_target, error_deadzone, CS.vEgo, accel_limits) - feedforward = a_target * self.feedforward_gain + feedforward = self._get_longitudinal_feedforward(a_target, CS.vEgo) output_accel = self.pid.update(error_deadzone, speed=CS.vEgo, feedforward=feedforward, freeze_integrator=freeze_integrator) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index c52277f98..356f7be77 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -4,6 +4,7 @@ from cereal import car import pytest import openpilot.selfdrive.controls.lib.longcontrol as longcontrol +from opendbc.car.gm.values import CAR, GMFlags from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans @@ -21,6 +22,20 @@ def make_toggles(**overrides): return SimpleNamespace(**defaults) +def make_longcontrol_cp(**overrides): + CP = car.CarParams.new_message() + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.0] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.0] + CP.longitudinalTuning.kfDEPRECATED = 1.0 + + for key, value in overrides.items(): + setattr(CP, key, value) + + return CP + + class TestLongControlStateTransition: def test_stay_stopped(self): @@ -454,6 +469,51 @@ def test_pedal_long_brake_bias_does_not_touch_non_pedal_or_mild_decel(): assert lc._apply_pedal_long_brake_bias(-0.4, -0.6, CS) == -0.4 +def test_bolt_acc_pedal_friction_feedforward_preserves_regen_scaling_within_envelope(): + CP = make_longcontrol_cp( + brand="gm", + enableGasInterceptorDEPRECATED=True, + flags=GMFlags.PEDAL_LONG.value, + carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + ) + CP.longitudinalTuning.kfDEPRECATED = 0.20 + + lc = LongControl(CP) + + assert lc._get_longitudinal_feedforward(-1.8, 4.73) == pytest.approx(-0.36) + + +def test_bolt_acc_pedal_friction_feedforward_restores_full_gain_beyond_regen_envelope(): + CP = make_longcontrol_cp( + brand="gm", + enableGasInterceptorDEPRECATED=True, + flags=GMFlags.PEDAL_LONG.value, + carFingerprint=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL, + ) + CP.longitudinalTuning.kfDEPRECATED = 0.20 + + lc = LongControl(CP) + pedal_regen_limit = float(longcontrol.interp(4.73, longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_BP, + longcontrol.BOLT_ACC_PEDAL_REGEN_LIMIT_V)) + expected = pedal_regen_limit * 0.20 + (-3.22 - pedal_regen_limit) + + assert lc._get_longitudinal_feedforward(-3.22, 4.73) == pytest.approx(expected) + + +def test_bolt_cc_pedal_friction_feedforward_remains_fully_scaled_by_kf(): + CP = make_longcontrol_cp( + brand="gm", + enableGasInterceptorDEPRECATED=True, + flags=GMFlags.PEDAL_LONG.value, + carFingerprint=CAR.CHEVROLET_BOLT_CC_2022_2023, + ) + CP.longitudinalTuning.kfDEPRECATED = 0.20 + + lc = LongControl(CP) + + assert lc._get_longitudinal_feedforward(-3.22, 4.73) == pytest.approx(-0.644) + + def test_gm_stock_truck_positive_i_bleeds_on_coast_request(): CP = car.CarParams.new_message() CP.brand = "gm"