Set waypoints via pymavlink

Hi @YuryArnaut,

Since missions are not officially supported in ArduSub, that may result in unstable behavior.
But if you are still willing to test it, you can base your work under pymavlink waypoint code: