controls: smooth CEM to ACC handoff

Blend MPC back in over the final 5 mph of the CEM limit while preserving strong E2E braking. Slew positive acceleration and freeze the integrator during the experimental-mode exit so the handoff stays smooth. Keep the existing lead and confidence gates on the release path.

Original PR: #115 by @1454

selfdrive/controls/lib/longcontrol.py

@@ -134,9 +134,6 @@ class LongControl:
   def update_mpc_mode(self, experimental_mode):
     new_mode = 'blended' if experimental_mode else 'acc'
 
-    if self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc':
-      self.mode_transition_timer = 0.0
-
     if new_mode != self.current_mode:
       self.prev_mode = self.current_mode
       self.transitioning = True
@@ -320,6 +317,9 @@ class LongControl:
       freeze_integrator = self.vehicle_tuning.get_integrator_freeze(
         self.last_output_accel, a_target, error, CS.vEgo, accel_limits,
       )
+      leaving_experimental = self.transitioning and self.prev_mode == 'blended' and self.current_mode == 'acc'
+      if leaving_experimental:
+        freeze_integrator = True
       raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward,
                                          freeze_integrator=freeze_integrator)
       raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS)
@@ -337,7 +337,13 @@ class LongControl:
         raw_output_accel, CS.vEgo, should_stop, leads,
       )
 
-      if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
+      if leaving_experimental:
+        if raw_output_accel > self.last_output_accel:
+          progress = min(1.0, self.mode_transition_timer / max(self.mode_transition_duration, 1e-3))
+          output_accel = self.last_output_accel + (raw_output_accel - self.last_output_accel) * progress
+        else:
+          output_accel = raw_output_accel
+      elif self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended':
         if raw_output_accel < 0 and raw_output_accel < self.last_output_accel:
           progress = min(1.0, self.mode_transition_timer / self.mode_transition_duration)
           # Soften transition at low urgency, but keep sharp for high decel

selfdrive/controls/lib/longitudinal_planner.py

@@ -291,6 +291,10 @@ EXPERIMENTAL_RELEASE_ACCEL_MAX_LATERAL_OFFSET = 1.5
 EXPERIMENTAL_RELEASE_ACCEL_MIN_HEADWAY_MARGIN = 0.0
 EXPERIMENTAL_RELEASE_ACCEL_MIN_DELTA_A = 0.12
 EXPERIMENTAL_RELEASE_ACCEL_STEP = 0.06
+# Last few mph below CESpeed/CESpeedLead: mix MPC back in so experimental
+# cannot crawl into the breakpoint and then snap to ACC.
+EXPERIMENTAL_SPEED_HANDOFF_BAND = 5.0 * CV.MPH_TO_MS
+EXPERIMENTAL_HANDOFF_KEEP_E2E_BRAKE = -0.15
 MATCHED_FOLLOW_TRANSITION_MIN_SPEED = 20.0
 TRACKED_VISION_MODEL_FLOOR_MIN_SPEED = 10.0
 TRACKED_VISION_MODEL_FLOOR_MIN_MODEL_PROB = 0.95
@@ -1776,6 +1780,38 @@ class LongitudinalPlanner:
       self.experimental_release_accel_until = 0.0
     self.prev_experimental_mode = bool(experimental_mode)
 
+  def get_experimental_speed_handoff_weight(self, v_ego, experimental_mode, following_lead,
+                                            starpilot_toggles, hold_experimental):
+    if not experimental_mode or hold_experimental:
+      return 0.0
+
+    limit_key = "conditional_limit_lead" if following_lead else "conditional_limit"
+    limit = float(getattr(starpilot_toggles, limit_key, 0.0) or 0.0)
+    if limit <= 1.0:
+      return 0.0
+
+    return float(np.clip(
+      (float(v_ego) - (limit - EXPERIMENTAL_SPEED_HANDOFF_BAND)) / EXPERIMENTAL_SPEED_HANDOFF_BAND,
+      0.0,
+      1.0,
+    ))
+
+  @staticmethod
+  def is_cem_following_lead(tracking_lead, d_rel, t_follow, v_ego):
+    # Same inputs as StarPilotFollowing.following_lead / CEM: published
+    # trackingLead and tFollow, plus leadOne.dRel inside 2*t_follow*v_ego.
+    return bool(tracking_lead and float(d_rel) < (float(t_follow) * 2.0) * float(v_ego))
+
+  @staticmethod
+  def apply_experimental_speed_handoff(output_a_target, output_a_target_mpc, output_a_target_e2e, speed_handoff):
+    if speed_handoff <= 0.0:
+      return output_a_target
+    # Keep a real E2E brake. Only mix MPC back in when experimental is crawling
+    # or matching ACC, not when it is already asking for more deceleration.
+    if output_a_target_e2e < min(output_a_target_mpc, EXPERIMENTAL_HANDOFF_KEEP_E2E_BRAKE):
+      return output_a_target
+    return (1.0 - speed_handoff) * output_a_target + speed_handoff * output_a_target_mpc
+
   def get_experimental_release_accel_target(self, lead, v_ego, base_t_follow,
                                             prev_output_a_target, output_a_target,
                                             release_active):
@@ -2404,6 +2440,26 @@ class LongitudinalPlanner:
       else:
         output_a_target = min(output_a_target_mpc, output_a_target_e2e)
         output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+        cem_following_lead = self.is_cem_following_lead(
+          tracking_lead,
+          self.lead_one.dRel,
+          sm['starpilotPlan'].tFollow,
+          scene_v_ego,
+        )
+        speed_handoff = self.get_experimental_speed_handoff_weight(
+          scene_v_ego,
+          experimental_mode,
+          cem_following_lead,
+          starpilot_toggles,
+          bool(
+            output_should_stop_e2e or
+            getattr(sm['starpilotPlan'], 'forcingStop', False) or
+            getattr(sm['starpilotPlan'], 'redLight', False)
+          ),
+        )
+        output_a_target = self.apply_experimental_speed_handoff(
+          output_a_target, output_a_target_mpc, output_a_target_e2e, speed_handoff,
+        )
     else:
       output_a_target, output_should_stop = get_accel_from_plan(
         self.v_desired_trajectory, self.a_desired_trajectory,

selfdrive/controls/tests/test_longcontrol.py

@@ -8,6 +8,7 @@ import openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes as vehicle_tun
 from opendbc.car.gm.values import CAR, GMFlags
 from opendbc.car.subaru.values import CAR as SUBARU_CAR
 from opendbc.car.toyota.values import CAR as TOYOTA_CAR
+from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.longcontrol import (
   LongControl,
   LongCtrlState,
@@ -1531,3 +1532,50 @@ def test_gm_stock_truck_update_gradually_releases_stale_brake_integral():
   )
 
   assert -0.66 < output_accel < -0.44
+
+
+def test_leaving_experimental_slews_positive_accel():
+  CP = make_longcontrol_cp()
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.pid
+  lc.experimental_mode = True
+  lc.current_mode = "blended"
+  lc.prev_mode = "acc"
+  lc.last_output_accel = 0.05
+  CS = car.CarState.new_message(vEgo=20.0, aEgo=0.05, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  lc.experimental_mode = False
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=1.5,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(),
+  )
+
+  assert lc.current_mode == "acc"
+  assert lc.transitioning
+  assert output_accel > 0.05
+  assert output_accel < 0.25
+
+
+def test_leaving_experimental_does_not_reset_mode_transition_timer():
+  CP = make_longcontrol_cp()
+  lc = LongControl(CP)
+  lc.current_mode = "blended"
+
+  lc.update_mpc_mode(False)
+  first = lc.mode_transition_timer
+  lc.update_mpc_mode(False)
+
+  assert lc.current_mode == "acc"
+  assert lc.transitioning
+  assert first == pytest.approx(DT_CTRL)
+  assert lc.mode_transition_timer == pytest.approx(2.0 * DT_CTRL)
+
+  for _ in range(int(lc.mode_transition_duration / DT_CTRL)):
+    lc.update_mpc_mode(False)
+
+  assert not lc.transitioning

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -7,6 +7,7 @@ import numpy as np
 import pytest
 
 from cereal import log
+from openpilot.common.constants import CV
 from opendbc.car.honda.interface import CarInterface
 from opendbc.car.honda.values import CAR
 from opendbc.car.gm.values import CAR as GM_CAR, GMFlags
@@ -653,6 +654,8 @@ def make_toggles(model_version: str = "v11", radar_takeoffs: bool = False):
     model_version=model_version,
     vEgoStopping=0.5,
     radar_takeoffs=radar_takeoffs,
+    conditional_limit=0.0,
+    conditional_limit_lead=0.0,
   )
 
 
@@ -3694,6 +3697,47 @@ def test_experimental_release_accel_transition_damps_moving_lead_handoff():
   assert target == pytest.approx(0.09)
 
 
+def test_experimental_speed_handoff_weight_ramps_into_cespeed():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP)
+  toggles = make_toggles()
+  limit = 35.0 * CV.MPH_TO_MS
+  band = longitudinal_planner_module.EXPERIMENTAL_SPEED_HANDOFF_BAND
+  toggles.conditional_limit = limit
+
+  assert planner.get_experimental_speed_handoff_weight(limit - band - 1.0, True, False, toggles, False) == 0.0
+  assert planner.get_experimental_speed_handoff_weight(limit, True, False, toggles, False) == pytest.approx(1.0)
+  assert planner.get_experimental_speed_handoff_weight(limit - 0.5 * band, True, False, toggles, False) == pytest.approx(0.5)
+  assert planner.get_experimental_speed_handoff_weight(limit, True, False, toggles, True) == 0.0
+  assert planner.get_experimental_speed_handoff_weight(limit, False, False, toggles, False) == 0.0
+
+
+def test_experimental_speed_handoff_uses_lead_limit_when_following():
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP)
+  toggles = make_toggles()
+  toggles.conditional_limit = 55.0 * CV.MPH_TO_MS
+  toggles.conditional_limit_lead = 20.0 * CV.MPH_TO_MS
+
+  assert planner.get_experimental_speed_handoff_weight(20.0 * CV.MPH_TO_MS, True, True, toggles, False) == pytest.approx(1.0)
+  assert planner.get_experimental_speed_handoff_weight(20.0 * CV.MPH_TO_MS, True, False, toggles, False) == 0.0
+
+
+def test_experimental_speed_handoff_keeps_stronger_e2e_brake():
+  kept = LongitudinalPlanner.apply_experimental_speed_handoff(-0.50, 0.20, -0.50, 1.0)
+  blended = LongitudinalPlanner.apply_experimental_speed_handoff(0.02, 0.40, 0.02, 0.5)
+
+  assert kept == pytest.approx(-0.50)
+  assert blended == pytest.approx(0.21)
+
+
+def test_experimental_speed_handoff_following_lead_matches_cem_window():
+  # Distant radar-active lead is not CEM following_lead.
+  assert LongitudinalPlanner.is_cem_following_lead(True, 40.0, 1.5, 20.0)
+  assert not LongitudinalPlanner.is_cem_following_lead(True, 80.0, 1.5, 20.0)
+  assert not LongitudinalPlanner.is_cem_following_lead(False, 10.0, 1.5, 20.0)
+
+
 def test_experimental_release_accel_transition_does_not_mask_stopped_lead():
   v_ego = 23.96
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)

starpilot/controls/lib/starpilot_following.py

@@ -101,7 +101,6 @@ class StarPilotFollowing:
     self.danger_jerk = self.base_danger_jerk
     self.speed_jerk = self.base_speed_jerk
 
-    self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
     self.slower_lead = False
 
     if self.starpilot_planner.starpilot_weather.weather_id != 0:
@@ -109,6 +108,9 @@ class StarPilotFollowing:
 
     self.update_lane_change_gap(long_control_active, v_ego, sm, starpilot_toggles)
 
+    # After t_follow adjustments so CEM and the published tFollow use the same window.
+    self.following_lead = self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.dRel < (self.t_follow * 2) * v_ego
+
     self.disable_throttle = False
     if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status:
       lead_distance = self.starpilot_planner.lead_one.dRel