Refactoring

Cleaning some Mavlink Code to an external class.