Tune acceleration profiles and lead response

openpilot/selfdrive/controls/lib/drive_helpers.py

@@ -14,8 +14,11 @@ MAX_LATERAL_JERK = 5.0  # m/s^3
 MAX_LATERAL_ACCEL_NO_ROLL = 3.0  # m/s^2
 
 
+STOPPING_SPEED = 0.25  # m/s
+
+
 def should_stop(v_ego: float, a_target: float) -> bool:
-  return bool(v_ego < 0.3 and a_target < 0.1)
+  return bool(v_ego < STOPPING_SPEED and a_target < 0.1)
 
 def clamp(val, min_val, max_val):
   clamped_val = float(np.clip(val, min_val, max_val))

openpilot/selfdrive/controls/lib/longcontrol.py

@@ -7,6 +7,8 @@ from openpilot.selfdrive.modeld.constants import ModelConstants
 
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 
+STOPPING_DECEL_RATE = 0.3  # m/s^2/s while trying to stop
+
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
@@ -67,8 +69,7 @@ class LongControl:
       output_accel = self.last_output_accel
       if output_accel > self.CP.stopAccel:
         output_accel = min(output_accel, 0.0)
-        # TODO: can we just go straight to stopAccel?
-        output_accel -= 1.0 * DT_CTRL  # m/s^2/s while trying to stop
+        output_accel -= STOPPING_DECEL_RATE * DT_CTRL
       self.reset()
 
     else:  # LongCtrlState.pid

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -141,7 +141,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_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
 

openpilot/selfdrive/controls/tests/test_longcontrol.py

@@ -1,10 +1,40 @@
+from types import SimpleNamespace
+
 from openpilot.common.test import OpenpilotTestCase
+from openpilot.common.realtime import DT_CTRL
 from openpilot.cereal import custom
-from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
+from openpilot.selfdrive.controls.lib.drive_helpers import should_stop
+from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
+
+
+def make_stopping_control(last_output_accel):
+  CP = SimpleNamespace(stopAccel=-2.0, longitudinalTuning=SimpleNamespace(kiBP=[0.0], kiV=[0.0]))
+  control = LongControl(CP, custom.CarParamsSP.new_message())
+  control.long_control_state = LongCtrlState.stopping
+  control.last_output_accel = last_output_accel
+  CS = SimpleNamespace(aEgo=0.0, brakePressed=False, cruiseState=SimpleNamespace(standstill=False), vEgo=0.0)
+  return control, CS
 
 
 class TestLongControlStateTransition(OpenpilotTestCase):
 
+  def test_stopping_threshold_boundaries(self):
+    assert should_stop(0.249, 0.099)
+    assert not should_stop(0.250, 0.099)
+    assert not should_stop(0.249, 0.1)
+
+  def test_stopping_ramp_rate(self):
+    control, CS = make_stopping_control(-0.2)
+    output = control.update(True, CS, a_target=-0.2, should_stop=True, accel_limits=(-3.5, 2.0))
+
+    self.assertAlmostEqual(output, -0.2 - 0.3 * DT_CTRL)
+
+  def test_stopping_does_not_release_stronger_braking(self):
+    control, CS = make_stopping_control(-2.2)
+    output = control.update(True, CS, a_target=-0.2, should_stop=True, accel_limits=(-3.5, 2.0))
+
+    self.assertEqual(output, -2.2)
+
   def test_stay_stopped(self):
     CP_SP = custom.CarParamsSP.new_message()
     active = True

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

@@ -10,15 +10,16 @@ import numpy as np
 from openpilot.cereal import custom
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import MAX_T, STOP_DISTANCE
 from openpilot.sunnypilot import get_sanitize_int_param
 
 AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
 
 MAX_ACCEL_BREAKPOINTS = [0., 3., 5., 8., 10., 25., 40.]
 MAX_ACCEL_PROFILES = {
-  AccelProfile.eco:    [1.50, 1.42, 0.90, 0.52, 0.44, 0.30, 0.23],
-  AccelProfile.normal: [1.60, 1.48, 1.00, 0.60, 0.50, 0.36, 0.28],
-  AccelProfile.sport:  [1.80, 1.70, 1.12, 0.70, 0.60, 0.44, 0.35],
+  AccelProfile.eco:    [1.20, 0.95, 0.70, 0.48, 0.40, 0.28, 0.18],
+  AccelProfile.normal: [1.60, 1.35, 1.05, 0.65, 0.52, 0.40, 0.28],
+  AccelProfile.sport:  [2.00, 1.90, 1.55, 1.00, 0.80, 0.60, 0.40],
 }
 TARGET_SPEED_DEADBAND = 0.2  # m/s
 TARGET_SPEED_APPROACH_WINDOW = 2.0  # m/s
@@ -27,6 +28,8 @@ TARGET_SPEED_APPROACH_MIN_SPEED = 3.0  # m/s
 TARGET_SPEED_APPROACH_FULL_SPEED = 5.0  # m/s
 CATCHUP_ERROR_BREAKPOINTS = [TARGET_SPEED_DEADBAND, 0.5, 1.0, 2.0, 3.0, 4.0]
 CATCHUP_ACCEL_SCALE = [0.0, 0.3, 0.5, 0.7, 0.85, 1.0]
+LEAD_COMFORT_ACCEL = -0.4  # m/s^2
+LEAD_COMFORT_JERK = 0.6  # m/s^3
 
 
 class AccelController:
@@ -76,3 +79,31 @@ class AccelController:
     adjusted_error = np.sign(speed_error) * max(0.0, abs(speed_error) - deadband)
     gain = 1.0 - (1.0 - TARGET_SPEED_APPROACH_GAIN) * speed_blend * target_blend
     return float(v_ego + gain * adjusted_error)
+
+  def get_lead_departure_accel(self, v_ego: float, v_lead: float, a_lead: float, mpc_accel: float) -> float:
+    values = (v_ego, v_lead, a_lead, mpc_accel)
+    if not self._enabled or not all(np.isfinite(value) for value in values) or a_lead < 0.0 or mpc_accel < 0.0 or v_lead <= v_ego:
+      return mpc_accel
+
+    return max(mpc_accel, min(v_lead - v_ego, self.get_max_accel(v_ego, v_lead)))
+
+  def get_lead_accel(self, v_ego: float, d_rel: float, v_rel: float, a_lead: float,
+                     cruise_accel: float, previous_accel: float, t_follow: float) -> float:
+    values = (v_ego, d_rel, v_rel, a_lead, cruise_accel, previous_accel, t_follow)
+    if not self._enabled or not all(np.isfinite(value) for value in values) or v_ego < TARGET_SPEED_APPROACH_FULL_SPEED or d_rel <= 0.0:
+      return cruise_accel
+
+    v_lead = max(v_ego + v_rel, 0.0)
+    clearance = d_rel - (STOP_DISTANCE + t_follow * v_lead)
+    lead_lookahead = 2.0 * t_follow
+    projected_closing = max(-(v_rel + a_lead * lead_lookahead), 0.0)
+    if projected_closing <= 0.0 or clearance / projected_closing >= MAX_T:
+      return cruise_accel
+
+    required_accel = a_lead - projected_closing ** 2 / (2.0 * max(clearance, STOP_DISTANCE))
+    target_accel = float(np.clip(required_accel, LEAD_COMFORT_ACCEL, 0.0))
+
+    max_step = LEAD_COMFORT_JERK * DT_MDL
+    previous_comfort_accel = max(previous_accel, LEAD_COMFORT_ACCEL)
+    lead_accel = float(np.clip(target_accel, previous_comfort_accel - max_step, previous_comfort_accel + max_step))
+    return min(cruise_accel, lead_accel)

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

@@ -7,14 +7,17 @@ See the LICENSE.md file in the root directory for more details.
 
 import numpy as np
 
+from opendbc.car.interfaces import ACCEL_MAX
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.selfdrive.controls.lib.longitudinal_planner import (
   A_CRUISE_MAX_BP, A_CRUISE_MIN, J_CRUISE_VALS, get_cruise_accel, get_max_accel,
 )
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import MAX_T, STOP_DISTANCE
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
   AccelController, AccelProfile, CATCHUP_ACCEL_SCALE, CATCHUP_ERROR_BREAKPOINTS, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES,
+  LEAD_COMFORT_ACCEL, LEAD_COMFORT_JERK,
   TARGET_SPEED_APPROACH_FULL_SPEED, TARGET_SPEED_APPROACH_GAIN, TARGET_SPEED_APPROACH_MIN_SPEED, TARGET_SPEED_APPROACH_WINDOW,
   TARGET_SPEED_DEADBAND,
 )
@@ -51,38 +54,41 @@ class TestAccelController(OpenpilotTestCase):
         assert value <= previous[profile]
         previous[profile] = value
 
-  def test_normal_stays_below_stock(self):
-    controller = self.set_profile(AccelProfile.normal)
-    for speed in np.linspace(0.0, 55.0, 551):
-      assert controller.get_max_accel(speed) <= get_max_accel(speed) + 1e-12
+  def test_profiles_stay_within_openpilot_accel_max(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
+      controller = self.set_profile(profile)
+      for speed in np.linspace(0.0, 55.0, 551):
+        assert controller.get_max_accel(speed) <= ACCEL_MAX
 
-  def test_profiles_taper_below_stock_at_road_speed(self):
+  def test_profiles_do_not_exceed_stock_at_road_speed(self):
     for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
       controller = self.set_profile(profile)
       for speed in np.linspace(8.0, 40.0, 321):
-        assert controller.get_max_accel(speed) < get_max_accel(speed)
+        assert controller.get_max_accel(speed) <= get_max_accel(speed) + 1e-12
 
-  def test_launch_caps_stay_close_to_stock(self):
-    for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport):
-      controller = self.set_profile(profile)
-      for speed in np.linspace(0.0, 3.0, 61):
-        assert controller.get_max_accel(speed) >= 0.9 * get_max_accel(speed)
+  def test_profiles_have_material_separation(self):
+    controllers = [self.set_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)]
+    for speed in MAX_ACCEL_BREAKPOINTS:
+      eco, normal, sport = (controller.get_max_accel(speed) for controller in controllers)
+      assert normal - eco >= 0.1
+      assert sport - normal >= 0.1
+    for speed in MAX_ACCEL_BREAKPOINTS[1:-1]:
+      assert controllers[2].get_max_accel(speed) - controllers[0].get_max_accel(speed) >= 0.3 - 1e-12
 
   def test_eco_keeps_useful_road_speed_acceleration(self):
     controller = self.set_profile(AccelProfile.eco)
     for speed in np.linspace(8.0, 40.0, 321):
-      assert controller.get_max_accel(speed) >= 0.35 * get_max_accel(speed) - 1e-12
+      assert controller.get_max_accel(speed) >= 0.25 * get_max_accel(speed) - 1e-12
 
-  def test_profile_caps_drop_quickly_after_launch(self):
-    for values in MAX_ACCEL_PROFILES.values():
-      assert values[2] <= 0.7 * values[1]
-      assert values[3] <= 0.5 * values[1]
+  def test_comfort_profile_caps_taper_after_launch(self):
+    for profile in (AccelProfile.eco, AccelProfile.normal):
+      values = MAX_ACCEL_PROFILES[profile]
+      assert values[3] <= 0.55 * values[0]
 
-  def test_sport_stays_below_reported_route_acceleration(self):
+  def test_sport_uses_openpilot_accel_max_at_launch(self):
     controller = self.set_profile(AccelProfile.sport)
-    route_samples = ((8.1, 0.877), (12.0, 0.858), (15.5, 0.802), (18.8, 0.750), (21.7, 0.654))
-    for speed, recorded_accel in route_samples:
-      assert controller.get_max_accel(speed) <= 0.85 * recorded_accel
+    assert controller.get_max_accel(0.0) == ACCEL_MAX
+    assert all(controller.get_max_accel(speed) <= ACCEL_MAX for speed in np.linspace(0.0, 55.0, 551))
 
   def test_positive_catchup_limit_is_continuous_and_monotonic(self):
     controller = self.set_profile(AccelProfile.normal)
@@ -125,6 +131,21 @@ class TestAccelController(OpenpilotTestCase):
     assert np.all(np.diff(shaped_errors) >= 0.0)
     assert np.all(np.abs(shaped_errors) <= np.abs(errors) + 1e-12)
 
+  def test_lead_departure_accel_is_bounded_by_lead_speed_and_profile(self):
+    controller = self.set_profile(AccelProfile.eco)
+    assert controller.get_lead_departure_accel(0.0, 0.7, 0.1, 0.1) == 0.7
+    assert controller.get_lead_departure_accel(0.0, 2.0, 0.1, 0.1) == MAX_ACCEL_PROFILES[AccelProfile.eco][0]
+
+  def test_lead_departure_accel_preserves_non_acceleration_cases(self):
+    controller = self.set_profile(AccelProfile.normal)
+    assert controller.get_lead_departure_accel(0.0, 1.0, 0.1, -0.1) == -0.1
+    assert controller.get_lead_departure_accel(0.0, 1.0, -0.1, 0.1) == 0.1
+    assert controller.get_lead_departure_accel(1.0, 1.0, 0.1, 0.1) == 0.1
+    assert controller.get_lead_departure_accel(0.0, float("nan"), 0.1, 0.1) == 0.1
+
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    assert AccelController().get_lead_departure_accel(0.0, 1.0, 0.1, 0.1) == 0.1
+
   def test_catchup_limit_does_not_touch_launch(self):
     controller = self.set_profile(AccelProfile.sport)
 
@@ -193,6 +214,86 @@ class TestAccelController(OpenpilotTestCase):
     controller.update()
     assert not controller.is_enabled()
 
+  def test_closing_lead_prevents_acceleration_rebound(self):
+    controller = self.set_profile(AccelProfile.normal)
+    previous_accel = -0.647
+    samples = (
+      (17.33, 120.82, -16.35, -1.70, 0.50),
+      (17.30, 88.70, -14.95, -0.76, -0.332),
+      (17.18, 103.98, -17.15, -0.24, -0.992),
+    )
+    outputs = []
+    for v_ego, d_rel, v_rel, a_lead, mpc_accel in samples:
+      lead_accel = controller.get_lead_accel(v_ego, d_rel, v_rel, a_lead, 0.31, previous_accel, 1.45)
+      previous_accel = min(lead_accel, mpc_accel)
+      outputs.append(previous_accel)
+
+    assert np.allclose(outputs, [LEAD_COMFORT_ACCEL, LEAD_COMFORT_ACCEL, -0.992])
+    assert all(accel <= 0.0 for accel in outputs)
+
+  def test_lead_comfort_does_not_replay_hard_braking(self):
+    controller = self.set_profile(AccelProfile.normal)
+    output = controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, 0.5, -1.77, 1.45)
+    assert output == LEAD_COMFORT_ACCEL
+
+  def test_lead_approach_uses_existing_mpc_horizon(self):
+    controller = self.set_profile(AccelProfile.normal)
+    for t_follow in (1.25, 1.45, 1.75):
+      v_ego, v_rel, closing = 20.0, -2.0, 2.0
+      boundary = STOP_DISTANCE + t_follow * (v_ego + v_rel) + closing * MAX_T
+      assert controller.get_lead_accel(v_ego, boundary + 1e-3, v_rel, 0.0, 0.5, 0.2, t_follow) == 0.5
+      assert controller.get_lead_accel(v_ego, boundary - 1e-3, v_rel, 0.0, 0.5, 0.2, t_follow) < 0.2
+
+  def test_braking_lead_gently_removes_positive_acceleration(self):
+    controller = self.set_profile(AccelProfile.normal)
+    previous_accel = 0.544
+    outputs = []
+    for _ in range(40):
+      output = controller.get_lead_accel(30.56, 47.62, 5.5, -1.93, 0.544, previous_accel, 1.45)
+      outputs.append(output)
+      previous_accel = output
+
+    max_step = LEAD_COMFORT_JERK * DT_MDL
+    assert np.isclose(outputs[0], 0.544 - max_step)
+    assert np.all(np.diff(outputs) >= -max_step - 1e-12)
+    assert np.isclose(outputs[-1], LEAD_COMFORT_ACCEL)
+
+  def test_transient_lead_deceleration_is_one_small_step(self):
+    controller = self.set_profile(AccelProfile.normal)
+    first = controller.get_lead_accel(31.1, 51.3, 2.8, -2.5, 0.54, 0.4, 1.45)
+    second = controller.get_lead_accel(31.1, 52.0, 3.0, 0.0, 0.54, first, 1.45)
+
+    assert np.isclose(first, 0.4 - LEAD_COMFORT_JERK * DT_MDL)
+    assert second == 0.54
+
+  def test_non_closing_and_distant_leads_do_not_change_cruise(self):
+    controller = self.set_profile(AccelProfile.normal)
+    assert controller.get_lead_accel(30.0, 52.0, 0.0, 0.0, 0.1, 0.1, 1.45) == 0.1
+    assert controller.get_lead_accel(20.0, 200.0, -1.0, 0.0, 0.1, 0.1, 1.45) == 0.1
+
+  def test_accelerating_lead_returns_cruise(self):
+    controller = self.set_profile(AccelProfile.normal)
+    output = controller.get_lead_accel(20.0, 45.0, -1.0, 1.0, 0.3, -0.2, 1.45)
+    assert output == 0.3
+
+  def test_active_approach_cannot_rebound_from_brake_to_gas(self):
+    controller = self.set_profile(AccelProfile.normal)
+    output = controller.get_lead_accel(20.0, 30.0, -5.0, 1.0, 0.3, -0.2, 1.45)
+    assert -0.2 < output <= 0.0
+
+  def test_stock_lead_braking_always_wins(self):
+    controller = self.set_profile(AccelProfile.normal)
+    assert controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, -2.0, 0.2, 1.45) == -2.0
+
+  def test_invalid_disabled_and_launch_cases_match_cruise(self):
+    controller = self.set_profile(AccelProfile.normal)
+    assert controller.get_lead_accel(20.0, float("nan"), -8.0, -1.0, -0.5, 0.2, 1.45) == -0.5
+    assert controller.get_lead_accel(TARGET_SPEED_APPROACH_FULL_SPEED - 0.01, 20.0, -5.0, -1.0, 0.5, 0.0, 1.45) == 0.5
+
+    self.params.put_bool("AccelPersonalityEnabled", False, block=True)
+    controller = AccelController()
+    assert controller.get_lead_accel(20.0, 60.0, -8.0, -1.0, -0.5, 0.2, 1.45) == -0.5
+
 
 class TestPlannerIntegration(OpenpilotTestCase):
   def setUp(self):

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

@@ -6,9 +6,11 @@ See the LICENSE.md file in the root directory for more details.
 """
 
 from collections.abc import Callable
+from unittest import mock
 
 import numpy as np
 
+from openpilot.common.constants import CV
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
@@ -18,7 +20,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import Longi
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import (
   AccelController, AccelProfile, MAX_ACCEL_BREAKPOINTS, MAX_ACCEL_PROFILES, TARGET_SPEED_DEADBAND,
 )
-from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
+from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
 
 
 class CarParams:
@@ -57,7 +59,142 @@ def run_profile(profile: int, *, enabled: bool = True, speed: float = 0.0, v_cru
   return rows
 
 
+def run_vehicle_profile(profile: int, duration: float = 80.0):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", True, block=True)
+  params.put("AccelPersonality", profile, block=True)
+
+  plant = PlantSP(speed=0.0, actuator_model=PRIUS_TSS2_ROUTE_MODEL, run_long_control=True)
+  _set_mpc_acceleration(plant)
+  rows = []
+  while plant.current_time < duration:
+    result = plant.step(v_cruise=25.0)
+    rows.append((plant.current_time, result["speed"], result["a_target"], result["actuator_command"], result["acceleration"]))
+  return np.asarray(rows)
+
+
+def run_lead_comfort(*, comfort_enabled: bool, speed: float, distance_lead: float,
+                     lead_speed_fn: Callable[[float], float], duration: float = 12.0):
+  params = Params()
+  params.put_bool("DynamicExperimentalControl", False, block=True)
+  params.put_bool("AccelPersonalityEnabled", True, block=True)
+  params.put("AccelPersonality", AccelProfile.eco, block=True)
+
+  plant = PlantSP(lead_relevancy=True, speed=speed, distance_lead=distance_lead, only_radar=True)
+  plant.v_lead_prev = lead_speed_fn(0.0)
+  planner = plant.planner
+  raw_mpc, raw_cruise, adjusted_cruise, rows = [], [], [], []
+  mpc_failures = 0
+  original_arbitrate = planner.arbitrate_cruise_candidate
+  original_lead_accel = planner.accel_controller.get_lead_accel
+  original_mpc_reset = planner.mpc.reset
+
+  def record_mpc_reset(*args, **kwargs):
+    nonlocal mpc_failures
+    mpc_failures += int(planner.mpc.solution_status != 0)
+    return original_mpc_reset(*args, **kwargs)
+
+  def record_arbitration(sm, gated, ungated, mpc_accel, mpc_source, **kwargs):
+    raw_mpc.append(mpc_accel)
+    return original_arbitrate(sm, gated, ungated, mpc_accel, mpc_source, **kwargs)
+
+  def record_lead_accel(v_ego, d_rel, v_rel, a_lead, a_target, previous_accel, t_follow):
+    raw_cruise.append(a_target)
+    adjusted = original_lead_accel(v_ego, d_rel, v_rel, a_lead, a_target, previous_accel, t_follow) if comfort_enabled else a_target
+    adjusted_cruise.append(adjusted)
+    return adjusted
+
+  with (
+    mock.patch.object(planner.mpc, "reset", side_effect=record_mpc_reset),
+    mock.patch.object(planner, "arbitrate_cruise_candidate", side_effect=record_arbitration),
+    mock.patch.object(planner.accel_controller, "get_lead_accel", side_effect=record_lead_accel),
+  ):
+    while plant.current_time < duration:
+      output = plant.step(v_lead=lead_speed_fn(plant.current_time), v_cruise=30.0)
+      rows.append((
+        output["distance_lead"] - output["distance"], output["actuator_command"], output["a_target"], output["fcw"], planner.mpc.solution_status,
+      ))
+
+  data = np.asarray(rows, dtype=float)
+  commands = data[:, 1]
+  braking_frames = np.flatnonzero(commands < -0.05)
+  return {
+    "min_gap": float(np.min(data[:, 0])),
+    "min_command": float(np.min(commands)),
+    "min_jerk": float(np.min(np.diff(commands) / DT_MDL)),
+    "first_brake_frame": int(braking_frames[0]) if len(braking_frames) else len(commands),
+    "a_targets": data[:, 2],
+    "fcw": bool(np.any(data[:, 3])),
+    "mpc_failures": mpc_failures + int(data[-1, 4] != 0),
+    "raw_mpc": np.asarray(raw_mpc),
+    "raw_cruise": np.asarray(raw_cruise),
+    "adjusted_cruise": np.asarray(adjusted_cruise),
+  }
+
+
 class TestAccelControllerClosedLoop(OpenpilotTestCase):
+  def test_profiles_are_immediate_smooth_and_clearly_distinct(self):
+    traces = {profile: run_vehicle_profile(profile) for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)}
+
+    def crossing(trace, speed):
+      return float(trace[np.flatnonzero(trace[:, 1] >= speed)[0], 0])
+
+    time_to_20 = {profile: crossing(trace, 20.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    time_to_50 = {profile: crossing(trace, 50.0 * CV.MPH_TO_MS) for profile, trace in traces.items()}
+    first_motion = {profile: int(np.flatnonzero(trace[:, 1] > 0.01)[0]) for profile, trace in traces.items()}
+
+    self.assertEqual(len(set(first_motion.values())), 1)
+    self.assertTrue(all(trace[0, 2] > 0.0 and trace[1, 3] > 0.0 for trace in traces.values()))
+    self.assertGreaterEqual(time_to_20[AccelProfile.eco] - time_to_20[AccelProfile.normal], 2.0)
+    self.assertGreaterEqual(time_to_20[AccelProfile.normal] - time_to_20[AccelProfile.sport], 2.0)
+    self.assertGreaterEqual(time_to_50[AccelProfile.eco] - time_to_50[AccelProfile.normal], 10.0)
+    self.assertGreaterEqual(time_to_50[AccelProfile.normal] - time_to_50[AccelProfile.sport], 10.0)
+
+    for trace in traces.values():
+      command_jerk = np.abs(np.diff(trace[:, 3])) / DT_MDL
+      self.assertLessEqual(float(np.max(command_jerk)), PRIUS_TSS2_ROUTE_MODEL.command_rate_limit + 1e-9)
+
+      settled = np.flatnonzero(trace[:, 1] >= 25.0 - TARGET_SPEED_DEADBAND - 0.1)
+      self.assertGreater(len(settled), 0)
+      settled_trace = trace[settled[0]:]
+      self.assertGreaterEqual(float(np.min(settled_trace[:, 3])), -0.05)
+      self.assertGreaterEqual(float(np.min(np.diff(settled_trace[:, 1]))), -1e-8)
+      self.assertLessEqual(float(np.max(trace[:, 1])), 25.0 + 1e-9)
+      self.assertLessEqual(25.0 - float(trace[-1, 1]), TARGET_SPEED_DEADBAND + 0.02)
+
+  def test_lead_comfort_brakes_earlier_without_stronger_peak_for_slowing_lead(self):
+    def slowing_lead(t: float) -> float:
+      return float(np.clip(20.0 - 2.5 * max(t - 2.0, 0.0), 15.0, 20.0))
+
+    baseline = run_lead_comfort(comfort_enabled=False, speed=20.0, distance_lead=70.0, lead_speed_fn=slowing_lead)
+    comfort = run_lead_comfort(comfort_enabled=True, speed=20.0, distance_lead=70.0, lead_speed_fn=slowing_lead)
+
+    self.assertFalse(comfort["fcw"])
+    self.assertLessEqual(comfort["mpc_failures"], baseline["mpc_failures"])
+    self.assertGreater(comfort["min_gap"], 0.0)
+    self.assertGreaterEqual(comfort["min_gap"], baseline["min_gap"] - 0.1)
+    self.assertGreaterEqual(comfort["min_command"], baseline["min_command"] - 0.01)
+    self.assertGreaterEqual(comfort["min_jerk"], baseline["min_jerk"] - 0.05)
+    self.assertLessEqual(comfort["first_brake_frame"], baseline["first_brake_frame"])
+    self.assertTrue(np.all(comfort["a_targets"] <= comfort["raw_mpc"] + 1e-9))
+    self.assertTrue(np.all(comfort["adjusted_cruise"] <= comfort["raw_cruise"] + 1e-9))
+    self.assertTrue(np.any(comfort["adjusted_cruise"] < comfort["raw_cruise"] - 1e-3))
+
+  def test_lead_comfort_preserves_stopped_lead_safety(self):
+    def stopped_lead(_t: float) -> float:
+      return 0.0
+
+    baseline = run_lead_comfort(comfort_enabled=False, speed=20.0, distance_lead=90.0, lead_speed_fn=stopped_lead)
+    comfort = run_lead_comfort(comfort_enabled=True, speed=20.0, distance_lead=90.0, lead_speed_fn=stopped_lead)
+
+    self.assertFalse(comfort["fcw"])
+    self.assertLessEqual(comfort["mpc_failures"], baseline["mpc_failures"])
+    self.assertGreater(comfort["min_gap"], 0.0)
+    self.assertGreaterEqual(comfort["min_gap"], baseline["min_gap"] - 0.1)
+    self.assertGreaterEqual(comfort["min_command"], baseline["min_command"] - 0.01)
+    self.assertTrue(np.all(comfort["a_targets"] <= comfort["raw_mpc"] + 1e-9))
+    self.assertTrue(np.all(comfort["adjusted_cruise"] <= comfort["raw_cruise"] + 1e-9))
+
   def test_blended_positive_model_request_uses_profile_cruise_cap(self):
     params = Params()
     params.put_bool("DynamicExperimentalControl", False, block=True)
@@ -198,14 +335,10 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
 
     self.assertEqual(len(set(first_motion.values())), 1)
     self.assertTrue(all(frame == stock_first_motion for frame in first_motion.values()))
-    for rows in traces.values():
-      time_to_two = next(frame for frame, row in enumerate(rows) if row[0] >= 2.0) * DT_MDL
-      time_to_three = next(frame for frame, row in enumerate(rows) if row[0] >= 3.0) * DT_MDL
-      self.assertLessEqual(time_to_two, stock_time_to_two + DT_MDL)
-      self.assertLessEqual(time_to_three, stock_time_to_three + 0.1)
-    self.assertLessEqual(time_to_five[AccelProfile.sport], time_to_five[AccelProfile.normal])
-    self.assertLessEqual(time_to_five[AccelProfile.normal], time_to_five[AccelProfile.eco])
-    self.assertLessEqual(time_to_five[AccelProfile.eco], 1.25 * time_to_five[AccelProfile.normal])
+    self.assertLess(stock_time_to_two, next(frame for frame, row in enumerate(traces[AccelProfile.eco]) if row[0] >= 2.0) * DT_MDL)
+    self.assertLess(stock_time_to_three, next(frame for frame, row in enumerate(traces[AccelProfile.eco]) if row[0] >= 3.0) * DT_MDL)
+    self.assertGreaterEqual(time_to_five[AccelProfile.eco] - time_to_five[AccelProfile.normal], 0.5)
+    self.assertGreaterEqual(time_to_five[AccelProfile.normal] - time_to_five[AccelProfile.sport], 0.5)
 
   def test_road_speed_catchup_stays_useful(self):
     traces = {
@@ -213,8 +346,8 @@ class TestAccelControllerClosedLoop(OpenpilotTestCase):
       for profile in (AccelProfile.eco, AccelProfile.normal, AccelProfile.sport)
     }
     gains = {profile: rows[-1][0] - 20.0 for profile, rows in traces.items()}
-    self.assertGreaterEqual(gains[AccelProfile.sport], gains[AccelProfile.normal])
-    self.assertGreaterEqual(gains[AccelProfile.eco], 0.72 * gains[AccelProfile.normal])
+    self.assertGreaterEqual(gains[AccelProfile.normal], 1.25 * gains[AccelProfile.eco])
+    self.assertGreaterEqual(gains[AccelProfile.sport], 1.25 * gains[AccelProfile.normal])
 
   def test_catchup_settles_inside_deadband_without_oscillation(self):
     target_speed = 24.0

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

@@ -78,10 +78,11 @@ class LeadDepartureController:
       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 all(math.isfinite(value) for value in (lead.dRel, lead.vLeadK, lead.vRel, lead.aLeadK))
       and lead.dRel > 0.0
       and lead.vLeadK >= LEAD_DEPARTURE_MIN_SPEED
       and lead.vRel >= LEAD_DEPARTURE_MIN_SPEED
+      and lead.aLeadK >= 0.0
       and a_target >= 0.0
     )
     if not lead_valid:

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

@@ -11,6 +11,7 @@ from openpilot.cereal import messaging, custom, log
 from opendbc.car import structs
 from openpilot.common.constants import CV
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
+from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW
 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.e2e_alerts_helper import E2EAlertsHelper
@@ -77,15 +78,29 @@ class LongitudinalPlannerSP:
                                  e2e: bool, force_decel: bool) -> float:
     finite = all(math.isfinite(value) for value in (gated, ungated, mpc_accel))
     coast_gate_changed_source = gated < mpc_accel <= ungated
+    cruise_accel = gated
     if (finite and not allow_throttle and not e2e and not force_decel
         and self._has_valid_selected_lead(sm, mpc_source) and coast_gate_changed_source):
-      return ungated
+      cruise_accel = ungated
 
-    return gated
+    lead_valid = (self.accel_controller.is_enabled() and self.source == LongitudinalPlanSource.cruise
+                  and self._has_valid_selected_lead(sm, mpc_source))
+    if not lead_valid or force_decel:
+      return cruise_accel
 
-  def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> bool:
+    lead = sm['radarState'].leadOne if mpc_source == MpcPlanSource.lead0 else sm['radarState'].leadTwo
+    return self.accel_controller.get_lead_accel(
+      sm['carState'].vEgo, lead.dRel, lead.vRel, lead.aLeadK, cruise_accel, self.output_a_target,
+      get_T_FOLLOW(sm['selfdriveState'].personality),
+    )
+
+  def update_lead_departure(self, sm: messaging.SubMaster, a_target: float, should_stop: bool, reset: bool) -> tuple[float, 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)
+    should_stop = self.lead_departure_controller.update(sm, self.mpc.source, a_target, should_stop, reset, radar_valid)
+    if self.lead_departure_controller.active:
+      lead = sm['radarState'].leadOne if self.mpc.source == MpcPlanSource.lead0 else sm['radarState'].leadTwo
+      a_target = self.accel_controller.get_lead_departure_accel(sm['carState'].vEgo, lead.vLeadK, lead.aLeadK, a_target)
+    return a_target, should_stop
 
   def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
     CS = sm['carState']

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

@@ -9,9 +9,11 @@ from types import SimpleNamespace
 from unittest import mock
 
 from openpilot.cereal import log
+from openpilot.common.params import Params
 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.accel_controller.accel_controller import AccelProfile
 from openpilot.sunnypilot.selfdrive.controls.lib.lead_departure_controller import LEAD_DEPARTURE_MIN_SPEED, LeadDepartureController
 from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, PlantSP
 
@@ -19,8 +21,9 @@ from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRI
 MpcPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
 
 
-def make_lead(*, d_rel: float = 4.0, v_lead: float = 0.7, v_rel: float = 0.7, 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_lead(*, d_rel: float = 4.0, v_lead: float = 0.7, v_rel: float = 0.7, a_lead: float = 0.0,
+              present: bool = True, radar: bool = True, track_id: int = 7):
+  return SimpleNamespace(dRel=d_rel, vLeadK=v_lead, vRel=v_rel, aLeadK=a_lead, present=present, radar=radar, radarTrackId=track_id)
 
 
 def make_sm(
@@ -58,7 +61,12 @@ def activate(controller: LeadDepartureController):
   assert controller.active
 
 
-def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None):
+def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration: float, model_should_stop: bool | None = None,
+                    profile_enabled: bool = False):
+  params = Params()
+  params.put_bool("AccelPersonalityEnabled", profile_enabled, block=True)
+  params.put("AccelPersonality", AccelProfile.eco, block=True)
+
   def observe_lead(_t, _name, truth):
     truth.update(radar=True, radarTrackId=7)
     return truth
@@ -76,6 +84,7 @@ def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration:
     e2e=model_should_stop is not None,
     model_action_fn=model_action if model_should_stop is not None else None,
   )
+  plant.v_lead_prev = lead_speed(0.0)
   plant.planner.lead_departure_controller.enabled = controller_enabled
 
   original_update = plant.planner.update
@@ -104,7 +113,8 @@ def run_closed_loop(controller_enabled: bool, gap: float, lead_speed, duration:
       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'])
+        (t, result['speed'], result['distance'], result['distance_lead'] - result['distance'], result['actuator_command'], result['should_stop'], result['fcw'],
+         result['a_target'], plant.applied_actuator_command)
       )
       active.append(plant.planner.lead_departure_controller.active)
 
@@ -181,6 +191,7 @@ class TestLeadDepartureController(OpenpilotTestCase):
       ('track changed', make_sm(lead_one=make_lead(track_id=9))),
       ('lead too slow', make_sm(lead_one=make_lead(v_lead=LEAD_DEPARTURE_MIN_SPEED - 0.01))),
       ('relative speed too low', make_sm(lead_one=make_lead(v_rel=LEAD_DEPARTURE_MIN_SPEED - 0.01))),
+      ('lead braking', make_sm(lead_one=make_lead(a_lead=-0.01))),
       ('gas', make_sm(gas=True)),
       ('brake', make_sm(brake=True)),
       ('override', make_sm(override=True)),
@@ -255,7 +266,7 @@ class TestLeadDepartureController(OpenpilotTestCase):
     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)
+    rows, active, solver_resets = run_closed_loop(True, 4.0, lead_speed, 6.0, model_should_stop=True, profile_enabled=True)
 
     assert any(active)
     assert solver_resets == 0
@@ -270,3 +281,33 @@ class TestLeadDepartureController(OpenpilotTestCase):
     assert stock == controller
     assert not any(stock_active) and not any(controller_active)
     assert stock_resets == controller_resets == 0
+
+  def test_profile_breakaway_assist_moves_with_a_creeping_lead(self):
+    def lead_speed(t):
+      return min(1.1, 0.66 + 0.4 * t)
+
+    baseline, baseline_active, baseline_resets = run_closed_loop(True, 2.8, lead_speed, 8.0)
+    assisted, assisted_active, assisted_resets = run_closed_loop(True, 2.8, lead_speed, 8.0, profile_enabled=True)
+
+    baseline_motion = next((row[0] for row in baseline if row[1] > 0.01), float("inf"))
+    assisted_motion = next(row[0] for row in assisted if row[1] > 0.01)
+    assert any(baseline_active) and any(assisted_active)
+    assert baseline_resets == assisted_resets == 0
+    assert assisted_motion <= 1.0
+    assert baseline_motion - assisted_motion >= 1.0
+    assert min(row[3] for row in assisted) >= min(row[3] for row in baseline) - 1e-9
+    assert not any(row[6] for row in baseline + assisted)
+    assert max(abs(right[8] - left[8]) for left, right in zip(assisted, assisted[1:], strict=False)) <= 4.0 * DT_MDL + 1e-9
+
+  def test_departure_assist_yields_when_the_lead_brakes(self):
+    def lead_speed(t):
+      return 0.8 if t < 0.8 else max(0.0, 0.8 - 4.0 * (t - 0.8))
+
+    rows, active, solver_resets = run_closed_loop(True, 2.8, lead_speed, 5.0, profile_enabled=True)
+
+    assert any(active)
+    assert solver_resets == 0
+    assert min(row[3] for row in rows) > 2.0
+    assert not any(row[6] for row in rows)
+    assert rows[-1][1] == 0.0
+    assert min(row[7] for row in rows if row[0] >= 0.8) < 0.0

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

@@ -8,34 +8,47 @@ See the LICENSE.md file in the root directory for more details.
 from types import SimpleNamespace
 from typing import cast
 
-from openpilot.cereal import messaging, log
+from openpilot.cereal import messaging, custom, log
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.test import OpenpilotTestCase
 from openpilot.selfdrive.controls.lib.longitudinal_planner import get_coast_accel, get_cruise_accel
+from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController
 from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP
 
 PlanSource = log.LongitudinalPlan.LongitudinalPlanSource
+SunnyPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
 
 
 class FakeSubMaster(dict):
   def __init__(self, lead_one: bool = True, lead_two: bool = False, *, valid: bool = True, alive: bool = True):
+    lead = {'dRel': 120.8, 'vRel': -16.4, 'aLeadK': -1.7}
     super().__init__(radarState=SimpleNamespace(
-      leadOne=SimpleNamespace(present=lead_one),
-      leadTwo=SimpleNamespace(present=lead_two),
-    ))
+      leadOne=SimpleNamespace(present=lead_one, **lead),
+      leadTwo=SimpleNamespace(present=lead_two, **lead),
+    ), carState=SimpleNamespace(vEgo=17.3),
+      selfdriveState=SimpleNamespace(personality=log.LongitudinalPersonality.standard))
     self.valid = {'radarState': valid}
     self.alive = {'radarState': alive}
 
 
 def arbitrate(sm, mpc_accel=0.19, gated_cruise=-0.26, ungated_cruise=0.5, source=PlanSource.lead0,
-              allow_throttle=False, e2e=False, force_decel=False):
-  planner = object.__new__(LongitudinalPlannerSP)
+              allow_throttle=False, e2e=False, force_decel=False, lead_comfort=False, previous_accel=0.0):
+  planner = make_planner(lead_comfort, previous_accel)
   return planner.arbitrate_cruise_candidate(
     cast(messaging.SubMaster, sm), gated_cruise, ungated_cruise, mpc_accel, source,
     allow_throttle=allow_throttle, e2e=e2e, force_decel=force_decel,
   )
 
 
+def make_planner(lead_comfort: bool = False, previous_accel: float = 0.0):
+  planner = object.__new__(LongitudinalPlannerSP)
+  planner.accel_controller = AccelController()
+  planner.accel_controller._enabled = lead_comfort
+  planner.source = SunnyPlanSource.cruise
+  planner.output_a_target = previous_accel
+  return planner
+
+
 class TestThrottleIntentArbitration(OpenpilotTestCase):
   def test_coast_gate_cannot_add_braking_a_valid_lead_does_not_need(self):
     selected = arbitrate(FakeSubMaster())
@@ -53,6 +66,22 @@ class TestThrottleIntentArbitration(OpenpilotTestCase):
     self.assertEqual(selected, -0.26)
     self.assertEqual(min(selected, -0.6), -0.6)
 
+  def test_lead_comfort_is_only_an_extra_braking_candidate(self):
+    for sm, source in ((FakeSubMaster(), PlanSource.lead0), (FakeSubMaster(False, True), PlanSource.lead1)):
+      cruise = arbitrate(sm, mpc_accel=0.5, gated_cruise=0.5, ungated_cruise=0.5, source=source,
+                         allow_throttle=True, lead_comfort=True, previous_accel=0.2)
+      self.assertLess(cruise, 0.2)
+      self.assertEqual(min(cruise, -1.2), -1.2)
+
+  def test_lead_comfort_requires_a_live_selected_lead(self):
+    for sm, source in ((FakeSubMaster(valid=False), PlanSource.lead0),
+                       (FakeSubMaster(alive=False), PlanSource.lead0),
+                       (FakeSubMaster(lead_one=False), PlanSource.lead0),
+                       (FakeSubMaster(lead_one=True, lead_two=False), PlanSource.lead1)):
+      with self.subTest(source=source):
+        self.assertEqual(arbitrate(sm, gated_cruise=0.5, ungated_cruise=0.5, source=source,
+                                   allow_throttle=True, lead_comfort=True), 0.5)
+
   def test_policy_requires_live_valid_selected_lead(self):
     cases = (
       FakeSubMaster(valid=False),
@@ -90,7 +119,7 @@ class TestThrottleIntentArbitration(OpenpilotTestCase):
       steerRatio = 15.0
       wheelbase = 2.7
 
-    planner = object.__new__(LongitudinalPlannerSP)
+    planner = make_planner()
     sm = FakeSubMaster()
     v_ego = 6.9
     v_cruise = 30.0