mawei/openpilot
1
0
Fork 0
mirror of https://jihulab.com/mr-one/openpilot.git synced 2026-10-27 21:13:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

AEM: smooth mode transition with linear blend over 2s

Browse Source
This commit is contained in:
mawei
2026-06-29 20:17:47 +08:00
parent b6a1a9910b
commit 5bae19ade8
2 changed files with 28 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
dragonpilot/selfdrive/controls/lib/aem.py
+18 -4
View File
@@ -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
selfdrive/controls/lib/longitudinal_planner.py
+10 -3
View File
@@ -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: