'Set wp' and 'upload new waypoints and set' functions added, working