set current waypoint on mavlink
set new current waypoint