'Working' waypoints code

Tested this with Michael today, and it was successfully flying to two
waypoints and just bouncing back. I still need to check whether it will
work when it was not oriented North originally and with the reduced
waypoint radius.