Waypoints drawing only in debug correction. Reopen #221