Fix several bugs in waypoint computations
Yet more bug fixing
Merge branch 'master' bug fixes into devel