Fixed mavlink bug