controls: boost under plan braking only while the plan isn't slowing

Plan braking slows early and gently for lights and curves, mostly between
-0.1 and -0.7 m/s^2, where the new accel boost is still at or near full
strength, so the boost was held off entirely. Instead scale the boost's
target by the plan's accel: none at -0.1 m/s^2 or below, full at 0 and
above. It still builds and releases at its own rates.

Replaying AccelBoostSP on routes 85 and 89 (plan braking on, Aggressive),
the share of the plan's slowing the boost would cancel falls from 24% to 3%
on route 85 (2% to 1% on 89), while the boost when the plan isn't slowing
stays at 0.37-0.39 m/s^2 (0.39-0.40 ungated).

The gate lives in LongitudinalPlannerSP.update_accel_boost, so the upstream
planner keeps a one-line change. The stock gas-override boost stays off
under plan braking.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -15,7 +15,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
 
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import AccelBoostSP
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
 
 J_CRUISE_VALS = [1.6, 1.2, 0.8, 0.6]
@@ -110,13 +109,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     self.a_cruise = get_cruise_accel(v_cruise, v_ego, self.a_cruise, self.dt)
     cruise_should_stop = should_stop(v_ego, self.a_cruise)
 
-    if self.plan_long:
-      # the plan already slows early on its own; a boost would cancel that
-      self.accel_boost.total_boost = 0.0
-      if isinstance(self.accel_boost, AccelBoostSP):
-        self.accel_boost.boost = 0.0  # ramp up from zero again if plan braking is turned off mid-drive
-    else:
-      output_a_target_e2e = self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
+    output_a_target_e2e = self.update_accel_boost(sm, output_a_target_e2e, output_a_target_mpc, self.a_cruise)
 
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop),

openpilot/sunnypilot/selfdrive/controls/lib/accel_boost.py

@@ -49,7 +49,7 @@ class AccelBoostSP:
     self.boost = self.total_boost = 0.0
     return True
 
-  def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise):
+  def update(self, sm, output_a_target_e2e, output_a_target_mpc, a_cruise, want_scale=1.0):
     model = sm['modelV2']
     brake_probs = model.meta.disengagePredictions.brakePressProbs
     gas_probs = model.meta.disengagePredictions.gasPressProbs
@@ -60,6 +60,7 @@ class AccelBoostSP:
     want = max(float(np.interp(brake_probs[1], BRAKE_PROB, [1.0, 0.0])), float(np.interp(gas_probs[1], GAS_PROB, [0.0, 1.0])))
     want = 0.0 if model.action.shouldStop else want
     want *= float(np.interp(abs(model.action.desiredCurvature) * sm['carState'].vEgo ** 2, CURVE_LAT_ACCEL, [1.0, 0.0]))
+    want *= want_scale  # the caller can hold the boost back; it still builds and releases at the limits below
     target = want * get_boost_max(sm['selfdriveState'].personality)
     self.boost = float(np.clip(target, self.boost - ACCEL_BOOST_RELEASE * DT_MDL, self.boost + ACCEL_BOOST_BUILD * DT_MDL))
     speed_scale = np.interp(sm['carState'].vEgo, [ACCEL_BOOST_MIN_SPEED, 2 * ACCEL_BOOST_MIN_SPEED], [0.0, 1.0])

openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -4,6 +4,7 @@ Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """
+import numpy as np
 
 from openpilot.cereal import messaging, custom
 from opendbc.car import structs
@@ -23,6 +24,8 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
+PLAN_LONG_BOOST_ACCEL = [-0.1, 0.]  # plan accel (m/s^2): with plan braking, no boost while the plan slows, full once it doesn't
+
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
@@ -84,6 +87,18 @@ class LongitudinalPlannerSP:
     self.events_sp.clear()
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
+  def update_accel_boost(self, sm: messaging.SubMaster, output_a_target_e2e: float, output_a_target_mpc: float, a_cruise: float) -> float:
+    if not self.plan_long:
+      return self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise)
+    if isinstance(self.accel_boost, AccelBoostSP):
+      # plan braking slows early and gently on its own (lights, curves), well above where the boost fades out (-0.2 to -0.7),
+      # so only boost while the plan isn't slowing
+      not_slowing = float(np.interp(output_a_target_e2e, PLAN_LONG_BOOST_ACCEL, [0., 1.]))
+      return self.accel_boost.update(sm, output_a_target_e2e, output_a_target_mpc, a_cruise, want_scale=not_slowing)
+    # the stock boost latches on after a gas override and would cancel the plan's early slowing
+    self.accel_boost.total_boost = 0.0
+    return output_a_target_e2e
+
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
     plan_sp_send = messaging.new_message('longitudinalPlanSP')
 

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_plan_long_boost.py

@@ -0,0 +1,94 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.accel_boost import AccelBoost
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_boost import ACCEL_BOOST_BUILD, ACCEL_BOOST_MAX, ACCEL_BOOST_RELEASE, AccelBoostSP
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import PLAN_LONG_BOOST_ACCEL, LongitudinalPlannerSP
+
+BOOST_MAX = ACCEL_BOOST_MAX[0]  # aggressive
+SLOWING, NOT_SLOWING = -0.15, 0.3  # plan accel, m/s^2; -0.15 is where the boost alone is still at full strength
+
+
+def make_sm(brake_prob=0.05, v_ego=30.0, enabled=True):
+  meta = SimpleNamespace(disengagePredictions=SimpleNamespace(brakePressProbs=[0.0, brake_prob, 0.0, 0.0, 0.0, 0.0],
+                                                              gasPressProbs=[0.0, 0.3, 0.0, 0.0, 0.0, 0.0]))
+  action = SimpleNamespace(shouldStop=False, desiredCurvature=0.0)
+  return {'modelV2': SimpleNamespace(meta=meta, action=action), 'carState': SimpleNamespace(vEgo=v_ego),
+          'selfdriveState': SimpleNamespace(enabled=enabled, personality=0)}
+
+
+def make_planner(plan_long, boost=None):
+  return SimpleNamespace(plan_long=plan_long, accel_boost=boost if boost is not None else AccelBoostSP())
+
+
+def run(planner, plan_accel, frames=200, sm=None):
+  sm = sm if sm is not None else make_sm()
+  out = plan_accel
+  for _ in range(frames):
+    out = LongitudinalPlannerSP.update_accel_boost(planner, sm, plan_accel, 9.0, 9.0)
+  return out
+
+
+def test_plan_braking_off_boosts_a_gentle_slowdown():
+  # the boost on its own stays at full strength down to -0.2 m/s^2
+  planner = make_planner(plan_long=False)
+  assert run(planner, SLOWING) == pytest.approx(SLOWING + BOOST_MAX)
+
+
+def test_plan_braking_keeps_the_plans_slowdown():
+  planner = make_planner(plan_long=True)
+  assert run(planner, SLOWING) == pytest.approx(SLOWING)
+  assert planner.accel_boost.total_boost == 0.0
+
+
+def test_plan_braking_boosts_when_the_plan_is_not_slowing():
+  planner = make_planner(plan_long=True)
+  assert run(planner, NOT_SLOWING) == pytest.approx(NOT_SLOWING + BOOST_MAX)
+
+
+def test_gate_is_off_at_the_bottom_and_full_at_the_top():
+  low, high = PLAN_LONG_BOOST_ACCEL
+  assert run(make_planner(plan_long=True), low) == pytest.approx(low)
+  assert run(make_planner(plan_long=True), high) == pytest.approx(high + BOOST_MAX)
+  mid = (low + high) / 2
+  assert run(make_planner(plan_long=True), mid) == pytest.approx(mid + BOOST_MAX / 2)
+
+
+def test_boost_builds_at_its_rate_when_the_plan_stops_slowing():
+  planner = make_planner(plan_long=True)
+  run(planner, SLOWING)
+  run(planner, NOT_SLOWING, frames=1)
+  assert planner.accel_boost.total_boost == pytest.approx(ACCEL_BOOST_BUILD * DT_MDL)
+  run(planner, NOT_SLOWING, frames=9)
+  assert planner.accel_boost.total_boost == pytest.approx(10 * ACCEL_BOOST_BUILD * DT_MDL)
+
+
+def test_boost_releases_at_its_rate_when_the_plan_starts_slowing():
+  planner = make_planner(plan_long=True)
+  run(planner, NOT_SLOWING)
+  run(planner, SLOWING, frames=1)
+  assert planner.accel_boost.total_boost == pytest.approx(BOOST_MAX - ACCEL_BOOST_RELEASE * DT_MDL)
+  run(planner, SLOWING, frames=int(BOOST_MAX / (ACCEL_BOOST_RELEASE * DT_MDL)))
+  assert planner.accel_boost.total_boost == 0.0
+
+
+def test_model_expecting_a_brake_still_holds_the_boost_off():
+  planner = make_planner(plan_long=True)
+  assert run(planner, NOT_SLOWING, sm=make_sm(brake_prob=0.5)) == pytest.approx(NOT_SLOWING)
+
+
+@pytest.mark.parametrize("plan_accel", [SLOWING, NOT_SLOWING])
+def test_stock_boost_stays_off_with_plan_braking(plan_accel):
+  stock = AccelBoost()
+  stock.total_boost = 0.2  # latched on after a gas override
+  planner = make_planner(plan_long=True, boost=stock)
+  assert run(planner, plan_accel, frames=1) == plan_accel
+  assert stock.total_boost == 0.0