Refactoring

Cleaning up some old TODOs and extracting mavlink code from followMe