Prevent longitudinal surging without softening launch

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -23,9 +23,9 @@ PROFILE_CONFIGS = {
 
 ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
 ACCEL_PROFILE_MAX_V = {
-  AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20],
-  AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35],
-  AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60],
+  AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16],
+  AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30],
+  AccelProfile.sport: [2.00, 1.90, 1.55, 0.80, 0.50],
 }
 
 CAP_FILTER_FRAMES = 5

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -68,9 +68,9 @@ class TestProfiles:
   def test_lookup_table_is_explicit_and_tunable(self):
     assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
     assert ACCEL_PROFILE_MAX_V == {
-      AccelProfile.eco: [1.55, 1.25, 0.85, 0.40, 0.20],
-      AccelProfile.normal: [1.70, 1.40, 1.05, 0.55, 0.35],
-      AccelProfile.sport: [2.00, 1.90, 1.70, 0.90, 0.60],
+      AccelProfile.eco: [1.55, 1.25, 0.72, 0.32, 0.16],
+      AccelProfile.normal: [1.70, 1.40, 0.97, 0.48, 0.30],
+      AccelProfile.sport: [2.00, 1.90, 1.55, 0.80, 0.50],
     }
 
   @pytest.mark.parametrize("profile", list(AccelProfile))
@@ -79,6 +79,8 @@ class TestProfiles:
       assert AccelController.get_profile_accel_max(profile, speed) == expected
     limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)]
     assert all(0.0 <= limit <= ACCEL_MAX for limit in limits)
+    post_launch_limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(3.0, 40.0, 149)]
+    assert np.all(np.diff(post_launch_limits) <= 0.0)
 
   @pytest.mark.parametrize("speed", [0.0, 3.0, 10.0, 25.0, 40.0])
   def test_profile_order_is_distinct(self, speed):
@@ -98,6 +100,15 @@ class TestProfiles:
     else:
       np.testing.assert_array_equal(result.mpc_accel_max, min(expected + POSITIVE_MPC_HEADROOM, ACCEL_MAX))
 
+  @pytest.mark.parametrize(("profile", "expected"), [
+    (AccelProfile.eco, 1.25), (AccelProfile.normal, 1.40), (AccelProfile.sport, 1.90),
+  ])
+  def test_launch_strength_is_preserved_through_three_meters_per_second(self, profile, expected):
+    result = update(make_controller(), v_ego=3.0, profile=profile)
+    assert result.profile_accel_max == expected
+    assert result.positive_accel_max == expected
+    assert result.effective_accel_max == expected
+
   def test_turn_or_throttle_limit_intersects_profile(self):
     result = update(make_controller(), profile=AccelProfile.sport, stock_accel_max=0.0)
     assert result.positive_accel_max == 0.0

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

@@ -15,10 +15,9 @@ class WMACConstants:
   LEAD_EXIT_PROB = 0.25
   LEAD_RISE_RATE = 1.0
   LEAD_FALL_RATE = 0.35
-  RADAR_LEAD_ACC_PROB = 0.5
-  RADAR_LEAD_ACC_EXIT_PROB = 0.4
-  RADAR_LEAD_ACC_RISE_RATE = 1.0
-  RADAR_LEAD_ACC_FALL_RATE = 0.25
+  RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL)))
+  RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL)))
+  RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL)))
 
   SLOW_DOWN_PROB = 0.5
   SLOW_DOWN_EXIT_PROB = 0.4

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

@@ -135,12 +135,6 @@ class DynamicExperimentalController:
       rise_rate=WMACConstants.LEAD_RISE_RATE,
       fall_rate=WMACConstants.LEAD_FALL_RATE,
     )
-    self._radar_acc_lead_tracker = HysteresisSignal(
-      enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB,
-      exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB,
-      rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE,
-      fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE,
-    )
     self._slow_down_tracker = HysteresisSignal(
       enter_threshold=WMACConstants.SLOW_DOWN_PROB,
       exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
@@ -155,7 +149,12 @@ class DynamicExperimentalController:
     )
 
     self._has_lead_filtered = False
+    self._has_any_lead = False
+    self._has_current_radar_acc_lead = False
     self._has_radar_acc_lead = False
+    self._radar_acc_lead_frames = 0
+    self._radar_fresh = True
+    self._radar_stale_frames = 0
     self._has_slow_down = False
     self._has_slowness = False
     self._has_mpc_fcw = False
@@ -186,7 +185,7 @@ class DynamicExperimentalController:
   def set_mpc_fcw_crash_cnt(self) -> None:
     self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
 
-  def _update_calculations(self, sm: messaging.SubMaster) -> None:
+  def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None:
     car_state = sm['carState']
     radar_state = sm['radarState']
     lead_one = radar_state.leadOne
@@ -202,9 +201,24 @@ class DynamicExperimentalController:
     else:
       self._standstill_count = max(0, self._standstill_count - 1)
 
-    self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
-    radar_acc_lead_score = max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two))
-    self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(radar_acc_lead_score)
+    self._radar_fresh = bool(radar_fresh)
+    if self._radar_fresh:
+      self._radar_stale_frames = 0
+      self._has_lead_filtered = self._lead_tracker.update(float(lead_one.status))
+      self._has_any_lead = bool(lead_one.status or lead_two.status)
+      self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)))
+      self._update_radar_acc_lead()
+    else:
+      self._radar_stale_frames += 1
+      self._has_current_radar_acc_lead = False
+      if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES:
+        self._update_radar_acc_lead()
+      else:
+        self._lead_tracker.reset()
+        self._has_lead_filtered = False
+        self._has_any_lead = False
+        self._has_radar_acc_lead = False
+        self._radar_acc_lead_frames = 0
     self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
     self._calculate_slow_down(md)
 
@@ -237,6 +251,18 @@ class DynamicExperimentalController:
     radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
     return float(lead_one.status and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
 
+  def _update_radar_acc_lead(self) -> None:
+    if self._has_current_radar_acc_lead:
+      self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES
+      self._has_radar_acc_lead = True
+      return
+
+    if not self._has_any_lead:
+      self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
+
+    self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
+    self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
+
   def _model_action_urgency(self, md) -> float:
     action = getattr(md, 'action', None)
     if action is None:
@@ -265,15 +291,23 @@ class DynamicExperimentalController:
     return urgency
 
   def _desired_mode(self) -> tuple[ModeType, bool]:
+    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
+    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
+
+    if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
+      return 'acc', True
+
+    if (not self._radar_fresh or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
+      self._radar_acc_lead_frames = 0
+      self._has_radar_acc_lead = False
+      return 'blended', True
+
     if not self._CP.radarUnavailable and self._has_radar_acc_lead:
       return 'acc', True
 
     if self._has_mpc_fcw:
       return 'blended', True
 
-    standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
-    urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
-
     if self._CP.radarUnavailable:
       if standstill or self._has_slow_down:
         return 'blended', urgent_slow_down
@@ -284,10 +318,10 @@ class DynamicExperimentalController:
 
     return 'acc', False
 
-  def update(self, sm: messaging.SubMaster) -> None:
+  def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True) -> None:
     self._read_params()
     self.set_mpc_fcw_crash_cnt()
-    self._update_calculations(sm)
+    self._update_calculations(sm, radar_fresh)
 
     mode, immediate = self._desired_mode()
     self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,

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

@@ -1,5 +1,6 @@
 import pytest
 
+from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
 
 
@@ -286,28 +287,171 @@ def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default
 def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
   default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  controller.update(default_sm)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
 
   default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
   controller.update(default_sm)
 
   assert controller._has_lead_filtered
   assert controller.mode() == "acc"
 
 
-def test_radar_lead_dropout_guard_expires(mock_cp, mock_mpc, default_sm):
+def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm):
   controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
   default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
-  controller.update(default_sm)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
+  for _ in range(2):
+    controller.update(default_sm)
+  assert controller._has_slow_down
 
-  default_sm['radarState'] = MockRadarState(status=0.0)
-  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
-  for _ in range(3):
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
     controller.update(default_sm)
     assert controller._has_radar_acc_lead
     assert controller.mode() == "acc"
 
   controller.update(default_sm)
   assert not controller._has_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1):
+    controller.update(default_sm)
   assert controller.mode() == "blended"
+
+
+def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+
+
+def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert controller._has_radar_acc_lead
+  assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1
+  assert controller._radar_stale_frames == 1
+  assert controller.mode() == "acc"
+
+
+def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1):
+    controller.update(default_sm, radar_fresh=False)
+    assert controller._has_radar_acc_lead
+
+  controller.update(default_sm, radar_fresh=False)
+
+  assert not controller._has_current_radar_acc_lead
+  assert not controller._has_radar_acc_lead
+  assert not controller._has_any_lead
+  assert not controller._has_lead_filtered
+
+
+def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  controller.update(default_sm)
+  for _ in range(WMACConstants.RADAR_STALE_FRAMES):
+    controller.update(default_sm, radar_fresh=False)
+
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm, radar_fresh=False)
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  controller.update(default_sm, radar_fresh=True)
+
+  assert controller._radar_stale_frames == 0
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+
+@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"])
+def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=0.0)
+  if urgent_source == "fcw":
+    mock_mpc.crash_cnt = 1
+  else:
+    default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
+  controller.update(default_sm)
+
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  mock_mpc.crash_cnt = 0
+  default_sm['modelV2'] = MockModelData(valid=True)
+  controller.update(default_sm)
+  assert controller.mode() == "blended"
+
+
+def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
+    controller.update(default_sm)
+    assert controller._has_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+
+  for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2):
+    if frame % 2 == 0:
+      default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0))
+    else:
+      default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8))
+    controller.update(default_sm)
+
+    assert controller._has_current_radar_acc_lead
+    assert controller.mode() == "acc"
+
+
+def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm):
+  controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
+  default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
+  default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
+  controller.update(default_sm)
+
+  default_sm['radarState'] = MockRadarState(status=1.0)
+  for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1):
+    controller.update(default_sm)
+  assert not controller._has_radar_acc_lead
+  assert controller.mode() == "blended"
+
+  lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
+  default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two)
+  controller.update(default_sm)
+
+  assert controller._has_current_radar_acc_lead
+  assert controller.mode() == "acc"

sunnypilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -204,7 +204,7 @@ class LongitudinalPlannerSP:
   def update(self, sm: messaging.SubMaster) -> None:
     self._read_accel_controller_params()
     self.events_sp.clear()
-    self.dec.update(sm)
+    self.dec.update(sm, radar_fresh=self._radar_fresh(sm))
     self.e2e_alerts_helper.update(sm, self.events_sp)
 
   def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:

sunnypilot/selfdrive/controls/lib/smart_cruise_control/tests/test_vision_controller.py

@@ -4,6 +4,8 @@ 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 numpy as np
 import pytest
 
@@ -13,8 +15,11 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET
 from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
 from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control import MIN_V
-from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import SmartCruiseControlVision, _ENTERING_PRED_LAT_ACC_TH
+from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import (
+  _ACCEL_RELEASE_RATE, _ENTERING_PRED_LAT_ACC_TH, _RELIEF_CONFIRMATION_FRAMES, _TARGET_RELEASE_RATE, SmartCruiseControlVision,
+)
 
 VisionState = custom.LongitudinalPlanSP.SmartCruiseControl.VisionState
 
@@ -118,6 +123,21 @@ class TestSmartCruiseControlVision:
   def reset_params(self):
     self.params.put_bool("SmartCruiseControlVision", True, block=True)
 
+  def set_lat_accels(self, current: float, predicted: float) -> None:
+    v_ego = 20.
+    self.sm['controlsState'].curvature = current / v_ego**2
+    self.sm['modelV2'].velocity.x = [1.] * len(ModelConstants.T_IDXS)
+    self.sm['modelV2'].orientationRate.z = [predicted] * len(ModelConstants.T_IDXS)
+
+  def update_lat_accels(self, current: float, predicted: float, cruise: float = 30., a_ego: float = 0.) -> None:
+    self.set_lat_accels(current, predicted)
+    self.scc_v.update(self.sm, True, False, 20., a_ego, cruise)
+
+  def enter_curve(self, predicted: float = 2.2) -> None:
+    self.update_lat_accels(0.5, predicted)
+    self.update_lat_accels(0.5, predicted)
+    assert self.scc_v.state == VisionState.entering
+
   def test_initial_state(self):
     assert self.scc_v.state == VisionState.disabled
     assert not self.scc_v.is_active
@@ -143,6 +163,146 @@ class TestSmartCruiseControlVision:
       self.scc_v.update(self.sm, True, False, 0., 0., 0.)
     assert self.scc_v.state == VisionState.enabled
 
+  def test_unconfirmed_leaving_and_reentry_never_request_propulsion(self):
+    self.enter_curve()
+    targets = [(self.scc_v.output_v_target, self.scc_v.output_a_target)]
+    assert targets[-1][1] < 0.
+
+    self.update_lat_accels(2., 2.2)
+    assert self.scc_v.state == VisionState.turning
+    targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target))
+
+    self.update_lat_accels(1.2, 1.2)
+    assert self.scc_v.state == VisionState.leaving
+    targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target))
+
+    self.update_lat_accels(1., 3.)
+    assert self.scc_v.state == VisionState.entering
+    targets.append((self.scc_v.output_v_target, self.scc_v.output_a_target))
+
+    v_targets, a_targets = np.array(targets).T
+    assert np.all(np.diff(v_targets[:-1]) <= 0.)
+    assert v_targets[-1] < v_targets[-2]
+    assert np.all(a_targets < 0.)
+    assert np.all(np.diff(a_targets[:-1]) <= 0.)
+    assert a_targets[-1] < a_targets[-2]
+
+  def test_new_curve_interrupts_confirmed_release_immediately(self):
+    self.enter_curve()
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 1):
+      self.update_lat_accels(0.8, 0.8)
+    releasing_v_target = self.scc_v.output_v_target
+    releasing_a_target = self.scc_v.output_a_target
+    assert self.scc_v.state == VisionState.leaving
+
+    self.update_lat_accels(0.8, 3.)
+    assert self.scc_v.state == VisionState.entering
+    assert self.scc_v.output_v_target < releasing_v_target
+    assert self.scc_v.output_a_target < releasing_a_target
+
+  def test_jitter_requires_confirmed_relief_then_releases_smoothly(self):
+    self.enter_curve()
+    held_v_target = self.scc_v.output_v_target
+    held_a_target = self.scc_v.output_a_target
+
+    for frame in range(_RELIEF_CONFIRMATION_FRAMES * 2):
+      self.update_lat_accels(1., 1.05 if frame % 2 == 0 else 1.15)
+      assert self.scc_v.state == VisionState.entering
+      assert self.scc_v.output_v_target == held_v_target
+      assert self.scc_v.output_a_target == held_a_target
+
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES):
+      self.update_lat_accels(1.15, 0.8)
+      assert self.scc_v.state == VisionState.entering
+      assert self.scc_v.output_v_target == held_v_target
+      assert self.scc_v.output_a_target == held_a_target
+
+    release_cruise = held_v_target + 2.5 * _TARGET_RELEASE_RATE * DT_MDL
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES - 1):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      assert self.scc_v.state == VisionState.entering
+      assert self.scc_v.output_v_target == held_v_target
+      assert self.scc_v.output_a_target == held_a_target
+
+    active_v_targets = [held_v_target]
+    active_a_targets = [held_a_target]
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 10):
+      self.update_lat_accels(0.8, 0.8, release_cruise)
+      if not self.scc_v.is_active:
+        break
+      assert self.scc_v.state == VisionState.leaving
+      assert self.scc_v.output_v_target != V_CRUISE_UNSET
+      active_v_targets.append(self.scc_v.output_v_target)
+      active_a_targets.append(self.scc_v.output_a_target)
+
+    assert self.scc_v.state == VisionState.enabled
+    assert self.scc_v.output_v_target == V_CRUISE_UNSET
+    assert active_v_targets[-1] == pytest.approx(release_cruise)
+    assert np.all((np.diff(active_v_targets) >= 0.) & (np.diff(active_v_targets) <= _TARGET_RELEASE_RATE * DT_MDL + 1e-9))
+    assert np.all((np.diff(active_a_targets) >= 0.) & (np.diff(active_a_targets) <= _ACCEL_RELEASE_RATE * DT_MDL + 1e-9))
+    emitted_a_targets = [*active_a_targets, self.scc_v.output_a_target]
+    assert np.max(np.abs(np.diff(emitted_a_targets)) / DT_MDL) <= _ACCEL_RELEASE_RATE + 1e-9
+    assert np.max(np.abs(np.diff(emitted_a_targets)) / DT_MDL) < 3.
+
+  def test_negative_accel_handoff_is_continuous_through_planner_arbitration(self):
+    car_control = messaging.new_message('carControl')
+    car_control.carControl.enabled = True
+    car_control.carControl.cruiseControl.override = False
+    self.sm['carControl'] = car_control.carControl
+    self.sm['carState'].vCruiseCluster = 108.
+
+    planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
+    planner.scc = SimpleNamespace(
+      vision=self.scc_v,
+      map=SimpleNamespace(output_v_target=V_CRUISE_UNSET, output_a_target=0.),
+      update=lambda sm, enabled, override, v_ego, a_ego, v_cruise: self.scc_v.update(
+        sm, enabled, override, v_ego, a_ego, v_cruise),
+    )
+    planner.resolver = SimpleNamespace(
+      speed_limit_valid=False, speed_limit_last_valid=False, speed_limit=0., speed_limit_final_last=0., distance=0.,
+      update=lambda _v_ego, _sm: None,
+    )
+    planner.sla = SimpleNamespace(
+      output_v_target=V_CRUISE_UNSET, output_a_target=0., update=lambda *_args: None,
+    )
+    planner.events_sp = SimpleNamespace()
+
+    self.set_lat_accels(0.5, 2.2)
+    planner.update_targets(self.sm, 20., 0., 30.)
+    planner.update_targets(self.sm, 20., 0., 30.)
+    assert planner.source == LongitudinalPlanSource.sccVision
+    held_v_target = self.scc_v.output_v_target
+    release_cruise = held_v_target + 0.5 * _TARGET_RELEASE_RATE * DT_MDL
+
+    self.set_lat_accels(0.8, 0.8)
+    for _ in range(_RELIEF_CONFIRMATION_FRAMES + 30):
+      planner.update_targets(self.sm, 20., 0., release_cruise)
+      if self.scc_v.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and abs(self.scc_v.output_a_target) <= _ACCEL_RELEASE_RATE * DT_MDL:
+        break
+
+    assert self.scc_v.state == VisionState.leaving
+    assert planner.source == LongitudinalPlanSource.sccVision
+    assert self.scc_v.output_v_target < release_cruise
+
+    prior_accel = planner.output_a_target
+    assert prior_accel > -1.
+    planner.update_targets(self.sm, 20., -1., release_cruise)
+    assert self.scc_v.state == VisionState.leaving
+    assert planner.source == LongitudinalPlanSource.sccVision
+    assert planner.output_a_target == -1.
+    assert planner.output_a_target < prior_accel
+    assert self.scc_v.output_v_target < release_cruise
+
+    active_accel = planner.output_a_target
+    planner.update_targets(self.sm, 20., -1., release_cruise)
+    assert self.scc_v.state == VisionState.leaving
+    assert planner.source == LongitudinalPlanSource.cruise
+    assert abs(planner.output_a_target - active_accel) / DT_MDL < 3.
+
+    planner.update_targets(self.sm, 20., -1., release_cruise)
+    assert self.scc_v.state == VisionState.enabled
+    assert planner.source == LongitudinalPlanSource.cruise
+
   @pytest.mark.parametrize(
     "case, should_enter",
     [

sunnypilot/selfdrive/controls/lib/smart_cruise_control/vision_controller.py

@@ -31,6 +31,10 @@ _A_LAT_REG_MAX = 2.  # Maximum lateral acceleration
 
 _NO_OVERSHOOT_TIME_HORIZON = 4.  # s. Time to use for velocity desired based on a_target when not overshooting.
 
+_RELIEF_CONFIRMATION_FRAMES = max(1, int(round(0.5 / DT_MDL)))
+_TARGET_RELEASE_RATE = 1.  # m/s^2
+_ACCEL_RELEASE_RATE = 1.  # m/s^3
+
 # Lookup table for the minimum smooth deceleration during the ENTERING state
 # depending on the actual maximum absolute lateral acceleration predicted on the turn ahead.
 _ENTERING_SMOOTH_DECEL_V = [-0.2, -1.]  # min decel value allowed on ENTERING state
@@ -65,13 +69,29 @@ class SmartCruiseControlVision:
     self.state = VisionState.disabled
     self.current_lat_acc = 0.
     self.max_pred_lat_acc = 0.
+    self.relief_frames = 0
 
   def get_a_target_from_control(self) -> float:
+    if self.is_active:
+      if self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
+        return min(self.a_ego, self.output_a_target + _ACCEL_RELEASE_RATE * DT_MDL)
+      return min(self.a_target, self.output_a_target, self.a_ego)
     return self.a_target
 
+  def _accel_release_ready(self) -> bool:
+    return abs(self.output_a_target - self.a_ego) <= _ACCEL_RELEASE_RATE * DT_MDL
+
   def get_v_target_from_control(self) -> float:
     if self.is_active:
-      return max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+      v_target = max(self.v_target, MIN_V) + self.a_target * _NO_OVERSHOOT_TIME_HORIZON
+      if self.output_v_target == V_CRUISE_UNSET:
+        return v_target
+      elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
+        if self.v_cruise_setpoint < self.output_v_target:
+          return self.v_cruise_setpoint
+        released_v_target = min(self.v_cruise_setpoint, self.output_v_target + _TARGET_RELEASE_RATE * DT_MDL)
+        return self.output_v_target if released_v_target >= self.v_cruise_setpoint and not self._accel_release_ready() else released_v_target
+      return min(v_target, self.output_v_target)
 
     return V_CRUISE_UNSET
 
@@ -101,6 +121,9 @@ class SmartCruiseControlVision:
 
   def _update_state_machine(self) -> tuple[bool, bool]:
     # ENABLED, ENTERING, TURNING, LEAVING, OVERRIDING
+    relief = self.current_lat_acc < _FINISH_LAT_ACC_TH and self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH
+    self.relief_frames = self.relief_frames + 1 if self.state in ACTIVE_STATES and relief else 0
+
     if self.state != VisionState.disabled:
       # longitudinal and feature disable always have priority in a non-disabled state
       if not self.long_enabled or not self.enabled:
@@ -128,23 +151,27 @@ class SmartCruiseControlVision:
           # Transition to Turning if current lateral acceleration is over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Abort if the predicted lateral acceleration drops
-          elif self.max_pred_lat_acc < _ABORT_ENTERING_PRED_LAT_ACC_TH:
-            self.state = VisionState.enabled
+          # Begin releasing only after both current and predicted lateral acceleration stay clear.
+          elif self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES:
+            self.state = VisionState.leaving
 
         # TURNING
         elif self.state == VisionState.turning:
-          # Transition to Leaving if current lateral acceleration drops below a threshold.
+          # Transition out of Turning if current lateral acceleration drops below a threshold.
           if self.current_lat_acc <= _LEAVING_LAT_ACC_TH:
-            self.state = VisionState.leaving
+            self.state = VisionState.entering if self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH else VisionState.leaving
 
         # LEAVING
         elif self.state == VisionState.leaving:
           # Transition back to Turning if current lateral acceleration goes back over the threshold.
           if self.current_lat_acc >= _TURNING_LAT_ACC_TH:
             self.state = VisionState.turning
-          # Finish if current lateral acceleration goes below a threshold.
-          elif self.current_lat_acc < _FINISH_LAT_ACC_TH:
+          # Start a new turn cycle immediately if another curve is predicted.
+          elif self.max_pred_lat_acc >= _ENTERING_PRED_LAT_ACC_TH:
+            self.state = VisionState.entering
+          # Finish after confirmed relief and a gradual release to the cruise setpoint.
+          elif (self.relief_frames >= _RELIEF_CONFIRMATION_FRAMES and self.output_v_target >= self.v_cruise_setpoint
+                and self._accel_release_ready()):
             self.state = VisionState.enabled
 
     # DISABLED
@@ -157,6 +184,8 @@ class SmartCruiseControlVision:
 
     enabled = self.state in ENABLED_STATES
     active = self.state in ACTIVE_STATES
+    if not active:
+      self.relief_frames = 0
 
     return enabled, active
 

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

@@ -282,6 +282,42 @@ def test_e2e_to_radar_acc_handoff_keeps_braking_continuous():
   assert active[transition]
 
 
+def test_dec_retains_acc_through_route_like_radar_marker_dropout():
+  dropout_start = 1.0
+  reacquisition_time = 1.8
+
+  def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation:
+    frame = round(current_time / DT_MDL)
+    if current_time < dropout_start:
+      marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo"
+      return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1}
+    if current_time < reacquisition_time:
+      return truth | {"radar": False, "radarTrackId": -1}
+    return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1}
+
+  plant = Plant(
+    e2e=True, lead_relevancy=True, speed=20.0, distance_lead=35.0, lead_observation_fn=observe,
+    model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20,
+  )
+  _configure_plant(plant, enabled=True, dec_enabled=True)
+  rows = []
+  while plant.current_time < 2.5:
+    result = plant.step(v_lead=18.0, v_cruise=30.0)
+    rows.append((plant.current_time, result["a_target"], result["dec_mode"], str(result["mpc_source"]), result["fcw"]))
+
+  time_values = np.asarray([row[0] for row in rows])
+  acceleration = np.asarray([row[1] for row in rows])
+  dropout = (time_values >= dropout_start) & (time_values < reacquisition_time)
+  response = (time_values >= dropout_start - DT_MDL) & (time_values <= reacquisition_time + 0.5)
+  assert all(row[2] == "acc" for row in rows)
+  assert all(row[3] != "e2e" for row in rows)
+  assert not any(row[4] for row in rows)
+  assert dropout.any()
+  assert not _has_propulsion_brake_cycle(acceleration[response])
+  assert np.max(np.abs(np.diff(acceleration[response]) / DT_MDL)) < 3.0
+  assert not plant.planner.accel_controller_fault_latched
+
+
 def test_active_controller_is_pre_mpc_and_preserves_stock_lead_authority():
   plant = Plant(lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20)
   _configure_plant(plant, enabled=True, profile=0)