Update MavlinkTest.ino

Fixed DoMath() function to properly calculate lat and lon