Updated mavlinkt est

Updated MavlinkTest.ino to work