handle wgs84 altitude correctly

also adds AMSL output in ft
fix unused variable warning
Merge pull request #837 from mavlink/altitude

handle wgs84 altitude correctly, AMSL also displayed in ft