AC_WPNav: set_wp_destination to use current target

Previously we used the projected stopping point if the vehicle had not
reached the destination but this could lead to large jumps in target
position