Test code implementation to Mavlink