From 8adcfc04729083003983ef89b2ce5c1e9094ae9f Mon Sep 17 00:00:00 2001
From: rav4kumar <36933347+rav4kumar@users.noreply.github.com>
Date: Wed, 19 Aug 2026 11:57:03 -0700
Subject: [PATCH] sng shit

---
 .../controls/lib/longitudinal_planner.py      |   2 +-
 .../selfdrive/ui/layouts/settings/toggles.py  |   6 +-
 .../mpc_comfort_controller.py                 |  95 ------
 .../tests/test_mpc_comfort_controller.py      | 291 ------------------
 .../controls/lib/lead_departure_controller.py | 115 +++++++
 .../controls/lib/longitudinal_planner.py      |  22 +-
 .../tests/test_lead_departure_controller.py   | 266 ++++++++++++++++
 .../settings_ui_src/pages/cruise.yaml         |   7 +-
 8 files changed, 396 insertions(+), 408 deletions(-)
 delete mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/mpc_comfort_controller.py
 delete mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_mpc_comfort_controller.py
 create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py
 create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py

diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py
index f0ffa220f1..0e38eb52dd 100755
--- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py
@@ -149,6 +149,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
     output_a_target_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
                                               action_t=action_t)
     output_should_stop_mpc = should_stop(v_ego, output_a_target_mpc)
+    output_should_stop_mpc = self.update_lead_departure(sm, output_a_target_mpc, output_should_stop_mpc, reset_state)
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
@@ -156,7 +157,6 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
 
     max_accel_override = self.get_max_accel_override(v_ego)
     min_accel_override = self.get_min_accel_override(v_ego, is_e2e, force_decel)
-    output_a_target_mpc = self.update_mpc_comfort(sm, output_a_target_mpc, self.a_desired_trajectory, CONTROL_N_T_IDX, reset_state, min_accel_override)
     self.a_cruise, self.accel_controller_active = get_cruise_accel(is_e2e, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
                                      accel_coast, self.allow_throttle, max_accel_override, min_accel_override)
diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py
index 89643b7552..a3b52e8a26 100644
--- a/openpilot/selfdrive/ui/layouts/settings/toggles.py
+++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py
@@ -28,11 +28,11 @@ DESCRIPTIONS = {
     "your steering wheel distance button."
   ),
   "AccelPersonalityEnabled": tr_noop(
-    "Sets the acceleration limits and early lead-deceleration response for each profile. Following distance, emergency braking, and final-stop " +
-    "state remain controlled by the longitudinal MPC."
+    "Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking, and stopping behavior remain " +
+    "independent of this setting."
   ),
   "AccelPersonality": tr_noop(
-    "Select the vehicle acceleration and early lead-deceleration response."
+    "Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across profiles."
   ),
   "IsLdwEnabled": tr_noop(
     "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/mpc_comfort_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/mpc_comfort_controller.py
deleted file mode 100644
index c59e2fe445..0000000000
--- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/mpc_comfort_controller.py
+++ /dev/null
@@ -1,95 +0,0 @@
-"""
-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.common.realtime import DT_MDL
-
-_BRAKE_JERK = 1.4
-_RELEASE_JERK = 0.8
-_ACTIVATION_JERK = 1.2
-_ACTIVATION_DELTA = 0.15
-_LEAD_LOSS_HOLD_TIME = 0.2
-_PREVIEW_START_TIME = 0.5
-_CONFIRMATION_FRAMES = 2
-
-
-class MpcComfortController:
-  def __init__(self, dt: float = DT_MDL):
-    self.dt = dt
-    self._a_target: float | None = None
-    self._last_raw_target: float | None = None
-    self._confirmation_frames = 0
-    self._hold_frames = 0
-
-  @property
-  def active(self) -> bool:
-    return self._a_target is not None
-
-  def reset(self) -> None:
-    self._a_target = None
-    self._last_raw_target = None
-    self._confirmation_frames = 0
-    self._hold_frames = 0
-
-  def update(self, a_target: float, a_trajectory, t_idxs, lead_present: bool, a_min: float, reset: bool = False) -> float:
-    if reset:
-      self.reset()
-      return a_target
-
-    a_trajectory = np.asarray(a_trajectory, dtype=float)
-    t_idxs = np.asarray(t_idxs, dtype=float)
-    if (
-      len(a_trajectory) != len(t_idxs)
-      or not np.isfinite(a_target)
-      or not np.isfinite(a_min)
-      or not np.all(np.isfinite(a_trajectory))
-      or not np.all(np.isfinite(t_idxs))
-    ):
-      self.reset()
-      return a_target
-
-    future_a = a_trajectory[t_idxs >= _PREVIEW_START_TIME]
-    if len(future_a) == 0:
-      self.reset()
-      return a_target
-
-    previous_raw_target = self._last_raw_target
-    self._last_raw_target = a_target
-    preview_a = max(float(np.min(future_a)), a_min)
-    preview_requested = lead_present and preview_a < a_target - _ACTIVATION_DELTA
-
-    if self._a_target is None:
-      raw_target_falling = previous_raw_target is not None and a_target - previous_raw_target < -_ACTIVATION_JERK * self.dt
-      if raw_target_falling:
-        self._confirmation_frames = 0
-        return a_target
-
-      self._confirmation_frames = self._confirmation_frames + 1 if preview_requested else 0
-      if self._confirmation_frames < _CONFIRMATION_FRAMES:
-        return a_target
-      self._a_target = a_target
-
-    # Never delay braking requested by MPC.
-    if a_target < self._a_target:
-      self.reset()
-      return a_target
-
-    if preview_requested and preview_a <= self._a_target:
-      self._hold_frames = round(_LEAD_LOSS_HOLD_TIME / self.dt)
-      self._a_target = max(preview_a, self._a_target - _BRAKE_JERK * self.dt)
-    elif self._hold_frames > 0:
-      self._hold_frames -= 1
-    else:
-      release_target = preview_a if preview_requested else a_target
-      self._a_target = min(release_target, self._a_target + _RELEASE_JERK * self.dt)
-
-    if self._a_target >= a_target:
-      self.reset()
-      return a_target
-
-    return self._a_target
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_mpc_comfort_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_mpc_comfort_controller.py
deleted file mode 100644
index bb2f7ba8f3..0000000000
--- a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_mpc_comfort_controller.py
+++ /dev/null
@@ -1,291 +0,0 @@
-"""
-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 contextlib import ExitStack
-from types import SimpleNamespace
-from unittest import mock
-
-import numpy as np
-
-from openpilot.common.realtime import DT_MDL
-from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import mpc_comfort_controller as comfort
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelProfile
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.mpc_comfort_controller import MpcComfortController
-from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, MpcPlanSource
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP, PRIUS_TSS2_ROUTE_MODEL
-
-
-class TestMpcComfortController(OpenpilotTestCase):
-  def setUp(self):
-    self.controller = MpcComfortController()
-    self.times = np.array([0.0, 0.5, 2.5])
-    self.risk_horizon = np.array([0.0, -0.5, -2.0])
-    self.a_min = -1.0
-
-  def activate(self, a_target: float = 0.0) -> float:
-    self.assertEqual(self.controller.update(a_target, self.risk_horizon, self.times, True, self.a_min), a_target)
-    return self.controller.update(a_target, self.risk_horizon, self.times, True, self.a_min)
-
-  def test_preview_brake_jerk_and_profile_limit(self):
-    output = self.activate()
-    self.assertAlmostEqual(output, -comfort._BRAKE_JERK * DT_MDL)
-
-    outputs = [output]
-    while outputs[-1] > self.a_min:
-      outputs.append(self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min))
-
-    brake_step = comfort._BRAKE_JERK * DT_MDL
-    assert all(next_a - a >= -brake_step - 1e-9 for a, next_a in zip(outputs, outputs[1:], strict=False))
-    self.assertEqual(outputs[-1], self.a_min)
-    assert self.controller.active
-
-  def test_falling_raw_target_blocks_activation(self):
-    self.assertEqual(self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min), 0.0)
-    raw_target = -(comfort._ACTIVATION_JERK + 0.1) * DT_MDL
-    self.assertEqual(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
-    assert not self.controller.active
-
-    self.assertEqual(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
-    self.assertLess(self.controller.update(raw_target, self.risk_horizon, self.times, True, self.a_min), raw_target)
-
-  def test_raw_emergency_wins_and_clears_state(self):
-    self.activate()
-    self.assertEqual(self.controller.update(-3.5, self.risk_horizon, self.times, True, self.a_min), -3.5)
-    assert not self.controller.active
-    self.assertEqual(self.controller.update(0.2, np.zeros(3), self.times, True, self.a_min), 0.2)
-
-  def test_transient_preview_does_not_activate(self):
-    for horizon in (self.risk_horizon, np.zeros(3)) * 4:
-      self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
-      assert not self.controller.active
-
-  def test_lead_dropout_holds_then_releases_at_bounded_jerk(self):
-    self.activate()
-    for _ in range(20):
-      self.controller.update(0.0, self.risk_horizon, self.times, True, self.a_min)
-
-    start = self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min)
-    hold_frames = round(comfort._LEAD_LOSS_HOLD_TIME / DT_MDL)
-    held = [start]
-    for _ in range(hold_frames - 1):
-      held.append(self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min))
-    assert all(a == start for a in held)
-
-    released = [held[-1]]
-    while self.controller.active:
-      released.append(self.controller.update(0.0, np.zeros(3), self.times, False, self.a_min))
-    release_step = comfort._RELEASE_JERK * DT_MDL
-    assert all(0.0 <= next_a - a <= release_step + 1e-9 for a, next_a in zip(released, released[1:], strict=False))
-    self.assertEqual(released[-1], 0.0)
-
-  def test_active_preview_releases_at_bounded_jerk(self):
-    self.activate()
-    for _ in range(20):
-      self.controller.update(0.5, self.risk_horizon, self.times, True, self.a_min)
-
-    milder_horizon = np.array([0.0, -0.2, -0.2])
-    previous = self.controller.update(0.5, self.risk_horizon, self.times, True, self.a_min)
-    for _ in range(round(comfort._LEAD_LOSS_HOLD_TIME / DT_MDL)):
-      self.assertEqual(self.controller.update(0.5, milder_horizon, self.times, True, self.a_min), previous)
-    output = self.controller.update(0.5, milder_horizon, self.times, True, self.a_min)
-    self.assertAlmostEqual(output - previous, comfort._RELEASE_JERK * DT_MDL)
-
-  def test_alternating_preview_does_not_reverse_jerk(self):
-    self.activate(0.5)
-    outputs = []
-    for frame in range(20):
-      horizon = self.risk_horizon if frame % 2 == 0 else np.zeros(3)
-      outputs.append(self.controller.update(0.5, horizon, self.times, True, self.a_min))
-
-    assert np.all(np.diff(outputs) <= 1e-9)
-
-  def test_reset_and_invalid_horizon_return_raw(self):
-    self.activate()
-    self.assertEqual(self.controller.update(0.4, self.risk_horizon, self.times, True, self.a_min, reset=True), 0.4)
-    self.assertEqual(self.controller.update(0.2, np.array([0.0, np.nan, np.inf]), self.times, True, self.a_min), 0.2)
-    assert not self.controller.active
-
-  def test_current_state_anchor_is_excluded(self):
-    horizon = np.array([-2.0, 0.0, 0.0])
-    self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
-    self.assertEqual(self.controller.update(0.0, horizon, self.times, True, self.a_min), 0.0)
-    assert not self.controller.active
-
-  def test_output_never_weakens_raw_target(self):
-    for a_target, horizon in ((0.2, self.risk_horizon), (-0.2, np.zeros(3)), (-3.5, self.risk_horizon)):
-      self.assertLessEqual(self.controller.update(a_target, horizon, self.times, True, self.a_min), a_target)
-
-
-class TestInheritedMpcComfortHook(OpenpilotTestCase):
-  def setUp(self):
-    self.planner = object.__new__(LongitudinalPlannerSP)
-    self.planner.mpc_comfort_controller = MpcComfortController()
-    self.planner.mpc = SimpleNamespace(source=MpcPlanSource.lead1)
-    self.times = np.array([0.0, 0.5, 2.5])
-    self.horizon = np.array([0.0, -0.5, -2.0])
-    self.a_min = -1.0
-
-  @staticmethod
-  def make_sm(v_ego: float = 10.0):
-    return {
-      'radarState': SimpleNamespace(leadOne=SimpleNamespace(present=False), leadTwo=SimpleNamespace(present=True)),
-      'carControl': SimpleNamespace(cruiseControl=SimpleNamespace(override=False)),
-      'carState': SimpleNamespace(standstill=False, vEgo=v_ego),
-    }
-
-  def test_lead_two_uses_inherited_hook(self):
-    sm = self.make_sm()
-    self.assertEqual(self.planner.update_mpc_comfort(sm, 0.0, self.horizon, self.times, False, self.a_min), 0.0)
-    self.assertLess(self.planner.update_mpc_comfort(sm, 0.0, self.horizon, self.times, False, self.a_min), 0.0)
-
-  def test_non_lead_source_is_ignored(self):
-    self.planner.mpc.source = MpcPlanSource.cruise
-    for _ in range(3):
-      self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(), 0.0, self.horizon, self.times, False, self.a_min), 0.0)
-    assert not self.planner.mpc_comfort_controller.active
-
-  def test_disabled_controller_is_exact_stock(self):
-    self.planner.mpc_comfort_controller.update(0.0, self.horizon, self.times, True, self.a_min)
-    self.planner.mpc_comfort_controller.update(0.0, self.horizon, self.times, True, self.a_min)
-
-    self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(), 0.2, self.horizon, self.times, False, None), 0.2)
-    assert not self.planner.mpc_comfort_controller.active
-
-  def test_stop_region_remains_raw(self):
-    self.assertEqual(self.planner.update_mpc_comfort(self.make_sm(0.29), 0.2, self.horizon, self.times, False, self.a_min), 0.2)
-    assert not self.planner.mpc_comfort_controller.active
-
-    sm = self.make_sm(0.3)
-    self.assertEqual(self.planner.update_mpc_comfort(sm, 0.2, self.horizon, self.times, False, self.a_min), 0.2)
-    self.assertLess(self.planner.update_mpc_comfort(sm, 0.2, self.horizon, self.times, False, self.a_min), 0.2)
-
-
-def _run_closed_loop(comfort_enabled, speed, gap, cruise, duration, lead_speed):
-  plant = PlantSP(lead_relevancy=True, speed=speed, distance_lead=gap, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
-  plant.v_lead_prev = lead_speed(0.0)
-  planner = plant.planner
-  planner.accel_controller._enabled = True
-  planner.accel_controller._profile = AccelProfile.eco
-  planner.dec._enabled = False
-  solver_failures = 0
-
-  with ExitStack() as patches:
-    patches.enter_context(mock.patch.object(planner.accel_controller, "update", return_value=None))
-    patches.enter_context(mock.patch.object(planner.dec, "_read_params", return_value=None))
-
-    if not comfort_enabled:
-
-      def bypass_comfort(_sm, a_target, *_args):
-        planner.mpc_comfort_controller.reset()
-        return a_target
-
-      patches.enter_context(mock.patch.object(planner, "update_mpc_comfort", side_effect=bypass_comfort))
-
-    original_reset = planner.mpc.reset
-
-    def count_reset(*args, **kwargs):
-      nonlocal solver_failures
-      solver_failures += int(planner.mpc.solution_status != 0)
-      return original_reset(*args, **kwargs)
-
-    patches.enter_context(mock.patch.object(planner.mpc, "reset", side_effect=count_reset))
-    rows = []
-    for _ in range(round(duration / DT_MDL)):
-      current_time = plant.current_time
-      v_lead = lead_speed(current_time)
-      result = plant.step(v_lead=v_lead, v_cruise=cruise)
-      current_gap = result["distance_lead"] - result["distance"]
-      closing_speed = max(result["speed"] - v_lead, 0.0)
-      rows.append(
-        (
-          current_time,
-          result["a_target"],
-          result["actuator_command"],
-          result["realized_acceleration"],
-          result["speed"],
-          current_gap,
-          current_gap / closing_speed if closing_speed > 0.01 else np.inf,
-          planner.mpc_comfort_controller.active,
-          result["fcw"],
-        )
-      )
-
-  data = np.asarray(rows, dtype=float)
-  return {
-    "time": data[:, 0],
-    "target": data[:, 1],
-    "command": data[:, 2],
-    "accel": data[:, 3],
-    "speed": data[:, 4],
-    "gap": data[:, 5],
-    "ttc": data[:, 6],
-    "active": data[:, 7].astype(bool),
-    "fcw": data[:, 8].astype(bool),
-    "solver_failures": solver_failures,
-  }
-
-
-def _sustained_onset(times, values, threshold=-0.2):
-  for frame in range(len(values) - 1):
-    if values[frame] <= threshold and values[frame + 1] <= threshold:
-      return times[frame]
-  return None
-
-
-def _command_switches(command, deadband=0.05):
-  states = np.where(command < -deadband, -1, np.where(command > deadband, 1, 0))
-  states = states[states != 0]
-  return int(np.count_nonzero(states[1:] != states[:-1]))
-
-
-class TestMpcComfortClosedLoop(OpenpilotTestCase):
-  def test_closing_lead_brakes_earlier_and_more_smoothly(self):
-    def lead_speed(t):
-      return 10.0 - 1.5 * np.clip(t - 0.75, 0.0, 3.0)
-
-    stock = _run_closed_loop(False, 12.0, 45.0, 20.0, 7.0, lead_speed)
-    enabled = _run_closed_loop(True, 12.0, 45.0, 20.0, 7.0, lead_speed)
-
-    assert stock["solver_failures"] == enabled["solver_failures"] == 0
-    assert not stock["fcw"].any() and not enabled["fcw"].any()
-    assert enabled["active"].any()
-
-    stock_target_onset = _sustained_onset(stock["time"], stock["target"])
-    enabled_target_onset = _sustained_onset(enabled["time"], enabled["target"])
-    stock_accel_onset = _sustained_onset(stock["time"], stock["accel"])
-    enabled_accel_onset = _sustained_onset(enabled["time"], enabled["accel"])
-    assert None not in (stock_target_onset, enabled_target_onset, stock_accel_onset, enabled_accel_onset)
-    assert enabled_target_onset <= stock_target_onset - 1.5
-    assert enabled_accel_onset <= stock_accel_onset - 1.5
-
-    stock_target_jerk = np.diff(stock["target"]) / DT_MDL
-    enabled_target_jerk = np.diff(enabled["target"]) / DT_MDL
-    stock_accel_jerk = np.diff(stock["accel"]) / DT_MDL
-    enabled_accel_jerk = np.diff(enabled["accel"]) / DT_MDL
-    assert enabled_target_jerk.min() >= stock_target_jerk.min()
-    assert enabled_target_jerk.max() <= stock_target_jerk.max()
-    assert enabled_accel_jerk.min() >= stock_accel_jerk.min()
-    assert enabled_accel_jerk.max() <= stock_accel_jerk.max()
-    assert np.percentile(np.abs(enabled_accel_jerk), 95) <= np.percentile(np.abs(stock_accel_jerk), 95)
-    assert enabled["accel"].min() >= stock["accel"].min() + 0.5
-    assert enabled["gap"].min() >= stock["gap"].min()
-    assert enabled["ttc"].min() >= stock["ttc"].min()
-    assert _command_switches(enabled["command"]) <= _command_switches(stock["command"])
-
-  def test_hard_lead_brake_remains_stock(self):
-    def lead_speed(t):
-      return 22.0 - 4.0 * np.clip(t - 3.0, 0.0, 1.0)
-
-    stock = _run_closed_loop(False, 22.0, 60.0, 27.0, 10.0, lead_speed)
-    enabled = _run_closed_loop(True, 22.0, 60.0, 27.0, 10.0, lead_speed)
-
-    assert stock["solver_failures"] == enabled["solver_failures"] == 0
-    assert not stock["fcw"].any() and not enabled["fcw"].any()
-    assert not enabled["active"].any()
-    for key in ("target", "command", "accel", "speed", "gap"):
-      np.testing.assert_array_equal(enabled[key], stock[key])
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py
new file mode 100644
index 0000000000..652afd492a
--- /dev/null
+++ b/openpilot/sunnypilot/selfdrive/controls/lib/lead_departure_controller.py
@@ -0,0 +1,115 @@
+"""
+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 collections import deque
+import math
+from typing import Any
+
+from openpilot.cereal import log
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+
+
+LEAD_DEPARTURE_MIN_SPEED = 0.3
+LEAD_DEPARTURE_CONFIRM_FRAMES = 3
+LEAD_DEPARTURE_MIN_DISTANCE = 0.03
+LEAD_DEPARTURE_MAX_EGO_SPEED = 0.3
+
+MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+
+
+class LeadDepartureController:
+  def __init__(self, enabled: bool):
+    self.enabled = enabled
+    self._track_id: int | None = None
+    self._distances: deque[float] = deque(maxlen=LEAD_DEPARTURE_CONFIRM_FRAMES)
+    self._active = False
+
+  @property
+  def active(self) -> bool:
+    return self._active
+
+  def reset(self) -> None:
+    self._track_id = None
+    self._distances.clear()
+    self._active = False
+
+  @staticmethod
+  def _selected_lead(radar_state: Any, source: Any) -> Any | None:
+    if source == MpcPlanSource.lead0:
+      return radar_state.leadOne
+    if source == MpcPlanSource.lead1:
+      return radar_state.leadTwo
+    return None
+
+  @staticmethod
+  def _radar_has_errors(radar_state: Any) -> bool:
+    errors = radar_state.radarErrors
+    return errors.canError or errors.radarFault or errors.wrongConfig or errors.radarUnavailableTemporary
+
+  def update(self, sm: Any, source: Any, a_target: float, should_stop: bool, reset: bool, radar_valid: bool) -> bool:
+    CS = sm['carState']
+    CC = sm['carControl']
+    controls_state = sm['controlsState']
+    radar_state = sm['radarState']
+
+    blocked = (
+      not self.enabled
+      or reset
+      or not CC.longActive
+      or CC.cruiseControl.override
+      or CS.gasPressed
+      or CS.brakePressed
+      or controls_state.forceDecel
+      or controls_state.longControlState == LongCtrlState.off
+      or not radar_valid
+      or self._radar_has_errors(radar_state)
+    )
+    if blocked or not math.isfinite(CS.vEgo) or CS.vEgo >= LEAD_DEPARTURE_MAX_EGO_SPEED or not math.isfinite(a_target):
+      self.reset()
+      return should_stop
+
+    lead = self._selected_lead(radar_state, source)
+    lead_valid = (
+      lead is not None
+      and lead.present
+      and lead.radar
+      and lead.radarTrackId >= 0
+      and all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel))
+      and lead.dRel > 0.0
+      and lead.vLeadK >= LEAD_DEPARTURE_MIN_SPEED
+      and lead.vRel >= LEAD_DEPARTURE_MIN_SPEED
+      and a_target >= 0.0
+    )
+    if not lead_valid:
+      self.reset()
+      return should_stop
+
+    track_id = int(lead.radarTrackId)
+    if self._active:
+      if track_id != self._track_id:
+        self.reset()
+        return should_stop
+      return False
+
+    if not should_stop:
+      self.reset()
+      return False
+
+    if controls_state.longControlState != LongCtrlState.stopping:
+      self.reset()
+      return should_stop
+
+    if track_id != self._track_id:
+      self._track_id = track_id
+      self._distances.clear()
+    self._distances.append(float(lead.dRel))
+
+    if len(self._distances) == LEAD_DEPARTURE_CONFIRM_FRAMES and self._distances[-1] - self._distances[0] >= LEAD_DEPARTURE_MIN_DISTANCE:
+      self._active = True
+      return False
+
+    return should_stop
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
index 555fb91e53..0e69ece9f6 100644
--- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
@@ -5,14 +5,14 @@ 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 openpilot.cereal import messaging, custom, log
+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.accel_controller.accel_controller import AccelController
-from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.mpc_comfort_controller import MpcComfortController
 from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
 from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
+from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
 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
 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit.speed_limit_resolver import SpeedLimitResolver
@@ -21,13 +21,12 @@ from openpilot.sunnypilot.models.helpers import get_active_bundle
 
 DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState
 LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
-MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
 
 
 class LongitudinalPlannerSP:
   def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
     self.accel_controller = AccelController()
-    self.mpc_comfort_controller = MpcComfortController(mpc.dt)
+    self.lead_departure_controller = LeadDepartureController(CP.openpilotLongitudinalControl and CP.autoResumeSng and not CP.notCar)
     self.events_sp = EventsSP()
     self.dec = DynamicExperimentalController(CP, mpc)
     self.scc = SmartCruiseControl()
@@ -57,16 +56,9 @@ class LongitudinalPlannerSP:
       return None
     return self.accel_controller.get_min_accel(v_ego)
 
-  def update_mpc_comfort(self, sm: messaging.SubMaster, a_target: float, a_trajectory, t_idxs,
-                         reset_state: bool, a_min: float | None) -> float:
-    if a_min is None:
-      self.mpc_comfort_controller.reset()
-      return a_target
-
-    lead_present = ((self.mpc.source == MpcPlanSource.lead0 and sm['radarState'].leadOne.present) or
-                    (self.mpc.source == MpcPlanSource.lead1 and sm['radarState'].leadTwo.present))
-    reset = reset_state or sm['carControl'].cruiseControl.override or sm['carState'].standstill or sm['carState'].vEgo < 0.3
-    return self.mpc_comfort_controller.update(a_target, a_trajectory, t_idxs, lead_present, a_min, reset)
+  def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool:
+    radar_valid = sm.valid.get('radarState', False) and getattr(sm, 'alive', {}).get('radarState', False)
+    return self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']
@@ -123,7 +115,7 @@ class LongitudinalPlannerSP:
 
     accel_controller = longitudinalPlanSP.accelController
     accel_controller.enabled = self.accel_controller.is_enabled()
-    accel_controller.active = self.accel_controller_active or self.mpc_comfort_controller.active
+    accel_controller.active = self.accel_controller_active
     accel_controller.profile = self.accel_controller.profile
 
     # Smart Cruise Control
diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py
new file mode 100644
index 0000000000..b48af3eabf
--- /dev/null
+++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lead_departure_controller.py
@@ -0,0 +1,266 @@
+"""
+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
+from unittest import mock
+
+from openpilot.cereal import log
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.test import OpenpilotTestCase
+from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
+from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LeadDepartureController
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
+
+
+MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+
+
+def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.5, v_rel: float = 0.5, present: bool = True, radar: bool = True, track_id: int = 7):
+  return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, present=present, radar=radar, radarTrackId=track_id)
+
+
+def make_sm(
+  *,
+  lead_one=None,
+  lead_two=None,
+  v_ego: float = 0.0,
+  long_active: bool = True,
+  long_state=LongCtrlState.stopping,
+  gas: bool = False,
+  brake: bool = False,
+  override: bool = False,
+  force_decel: bool = False,
+  radar_error: str | None = None,
+):
+  errors = SimpleNamespace(canError=False, radarFault=False, wrongConfig=False, radarUnavailableTemporary=False)
+  if radar_error is not None:
+    setattr(errors, radar_error, True)
+  return {
+    'carState': SimpleNamespace(vEgo=v_ego, gasPressed=gas, brakePressed=brake),
+    'carControl': SimpleNamespace(longActive=long_active, cruiseControl=SimpleNamespace(override=override)),
+    'controlsState': SimpleNamespace(longControlState=long_state, forceDecel=force_decel),
+    'radarState': SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead(track_id=8), radarErrors=errors),
+  }
+
+
+def update(controller, sm, *, source=MpcPlanSource.lead0, a_target: float = 0.05, should_stop: bool = True, reset: bool = False, radar_valid: bool = True):
+  return controller.update(sm, source, a_target, should_stop, reset, radar_valid)
+
+
+def activate(controller: LeadDepartureController):
+  assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
+  assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
+  assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
+  assert controller.active
+
+
+def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None):
+  def observe_lead(_t, _name, truth):
+    truth.update(radar=True, radarTrackId=7)
+    return truth
+
+  def model_action(_t, _v_ego, _a_ego):
+    return 0.0, bool(model_should_stop)
+
+  plant = PlantSP(
+    lead_relevancy=True,
+    speed=0.0,
+    distance_lead=gap,
+    lead_observation_fn=observe_lead,
+    actuator_model=PRIUS_TSS2_ROUTE_MODEL,
+    run_long_control=True,
+    e2e=model_should_stop is not None,
+    model_action_fn=model_action if model_should_stop is not None else None,
+  )
+  plant.planner.lead_departure_controller.enabled = controller_enabled
+
+  original_update = plant.planner.update
+
+  def long_active_update(sm):
+    sm['carControl'].longActive = True
+    original_update(sm)
+
+  solver_resets = 0
+  original_reset = plant.planner.mpc.reset
+
+  def counted_reset(*args, **kwargs):
+    nonlocal solver_resets
+    if plant.planner.mpc.solution_status != 0:
+      solver_resets += 1
+    return original_reset(*args, **kwargs)
+
+  rows = []
+  active = []
+  with (
+    mock.patch.object(plant.planner, 'get_max_accel_override', return_value=None),
+    mock.patch.object(plant.planner, 'get_min_accel_override', return_value=None),
+    mock.patch.object(plant.planner, 'update', side_effect=long_active_update),
+    mock.patch.object(plant.planner.mpc, 'reset', side_effect=counted_reset),
+  ):
+    for _ in range(round(duration / DT_MDL)):
+      t = plant.current_time
+      result = plant.step(v_lead=lead_speed(t), v_cruise=8.0)
+      rows.append(
+        (t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw'])
+      )
+      active.append(plant.planner.lead_departure_controller.active)
+
+  return rows, active, solver_resets
+
+
+def first_delay(rows, cue: float, column: int, predicate):
+  return next(row[0] - cue for row in rows if row[0] >= cue and predicate(row[column]))
+
+
+class TestLeadDepartureController(OpenpilotTestCase):
+  def test_requires_three_coherent_radar_frames(self):
+    controller = LeadDepartureController(True)
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)))
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.01)))
+    assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)))
+    assert controller.active
+
+  def test_distance_confirmation_uses_a_sliding_three_frame_window(self):
+    controller = LeadDepartureController(True)
+    for d_rel in (4.00, 4.01, 4.02, 4.03):
+      assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
+    assert not controller.active
+
+    assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.06)))
+    assert controller.active
+
+  def test_persistent_false_speed_cue_with_static_range_never_arms(self):
+    controller = LeadDepartureController(True)
+    for _ in range(10):
+      assert update(controller, make_sm(lead_one=make_lead(d_rel=4.0)))
+    assert not controller.active
+
+  def test_same_track_can_move_between_lead_slots(self):
+    controller = LeadDepartureController(True)
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00)), source=MpcPlanSource.lead0)
+    assert update(controller, make_sm(lead_two=make_lead(d_rel=4.01)), source=MpcPlanSource.lead1)
+    assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.04)), source=MpcPlanSource.lead0)
+
+  def test_different_track_restarts_confirmation(self):
+    controller = LeadDepartureController(True)
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.00, track_id=7)))
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.02, track_id=7)))
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.20, track_id=9)))
+    assert update(controller, make_sm(lead_one=make_lead(d_rel=4.22, track_id=9)))
+    assert not update(controller, make_sm(lead_one=make_lead(d_rel=4.24, track_id=9)))
+
+  def test_active_release_latches_through_native_threshold_churn(self):
+    controller = LeadDepartureController(True)
+    activate(controller)
+    sm = make_sm(lead_one=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
+    assert not update(controller, sm, a_target=0.12, should_stop=False)
+    assert not update(controller, sm, a_target=0.05, should_stop=True)
+    assert controller.active
+
+  def test_active_release_latches_across_same_track_source_churn(self):
+    controller = LeadDepartureController(True)
+    activate(controller)
+    sm = make_sm(lead_two=make_lead(d_rel=4.10), long_state=LongCtrlState.pid)
+    assert not update(controller, sm, source=MpcPlanSource.lead1)
+    assert controller.active
+
+  def test_active_release_cancels_on_invalid_state(self):
+    cases = (
+      ('lead lost', make_sm(lead_one=make_lead(present=False))),
+      ('vision lead', make_sm(lead_one=make_lead(radar=False))),
+      ('track changed', make_sm(lead_one=make_lead(track_id=9))),
+      ('lead too slow', make_sm(lead_one=make_lead(v_lead=0.29))),
+      ('relative speed too low', make_sm(lead_one=make_lead(v_rel=0.29))),
+      ('gas', make_sm(gas=True)),
+      ('brake', make_sm(brake=True)),
+      ('override', make_sm(override=True)),
+      ('force decel', make_sm(force_decel=True)),
+      ('long inactive', make_sm(long_active=False)),
+      ('long control off', make_sm(long_state=LongCtrlState.off)),
+      ('ego rolling', make_sm(v_ego=0.3)),
+      ('radar CAN error', make_sm(radar_error='canError')),
+      ('radar fault', make_sm(radar_error='radarFault')),
+      ('radar config', make_sm(radar_error='wrongConfig')),
+      ('radar unavailable', make_sm(radar_error='radarUnavailableTemporary')),
+    )
+    for name, sm in cases:
+      with self.subTest(name=name):
+        controller = LeadDepartureController(True)
+        activate(controller)
+        assert update(controller, sm)
+        assert not controller.active
+
+  def test_active_release_cancels_on_invalid_update_input(self):
+    cases = (('negative target', -0.01, False, True), ('reset', 0.05, True, True), ('radar invalid', 0.05, False, False))
+    for name, a_target, reset, radar_valid in cases:
+      with self.subTest(name=name):
+        controller = LeadDepartureController(True)
+        activate(controller)
+        assert update(controller, make_sm(), a_target=a_target, reset=reset, radar_valid=radar_valid)
+        assert not controller.active
+
+  def test_inactive_controller_arms_only_from_native_stop_and_stopping_state(self):
+    controller = LeadDepartureController(True)
+    for d_rel in (4.00, 4.02, 4.04):
+      assert not update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)), should_stop=False)
+    for d_rel in (4.00, 4.02, 4.04):
+      assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel), long_state=LongCtrlState.pid))
+    assert not controller.active
+
+  def test_capability_gate_disables_controller(self):
+    controller = LeadDepartureController(False)
+    for d_rel in (4.00, 4.02, 4.04):
+      assert update(controller, make_sm(lead_one=make_lead(d_rel=d_rel)))
+    assert not controller.active
+
+  def test_closed_loop_departure_releases_earlier_without_a_safety_regression(self):
+    lead_accel = 0.31
+    cue = 1.0 + 0.4 / lead_accel
+
+    def lead_speed(t):
+      return 0.0 if t < 1.0 else min(5.0, lead_accel * (t - 1.0))
+
+    stock, stock_active, stock_resets = run_closed_loop(False, 3.81, lead_speed, 8.0)
+    controller, controller_active, controller_resets = run_closed_loop(True, 3.81, lead_speed, 8.0)
+
+    stock_release = first_delay(stock, cue, 5, lambda should_stop: not should_stop)
+    controller_release = first_delay(controller, cue, 5, lambda should_stop: not should_stop)
+    stock_motion = first_delay(stock, cue, 1, lambda speed: speed > 0.01)
+    controller_motion = first_delay(controller, cue, 1, lambda speed: speed > 0.01)
+    stock_v01 = first_delay(stock, cue, 1, lambda speed: speed > 0.1)
+    controller_v01 = first_delay(controller, cue, 1, lambda speed: speed > 0.1)
+
+    assert any(controller_active) and not any(stock_active)
+    assert stock_resets == controller_resets == 0
+    assert not any(row[6] for row in stock + controller)
+    assert controller_release <= stock_release - 1.0
+    assert controller_motion <= stock_motion - 0.1
+    assert controller_v01 <= stock_v01 - 0.1
+    assert min(row[3] for row in controller) >= min(row[3] for row in stock)
+    assert max(abs(right[4] - left[4]) for left, right in zip(controller, controller[1:], strict=False)) <= max(
+      abs(right[4] - left[4]) for left, right in zip(stock, stock[1:], strict=False)
+    )
+
+  def test_model_stop_remains_authoritative(self):
+    def lead_speed(t):
+      return 0.0 if t < 1.0 else min(5.0, 0.8 * (t - 1.0))
+
+    rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True)
+
+    assert any(active)
+    assert solver_resets == 0
+    assert all(row[5] for row in rows)
+    assert all(row[1] == 0.0 and row[2] == 0.0 for row in rows)
+    assert not any(row[6] for row in rows)
+
+  def test_stationary_lead_remains_stock_identical(self):
+    stock, stock_active, stock_resets = run_closed_loop(False, 8.0, lambda _t: 0.0, 12.0)
+    controller, controller_active, controller_resets = run_closed_loop(True, 8.0, lambda _t: 0.0, 12.0)
+
+    assert stock == controller
+    assert not any(stock_active) and not any(controller_active)
+    assert stock_resets == controller_resets == 0
diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml
index 73756ef89b..bd6d08d6a0 100644
--- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml
+++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml
@@ -46,8 +46,8 @@ sections:
   - key: AccelPersonalityEnabled
     widget: toggle
     title: Enable Accel Controller
-    description: Sets the acceleration limits and early lead-deceleration response for each profile. Following distance,
-      emergency braking, and final-stop state remain controlled by the longitudinal MPC.
+    description: Sets your preferred acceleration and cruise-deceleration limits by profile. Lead following, braking,
+      and stopping behavior remain independent of this setting.
     visibility:
     - $ref: '#/macros/longitudinal'
     enablement:
@@ -55,7 +55,8 @@ sections:
   - key: AccelPersonality
     widget: multiple_button
     title: Acceleration Profile
-    description: Select the vehicle acceleration and early lead-deceleration response.
+    description: Select the vehicle acceleration response. Chauffeur braking and stopping behavior remain the same across
+      profiles.
     options:
     - value: 0
       label: Eco