From 161e0e10c8d8872fd35f891d443450d7bebc0e9c Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 11 Apr 2026 00:45:25 -0500 Subject: [PATCH] customaccel patch --- selfdrive/controls/lib/longcontrol.py | 12 ++- selfdrive/controls/tests/test_longcontrol.py | 93 +++++++++++++++++--- starpilot/common/starpilot_variables.py | 8 +- 3 files changed, 95 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index dec1939e..8e9ad207 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -213,7 +213,12 @@ class LongControl: self.reset() elif self.long_control_state == LongCtrlState.starting: - output_accel = (a_target if starpilot_toggles.human_acceleration else starpilot_toggles.startAccel) + if starpilot_toggles.human_acceleration: + output_accel = a_target + elif getattr(starpilot_toggles, "custom_accel_profile", False): + output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) + else: + output_accel = starpilot_toggles.startAccel self.reset() else: # LongCtrlState.pid @@ -287,7 +292,10 @@ class LongControl: self.reset_old_long(CS.vEgo) elif self.long_control_state == LongCtrlState.starting: - output_accel = starpilot_toggles.startAccel + if getattr(starpilot_toggles, "custom_accel_profile", False): + output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) + else: + output_accel = starpilot_toggles.startAccel self.reset_old_long(CS.vEgo) elif self.long_control_state == LongCtrlState.pid: diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index f61986ae..19f52535 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1,63 +1,130 @@ +from types import SimpleNamespace + from cereal import car -from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans + import openpilot.selfdrive.controls.lib.longcontrol as longcontrol -from openpilot.selfdrive.controls.lib.longcontrol import LongControl +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans +def make_toggles(**overrides): + defaults = { + "custom_accel_profile": False, + "human_acceleration": False, + "startAccel": 1.5, + "stopAccel": -0.5, + "stoppingDecelRate": 0.8, + "vEgoStarting": 0.5, + "vEgoStopping": 0.5, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) class TestLongControlStateTransition: def test_stay_stopped(self): CP = car.CarParams.new_message() + toggles = make_toggles() active = True current_state = LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=True, brake_pressed=False, cruise_standstill=False) + should_stop=True, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=True, cruise_standstill=False) + should_stop=False, brake_pressed=True, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=False, cruise_standstill=True) + should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, - should_stop=False, brake_pressed=False, cruise_standstill=False) + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.pid active = False next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, - should_stop=False, brake_pressed=False, cruise_standstill=False) + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.off + def test_engage(): CP = car.CarParams.new_message() + toggles = make_toggles() active = True current_state = LongCtrlState.off next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=True, brake_pressed=False, cruise_standstill=False) + should_stop=True, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=True, cruise_standstill=False) + should_stop=False, brake_pressed=True, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=False, cruise_standstill=True) + should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles) assert next_state == LongCtrlState.stopping next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=False, cruise_standstill=False) + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.pid + def test_starting(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + toggles = make_toggles(vEgoStarting=0.5) active = True current_state = LongCtrlState.starting next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1, - should_stop=False, brake_pressed=False, cruise_standstill=False) + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.starting next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0, - should_stop=False, brake_pressed=False, cruise_standstill=False) + should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles) assert next_state == LongCtrlState.pid +def test_starting_accel_unchanged_when_custom_profile_disabled(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + 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=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.1, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5), + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == 1.5 + + +def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + 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=0.0, aEgo=0.0, brakePressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=0.1, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(startAccel=1.5, custom_accel_profile=True), + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == 0.1 + + def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch): CP = car.CarParams.new_message() CP.brand = "gm" diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 1a73cb9e..bed52b62 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -818,13 +818,15 @@ class StarPilotVariables: toggle.lock_doors_timer = self.get_value("LockDoorsTimer", cast=float, condition=(toggle.car_make == "toyota")) longitudinal_tuning = toggle.openpilot_longitudinal and self.get_value("LongitudinalTune") + custom_accel_profile_tuning = advanced_longitudinal_tuning and self.get_value("CustomAccelProfile") + acceleration_profile_tuning = longitudinal_tuning or custom_accel_profile_tuning toggle.acceleration_profile = normalize_acceleration_profile( - self.get_value("AccelerationProfile", cast=None, condition=longitudinal_tuning, default=ACCELERATION_PROFILES["SPORT"]) + self.get_value("AccelerationProfile", cast=None, condition=acceleration_profile_tuning, default=ACCELERATION_PROFILES["SPORT"]) ) toggle.deceleration_profile = normalize_deceleration_profile( self.get_value("DecelerationProfile", cast=None, condition=longitudinal_tuning, default=DECELERATION_PROFILES["ECO"]) ) - toggle.custom_accel_profile = self.get_value("CustomAccelProfile", condition=longitudinal_tuning) + toggle.custom_accel_profile = custom_accel_profile_tuning custom_accel_defaults = build_custom_accel_profile_defaults(toggle.acceleration_profile, toggle.ev_tuning, toggle.truck_tuning) custom_accel_raw_values = {key: self.params_raw.get(key) for key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS} custom_accel_initialized = custom_accel_profile_is_initialized( @@ -833,7 +835,7 @@ class StarPilotVariables: ) if custom_accel_initialized: toggle.custom_accel_profile_values = [ - self.get_value(key, cast=float, condition=longitudinal_tuning, default=custom_accel_defaults[key], + self.get_value(key, cast=float, condition=advanced_longitudinal_tuning, default=custom_accel_defaults[key], min=CUSTOM_ACCEL_PROFILE_VALUE_MIN, max=CUSTOM_ACCEL_PROFILE_VALUE_MAX) for key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS ]