First draft of mavlink command

Needs to be tested