Copter: use target yaw instead of current yaw for close waypoints