fix waypoint switch bug
merge