Modification TakeWP2

On ne prends plus le waypoint mais utilise M dans tous les cas.
Plus efficace et évite certains blocage.