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

Working on better simulation implementation, fixed a bug where the port handle was not fully released