Properly fix waypoints