Fix waypoint bug