remove testing code add mavlink command call