diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index a9eb00e0f5..44d9df9f98 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1859,8 +1859,12 @@ void AP_Periph_FW::can_gps_update(void)
         }
         pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;
         pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;
-        pkt.height_ellipsoid_mm = loc.alt * 10;
         pkt.height_msl_mm = loc.alt * 10;
+        pkt.height_ellipsoid_mm = loc.alt * 10;
+        float undulation;
+        if (gps.get_undulation(undulation)) {
+            pkt.height_ellipsoid_mm -= undulation*1000;
+        }
         for (uint8_t i=0; i<3; i++) {
             // the canard dsdl compiler doesn't understand float16
             pkt.ned_velocity[i] = vel[i];