Waypoint fixes

This pull request contains the fixes from #416 but without param1 and param2 switched.

It has been tested in HIL using this branch:
https://github.com/julianoes/Firmware/tree/fw_autoland_att_tecs_navigator_termination_controlgroups

I suggest to hold this back until the above branch is in master.