diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index 05d6726dc2..5e4ef9dafd 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -1033,9 +1033,10 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
         sbufWriteU8(dst, gpsSol.numSat);
         sbufWriteU32(dst, gpsSol.llh.lat);
         sbufWriteU32(dst, gpsSol.llh.lon);
-        sbufWriteU16(dst, gpsSol.llh.alt);
+        sbufWriteU16(dst, MIN(gpsSol.llh.alt,65535));
         sbufWriteU16(dst, gpsSol.groundSpeed);
         sbufWriteU16(dst, gpsSol.groundCourse);
+        sbufWriteU32(dst, gpsSol.llh.alt);
         break;
 
     case MSP_COMP_GPS:
@@ -1899,8 +1900,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.numSat = sbufReadU8(src);
         gpsSol.llh.lat = sbufReadU32(src);
         gpsSol.llh.lon = sbufReadU32(src);
-        gpsSol.llh.alt = sbufReadU32(src);
+        gpsSol.llh.alt = sbufReadU16(src);
         gpsSol.groundSpeed = sbufReadU16(src);
+        if (sbufBytesRemaining(src) >= 4) {
+            gpsSol.llh.alt = sbufReadU32(src);
+        }
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
 #endif // USE_GPS