Fully working waypoint interface, thoroughly tested. Active waypoint setting...

Fully working waypoint interface, thoroughly tested. Active waypoint setting has to be debugged in the simulator, but works fine on real hardware