Smooth longitudinal braking 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_e2e = 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,

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

@@ -80,9 +80,10 @@ class AccelController:
     comfort_decel = COMFORT_DECEL[self.profile]
     return max(speed_error / EARLY_DECEL_RESPONSE_TIME, -comfort_decel)
 
-  def _update_early_decel(self, raw_target: float) -> None:
+  def _update_early_decel(self, raw_target: float, previous_plan_accel: float) -> None:
     raw_target = min(float(raw_target), 0.0)
-    previous = self._early_decel if self._early_decel is not None else 0.0
+    plan_accel = float(previous_plan_accel) if math.isfinite(previous_plan_accel) else 0.0
+    previous = self._early_decel if self._early_decel is not None else max(plan_accel, 0.0)
 
     if raw_target < previous - EARLY_DECEL_EPSILON:
       updated = max(raw_target, previous - EARLY_DECEL_TIGHTEN_RATE * self.dt)
@@ -94,7 +95,7 @@ class AccelController:
       updated = raw_target
       state = AccelControllerState.hold if updated < -EARLY_DECEL_EPSILON else AccelControllerState.free
 
-    if updated >= -EARLY_DECEL_EPSILON:
+    if raw_target >= -EARLY_DECEL_EPSILON and updated >= -EARLY_DECEL_EPSILON:
       self._early_decel = None
       self.early_decel = None
       self.state = AccelControllerState.free
@@ -105,7 +106,7 @@ class AccelController:
 
   def update(self, radar_state, *, v_ego: float, a_ego: float, v_cruise: float, follow_personality,
              engaged: bool, cruise_initialized: bool, acc_selected: bool, stock_accel_max: float,
-             radar_fresh: bool = True, force_decel: bool = False) -> AccelDecision:
+             radar_fresh: bool = True, force_decel: bool = False, previous_plan_accel: float = 0.0) -> AccelDecision:
     self.profile = sanitize_profile(self.profile)
     valid_context = self._valid_context(
       v_ego=v_ego, a_ego=a_ego, v_cruise=v_cruise, stock_accel_max=stock_accel_max,
@@ -131,7 +132,7 @@ class AccelController:
     self.selected_lead = lead_plan.selected_lead
     self.selected_lead_track_id = lead_plan.selected_lead_track_id
     self.required_decel = lead_plan.required_decel
-    self._update_early_decel(self._raw_early_decel(lead_plan))
+    self._update_early_decel(self._raw_early_decel(lead_plan), previous_plan_accel)
 
     profile_binding = self.cruise_accel_max < positive_stock_max - EARLY_DECEL_EPSILON
     self.is_active = profile_binding or self.early_decel is not None

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py

@@ -119,6 +119,13 @@ class TestAccelDecision(OpenpilotTestCase):
 
 
 class TestEarlyDecel(OpenpilotTestCase):
+  def test_early_decel_starts_from_the_previous_positive_plan(self):
+    instance = controller()
+    decision = update(instance, restrictive_radar(), previous_plan_accel=0.48)
+
+    self.assertIsNotNone(decision.early_decel)
+    self.assertAlmostEqual(decision.early_decel, 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL)
+
   def test_early_decel_is_nonpositive_and_tightens_at_bound(self):
     instance = controller()
     samples = [0.0]

openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py

@@ -9,7 +9,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi
 from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel, get_max_accel
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
-  ACCEL_PROFILES, AccelProfile, profile_accel_scale,
+  ACCEL_PROFILES, EARLY_DECEL_TIGHTEN_RATE, AccelProfile, profile_accel_scale,
 )
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 
@@ -57,6 +57,7 @@ def planner_for_hook(*, enabled=True, profile=AccelProfile.normal):
   dynamic_planner.accel_controller = accel_controller(enabled=enabled, profile=profile)
   dynamic_planner.dec = SimpleNamespace(active=lambda: False)
   dynamic_planner.output_v_target = 30.0
+  dynamic_planner.previous_plan_accel = 0.0
   dynamic_planner._radar_fresh_this_cycle = True
   return planner
 
@@ -119,6 +120,18 @@ class TestPlannerHook(OpenpilotTestCase):
     self.assertEqual(result[-1][1:], (MpcSource.cruise, False))
     self.assertLessEqual(result[-1][0], 0.0)
 
+  def test_early_decel_enters_from_the_previous_positive_command(self):
+    planner = planner_for_hook(profile=AccelProfile.sport)
+    planner.previous_plan_accel = 0.48
+    sm = PlannerSM(radar_state=radar(lead(present=True, distance=25.0, speed=5.0)))
+    candidates = [(-0.02, MpcSource.lead0, False), (0.60, MpcSource.cruise, False)]
+
+    result = planner.update_accel_controller(sm, candidates)
+
+    self.assertEqual(result[:2], candidates)
+    self.assertAlmostEqual(result[-1][0], 0.48 - EARLY_DECEL_TIGHTEN_RATE * DT_MDL)
+    self.assertEqual(result[-1][1:], (MpcSource.cruise, False))
+
   def test_radar_freshness_requires_a_healthy_advanced_message(self):
     planner = planner_for_hook()
     planner._radar_log_mono_time = None

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,49 @@ class ModeTransitionManager:
     return self.current_mode
 
 
+class ModelAccelTransition:
+  """Smooths model acceleration while DEC enters blended mode."""
+
+  def __init__(self, dt: float = DT_MDL):
+    self._max_step = WMACConstants.MODEL_ACCEL_TRANSITION_RATE * dt
+    self._accel = 0.0
+    self._active = False
+    self._was_blended = False
+
+  def reset(self) -> None:
+    self._active = False
+    self._was_blended = False
+
+  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 not blended:
+      self.reset()
+      return selected_accel
+
+    if not self._was_blended:
+      self._accel = previous_accel
+      self._active = True
+    self._was_blended = True
+
+    if urgent:
+      self._accel = selected_accel
+      self._active = True
+      return selected_accel
+    if not self._active:
+      return selected_accel
+
+    preview_accel = max(self._accel - self._max_step, min(self._accel + self._max_step, model_accel))
+    output_accel = min(mpc_accel, preview_accel)
+    self._accel = output_accel
+    if mpc_accel >= model_accel and math.isclose(preview_accel, model_accel, abs_tol=1e-9):
+      self._active = False
+    return output_accel
+
+
 class DynamicExperimentalController:
   def __init__(self, CP: structs.CarParams, mpc, params=None):
     self._CP = CP
@@ -197,6 +242,9 @@ class DynamicExperimentalController:
   def active(self) -> bool:
     return self._active
 
+  def accel_transition_urgent(self) -> bool:
+    return self._has_mpc_fcw or (self._has_slow_down and self._urgency > 0.7)
+
   def set_mpc_fcw_crash_cnt(self) -> None:
     """Set MPC FCW crash count"""
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt

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

@@ -33,6 +33,9 @@ class MockDec:
   def enabled(self) -> bool:
     return True
 
+  def accel_transition_urgent(self) -> bool:
+    return False
+
 
 class MockSubMaster(dict):
   def __init__(self, services: dict):

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,45 @@ 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_only_entry_is_shaped(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")
+
+    self.assertAlmostEqual(transition.update(0.0, -1.50, output, blended=True), -1.50)
+    self.assertAlmostEqual(transition.update(0.0, -0.20, output, blended=False), 0.0)

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

@@ -12,7 +12,7 @@ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcSource
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
-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
@@ -29,6 +29,7 @@ class LongitudinalPlannerSP:
     self.accel_controller = AccelController(CP, dt=mpc.dt)
     self.events_sp = EventsSP()
     self.dec = DynamicExperimentalController(CP, mpc)
+    self.model_accel_transition = ModelAccelTransition(mpc.dt)
     self.scc = SmartCruiseControl()
     self.resolver = SpeedLimitResolver()
     self.sla = SpeedLimitAssist(CP, CP_SP)
@@ -40,6 +41,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
@@ -48,6 +50,13 @@ 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:
+    urgent = should_stop or fcw or self.dec.accel_transition_urgent()
+    return self.model_accel_transition.update(
+      mpc_accel, model_accel, self.previous_plan_accel, blended=blended, urgent=urgent, reset=reset or not self.dec.active(),
+    )
+
   def update_accel_controller(self, sm: messaging.SubMaster, candidates):
     cruise_index = next((i for i, candidate in enumerate(candidates) if candidate[1] == MpcSource.cruise), -1)
     if cruise_index < 0:
@@ -63,7 +72,7 @@ class LongitudinalPlannerSP:
       follow_personality=sm['selfdriveState'].personality, engaged=not reset_state,
       cruise_initialized=CS.vCruise != V_CRUISE_UNSET, acc_selected=not self.is_e2e(sm),
       stock_accel_max=max(cruise_accel, 0.0), radar_fresh=self._radar_fresh_this_cycle,
-      force_decel=sm['controlsState'].forceDecel,
+      force_decel=sm['controlsState'].forceDecel, previous_plan_accel=self.previous_plan_accel,
     )
     if decision.cruise_accel_max is None and decision.early_decel is None:
       return candidates

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

@@ -92,3 +92,38 @@ class TestAccelControllerPlannerIntegration(OpenpilotTestCase):
     self.assertTrue(any(candidate[2] for candidate in augmented))
     self.assertTrue(result["should_stop"])
     self.assertAlmostEqual(result["a_target"], min(augmented, key=lambda candidate: candidate[0])[0])
+
+  def test_dec_blended_entry_limits_the_first_model_brake_step(self):
+    class DecStub:
+      mode_name = "acc"
+
+      def update(self, _sm):
+        pass
+
+      def active(self):
+        return True
+
+      def mode(self):
+        return self.mode_name
+
+      def accel_transition_urgent(self):
+        return False
+
+    plant = Plant(
+      enabled=True, e2e=True, speed=22.0,
+      model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False),
+      actuator_delay=0.15, actuator_lag=0.20,
+    )
+    configure(plant)
+    dec = DecStub()
+    plant.planner.dec = dec
+
+    outputs = []
+    for frame in range(20):
+      if frame == 10:
+        dec.mode_name = "blended"
+      result = plant.step(v_lead=0.0, v_cruise=22.0)
+      outputs.append(result["a_target"])
+
+    self.assertGreaterEqual(outputs[10] - outputs[9], -0.15 - 1e-9)
+    self.assertFalse(result["fcw"])