[mavlink] Basic implementation (needs cleanup)