MavlinkTest update

Updated MavlinkTest.ino to include all project code to date