Start Accel bug fix

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

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()

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]

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)