Revert "Refactoring, Navmode & preparations for Waypoints"
Merge pull request #55 from zlsa/revert-54-waypoints

Revert "Refactoring, Navmode & preparations for Waypoints"