Restructured waypoint transmission via Mavlink.  Add extra command funcionality.