Fixing rotation on the waypoints

Ready for review!