Adding waypoints at current boat position