Refactoring: Extracting mavlink code from FollowMe
Cleaning up some old TODOs that are already fixed, and also fixing smaller TODOs.
Merge pull request #173 from arthurbenemann/Refactoring

Refactoring