diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2aa38d129..5ed1f6a8d 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -326,7 +326,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 3d717deb1..9d858a1be 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -396,7 +396,8 @@ class LongControl: ) return a_target * effective_gain - 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] @@ -427,6 +428,10 @@ class LongControl: output_accel = a_target 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 92c52fdff..9f49a78a1 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]