Fixed the last_5_waypoints queue I think