Fixed waypoint mode bugs

Fixed selection persisting after changing from waypoint mode, and
selecting waypoints outside of waypoint mode.