Mavlink stack usage improvements

Ready for testing / review.