diff --git a/dragonpilot/selfdrive/controls/lib/aem.py b/dragonpilot/selfdrive/controls/lib/aem.py index d98aa4506..0b4d12263 100644 --- a/dragonpilot/selfdrive/controls/lib/aem.py +++ b/dragonpilot/selfdrive/controls/lib/aem.py @@ -25,16 +25,10 @@ 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 @@ -43,15 +37,7 @@ class AEM: self._throttle_prob = probs[1] if len(probs) > 1 else 1.0 def get_mode(self, mode): - 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 + 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' diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index dd116185c..3f72aade3 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -177,16 +177,9 @@ 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) - 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': + mode = self.aem.get_mode(mode) + + if 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: