Update MavlinkTest.ino

added ultrasonics