feat(dec): smooth acceleration through e2e/acc mode transitions

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -77,6 +77,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
   def update(self, sm):
     LongitudinalPlannerSP.update(self, sm)
+    self.previous_plan_accel = self.output_a_target
 
     if len(sm['carControl'].orientationNED) == 3:
       accel_coast = get_coast_accel(sm['carControl'].orientationNED[1])
@@ -140,6 +141,10 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
     is_e2e = self.is_e2e(sm)
+    output_a_target_model = self.select_model_accel(
+      output_a_target_mpc, output_a_target_e2e, blended=is_e2e,
+      should_stop=output_should_stop_e2e or output_should_stop_mpc, fcw=self.fcw, reset=reset_state,
+    )
 
     self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
@@ -149,7 +154,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc),
                   (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)]
     if is_e2e:
-      candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+      candidates.append((output_a_target_model, LongitudinalPlanSource.e2e, output_should_stop_e2e))
+    elif self.model_accel_transition.active:
+      candidates[0] = (output_a_target_model, self.mpc.source, output_should_stop_mpc)
 
     output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0])
     self.output_should_stop = any(should_stop for _, _, should_stop in candidates)

openpilot/sunnypilot/selfdrive/controls/lib/dec/constants.py

@@ -1,4 +1,6 @@
 class WMACConstants:
+  MODEL_ACCEL_TRANSITION_RATE = 3.0
+
   # Lead detection parameters
   LEAD_WINDOW_SIZE = 6  # Stable detection window
   LEAD_PROB = 0.45  # Balanced threshold for lead detection

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

@@ -6,13 +6,15 @@ See the LICENSE.md file in the root directory for more details.
 """
 # Version = 2025-6-30
 
+import math
+from typing import Literal
+
 from openpilot.cereal import messaging
 from opendbc.car import structs
 from numpy import interp
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
-from typing import Literal
 
 # d-e2e, from modeldata.h
 TRAJECTORY_SIZE = 33
@@ -130,6 +132,51 @@ class ModeTransitionManager:
     return self.current_mode
 
 
+class ModelAccelTransition:
+  """Smooths acceleration while DEC changes modes."""
+
+  def __init__(self, dt: float = DT_MDL):
+    self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt
+    self._accel = 0.0
+    self._active = False
+    self._blended = False
+
+  def reset(self) -> None:
+    self._active = False
+    self._blended = False
+
+  @property
+  def active(self) -> bool:
+    return self._active
+
+  def update(self, mpc_accel: float, model_accel: float, previous_accel: float, *, blended: bool,
+             urgent: bool = False, reset: bool = False) -> float:
+    selected_accel = min(mpc_accel, model_accel) if blended else mpc_accel
+    if reset or not all(math.isfinite(accel) for accel in (mpc_accel, model_accel, previous_accel)):
+      self.reset()
+      return selected_accel
+
+    if blended != self._blended:
+      self._accel = previous_accel
+      self._active = True
+      self._blended = blended
+
+    if urgent and selected_accel <= self._accel:
+      self._accel = selected_accel
+      self._active = True
+      return selected_accel
+    if not self._active:
+      return selected_accel
+
+    transition_target = model_accel if blended else mpc_accel
+    preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, transition_target))
+    output_accel = min(mpc_accel, preview_accel)
+    self._accel = output_accel
+    if mpc_accel >= transition_target and math.isclose(preview_accel, transition_target, abs_tol=1e-9):
+      self._active = False
+    return output_accel
+
+
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
     self._CP = CP

openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dynamic_controller.py

@@ -1,5 +1,7 @@
+from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition
 
 class MockLeadOne:
   def __init__(self, present=0.0):
@@ -89,3 +91,56 @@ class TestDynamicExperimentalController(OpenpilotTestCase):
       controller.update(default_sm)
 
     assert controller.mode() == "blended"
+
+
+class TestModelAccelTransition(OpenpilotTestCase):
+  def test_blended_entry_is_rate_bounded(self):
+    transition = ModelAccelTransition()
+    previous = 0.30
+    outputs = []
+    for _ in range(10):
+      previous = transition.update(0.50, -0.88, previous, blended=True)
+      outputs.append(previous)
+
+    max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * DT_MDL
+    self.assertAlmostEqual(outputs[0], 0.15)
+    assert min(b - a for a, b in zip([0.30, *outputs[:-1]], outputs, strict=True)) >= -max_step - 1e-9
+    self.assertAlmostEqual(outputs[-1], -0.88)
+
+  def test_harder_mpc_braking_is_immediate(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-2.0, -0.88, 0.30, blended=True), -2.0)
+
+  def test_urgent_model_braking_is_immediate(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(0.0, -2.0, 0.30, blended=True, urgent=True), -2.0)
+    self.assertAlmostEqual(transition.update(0.0, 0.0, -2.0, blended=True), -1.85)
+
+  def test_harder_mpc_release_is_rate_bounded(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-2.0, -0.5, 0.30, blended=True), -2.0)
+    self.assertAlmostEqual(transition.update(0.0, -0.5, -2.0, blended=True), -1.85)
+
+  def test_entry_and_exit_are_rate_bounded(self):
+    transition = ModelAccelTransition()
+    output = 0.30
+    for _ in range(20):
+      output = transition.update(0.0, -0.88, output, blended=True)
+      if output <= -0.88:
+        break
+    else:
+      self.fail("transition did not converge")
+
+    output = transition.update(0.0, -1.50, output, blended=True)
+    self.assertAlmostEqual(output, -1.50)
+    self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), -1.35)
+
+  def test_harder_mpc_braking_bypasses_exit_ramp(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(2.0, 0.76, 0.76, blended=True), 0.76)
+    self.assertAlmostEqual(transition.update(-2.0, 0.76, 0.76, blended=False), -2.0)
+
+  def test_urgent_release_remains_rate_bounded(self):
+    transition = ModelAccelTransition()
+    self.assertAlmostEqual(transition.update(-1.0, -1.0, -1.0, blended=True), -1.0)
+    self.assertAlmostEqual(transition.update(1.0, -1.0, -1.0, blended=False, urgent=True), -0.85)

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

@@ -9,7 +9,7 @@ from openpilot.cereal import messaging, custom
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
-from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, ModelAccelTransition
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_assist import SpeedLimitAssist
@@ -26,6 +26,7 @@ class LongitudinalPlannerSP:
     self.events_sp = EventsSP()
     self.resolver = SpeedLimitResolver()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.model_accel_transition = ModelAccelTransition(mpc.dt)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -35,6 +36,7 @@ class LongitudinalPlannerSP:
 
     self.output_v_target = 0.
     self.output_a_target = 0.
+    self.previous_plan_accel = 0.
 
   def is_e2e(self, sm: messaging.SubMaster) -> bool:
     experimental_mode = sm['selfdriveState'].experimentalMode
@@ -43,6 +45,12 @@ class LongitudinalPlannerSP:
 
     return experimental_mode and self.dec.mode() == "blended"
 
+  def select_model_accel(self, mpc_accel: float, model_accel: float, *, blended: bool,
+                         should_stop: bool, fcw: bool, reset: bool) -> float:
+    return self.model_accel_transition.update(
+      mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=should_stop or fcw, reset=reset or not self.dec.active(),
+    )
+
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
     v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)