From ce86d9d05af79f6e21914a16df78bbfbd1fc5b97 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 17 Jul 2026 19:25:25 -0500 Subject: [PATCH] Start Accel bug fix --- selfdrive/controls/controlsd.py | 3 +- selfdrive/controls/lib/longcontrol.py | 7 ++- selfdrive/controls/tests/test_longcontrol.py | 61 ++++++++++++++++++- .../tests/test_starpilot_acceleration.py | 9 --- 4 files changed, 68 insertions(+), 12 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d6fe5ee76..33d4af48d 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -424,7 +424,8 @@ class Controls: self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode) actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles, has_lead=long_plan.hasLead, - traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled), + traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled, + profile_max_accel=self.sm['starpilotPlan'].maxAcceleration), self.starpilot_toggles.max_desired_acceleration)) # Steering PID loop and lateral MPC diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 9911ce266..993f5752d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -227,7 +227,8 @@ 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 update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False, traffic_mode_enabled=False): + def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False, + traffic_mode_enabled=False, profile_max_accel=0.0): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] @@ -256,6 +257,10 @@ class LongControl: output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) elif getattr(starpilot_toggles, "custom_accel_profile", False): output_accel = clip(a_target, 0.0, starpilot_toggles.startAccel) + elif profile_max_accel > 0.0: + # Keep the StartAccel friction-overcoming shove, but cap it at the selected + # acceleration profile's launch ceiling so Eco launches soft and Sport hard. + output_accel = min(starpilot_toggles.startAccel, profile_max_accel) else: output_accel = starpilot_toggles.startAccel self.reset() diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index c42a55a46..32360ff6f 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -197,7 +197,7 @@ def test_starting_accel_obeys_a_target_cap_when_traffic_mode_enabled(): assert output_accel == pytest.approx(1.10) -def test_starting_accel_uses_raw_start_accel_when_traffic_mode_disabled(): +def test_starting_accel_uses_raw_start_accel_when_no_profile_ceiling(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] CP.longitudinalTuning.kpV = [0.1] @@ -208,6 +208,8 @@ def test_starting_accel_uses_raw_start_accel_when_traffic_mode_disabled(): CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False) CS.cruiseState.standstill = False + # No usable profile ceiling published (e.g. a stale/zero starpilotPlan) -> keep the + # raw StartAccel shove so a publish gap never zeroes out the launch. output_accel = lc.update( active=True, CS=CS, @@ -216,12 +218,69 @@ def test_starting_accel_uses_raw_start_accel_when_traffic_mode_disabled(): accel_limits=(-3.0, 4.0), starpilot_toggles=make_toggles(startAccel=3.5), traffic_mode_enabled=False, + profile_max_accel=0.0, ) assert lc.long_control_state == LongCtrlState.starting assert output_accel == pytest.approx(3.5) +def test_starting_accel_capped_by_profile_ceiling(): + 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 + + # A large StartAccel override (3.5) must be capped to the selected profile's launch + # ceiling (e.g. Eco = 1.5) so a soft profile launches soft. + output_accel = lc.update( + active=True, + CS=CS, + a_target=1.10, + should_stop=False, + accel_limits=(-3.0, 4.0), + starpilot_toggles=make_toggles(startAccel=3.5), + traffic_mode_enabled=False, + profile_max_accel=1.5, + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == pytest.approx(1.5) + + +def test_starting_accel_keeps_start_accel_shove_below_profile_ceiling(): + 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 + + # StartAccel below the profile ceiling (e.g. Sport+ = 3.5) is preserved in full - + # the cap only trims launches that exceed the profile, it does not weaken the shove. + output_accel = lc.update( + active=True, + CS=CS, + a_target=1.10, + should_stop=False, + accel_limits=(-3.0, 4.0), + starpilot_toggles=make_toggles(startAccel=1.5), + traffic_mode_enabled=False, + profile_max_accel=3.5, + ) + + assert lc.long_control_state == LongCtrlState.starting + assert output_accel == pytest.approx(1.5) + + def test_update_requires_sustained_moderate_positive_target_to_leave_stopping(): CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) CP.longitudinalTuning.kpBP = [0.0] diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index a4a6b128c..25ddb3112 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -190,15 +190,6 @@ def test_traffic_mode_overrides_custom_accel_profile(): assert accel.max_accel == pytest.approx(get_max_accel_traffic(5.0)) -def test_traffic_mode_skips_human_acceleration_shaping(): - accel = StarPilotAcceleration(FakePlanner(v_cruise=2.0)) - sm = make_sm(traffic_mode=True) - - accel.update(1.0, sm, make_toggles(human_acceleration=True)) - - assert accel.max_accel == pytest.approx(get_max_accel_traffic(1.0)) - - def test_traffic_mode_sets_soft_cruise_decel_floor(): accel = StarPilotAcceleration(FakePlanner(v_cruise=25.0)) sm = make_sm(traffic_mode=True)