Update MavlinkTest.ino

The Do_MATH() function was incorrect.  Fixed calculations of lat and longitude to assume cartesian coordinates due to short distance between waypoints.