From 5bae19ade8a493e6f9e77c807da2e934b0c57af4 Mon Sep 17 00:00:00 2001
From: mawei <mawei@mr-one.cn>
Date: Mon, 29 Jun 2026 20:17:47 +0800
Subject: [PATCH] AEM: smooth mode transition with linear blend over 2s

---
 dragonpilot/selfdrive/controls/lib/aem.py     | 22 +++++++++++++++----
 .../controls/lib/longitudinal_planner.py      | 13 ++++++++---
 2 files changed, 28 insertions(+), 7 deletions(-)

diff --git a/dragonpilot/selfdrive/controls/lib/aem.py b/dragonpilot/selfdrive/controls/lib/aem.py
index 0b4d12263..d98aa4506 100644
--- a/dragonpilot/selfdrive/controls/lib/aem.py
+++ b/dragonpilot/selfdrive/controls/lib/aem.py
@@ -25,10 +25,16 @@ THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR I
 THROTTLE_ACC_PROB     = 0.6  # gasPressProb >= this -> model wants throttle -> ACC
 THROTTLE_BLENDED_PROB = 0.4  # gasPressProb <= this -> model easing off     -> BLENDED
 
+# Blending factor for smooth mode transitions.
+# At 20 Hz, 40 frames ≈ 2 seconds to fully blend.
+BLEND_RAMP_FRAMES = 40
+
 
 class AEM:
   def __init__(self):
     self._throttle_prob = 1.0
+    self._blend = 0.0            # 0 = full blended, 1 = full acc
+    self._prev_mode = 'blended'
 
   def update_states(self, model_msg, radar_msg, v_ego):
     # Probability the model wants to be on throttle (same signal the planner uses
@@ -37,7 +43,15 @@ class AEM:
     self._throttle_prob = probs[1] if len(probs) > 1 else 1.0
 
   def get_mode(self, mode):
-    if mode == 'blended':  # blended-first: borrow ACC only when clearly wanting throttle
-      return 'acc' if self._throttle_prob >= THROTTLE_ACC_PROB else 'blended'
-    # acc-first: borrow BLENDED only when clearly easing off
-    return 'blended' if self._throttle_prob <= THROTTLE_BLENDED_PROB else 'acc'
+    target = 'acc' if self._throttle_prob >= THROTTLE_ACC_PROB else 'blended'
+    if mode != 'blended':
+      # acc-first mode logic: swap when easing off
+      target = 'blended' if self._throttle_prob <= THROTTLE_BLENDED_PROB else 'acc'
+
+    # Ramp blend factor toward target
+    if target == 'acc':
+      self._blend = min(self._blend + 1.0 / BLEND_RAMP_FRAMES, 1.0)
+    else:
+      self._blend = max(self._blend - 1.0 / BLEND_RAMP_FRAMES, 0.0)
+
+    return self._blend, target
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 3f72aade3..dd116185c 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -177,9 +177,16 @@ class LongitudinalPlanner:
     mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc'
     if dp_flags & DPFlags.AEM:
       self.aem.update_states(model_msg=sm['modelV2'], radar_msg=sm['radarState'], v_ego=sm['carState'].vEgo)
-      mode = self.aem.get_mode(mode)
-
-    if mode == 'blended':
+      aem_blend, aem_mode = self.aem.get_mode(mode)
+      # Smooth blend between e2e (blended) and mpc (acc) using linear interpolation
+      a_target_e2e = min(output_a_target_e2e, output_a_target_mpc)
+      output_a_target = a_target_e2e * (1.0 - aem_blend) + output_a_target_mpc * aem_blend
+      self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
+      if aem_mode == 'blended' and a_target_e2e < output_a_target_mpc:
+        self.mpc.source = LongitudinalPlanSource.e2e
+      else:
+        self.mpc.source = LongitudinalPlanSource.mpc
+    elif mode == 'blended':
       output_a_target = min(output_a_target_e2e, output_a_target_mpc)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
       if output_a_target < output_a_target_mpc: