Revert "Refactoring, Navmode & preparations for Waypoints"

Reverts zlsa/atc#54