diff --git a/mavros/include/mavros/mavros_uas.hpp b/mavros/include/mavros/mavros_uas.hpp
index 45e1c90e4..06b1a83d8 100644
--- a/mavros/include/mavros/mavros_uas.hpp
+++ b/mavros/include/mavros/mavros_uas.hpp
@@ -161,7 +161,7 @@ class Data
   inline double geoid_to_ellipsoid_height(const T lla)
   {
     if (egm96_5) {
-      return int(GeographicLib::Geoid::GEOIDTOELLIPSOID) * (*egm96_5)(lla->latitude, lla->longitude);
+      return egm96_5->ConvertHeight(lla->latitude, lla->longitude, 0.0, GeographicLib::Geoid::GEOIDTOELLIPSOID);
     } else {
       return 0.0;
     }
@@ -181,7 +181,7 @@ class Data
   inline double ellipsoid_to_geoid_height(const T lla)
   {
     if (egm96_5) {
-      return int(GeographicLib::Geoid::ELLIPSOIDTOGEOID) * (*egm96_5)(lla->latitude, lla->longitude);
+      return egm96_5->ConvertHeight(lla->latitude, lla->longitude, 0.0, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
     } else {
       return 0.0;
     }