diff --git a/src/comm/MAVLinkXMLParser.cc b/src/comm/MAVLinkXMLParser.cc
index 5aeb84338118faec5d3ec7a15bd32d093689d9c1..2ef5ab873b14beba809038a7ae17b66d8202b5a1 100644
--- a/src/comm/MAVLinkXMLParser.cc
+++ b/src/comm/MAVLinkXMLParser.cc
@@ -90,6 +90,16 @@ bool MAVLinkXMLParser::generate()
 
     int mavlinkVersion = 0;
 
+    // we need to gather the message lengths across multiple includes,
+    // which we can do via detecting recursion
+    static unsigned message_lengths[256];
+    static int highest_message_id;
+    static int recursion_level;
+
+    if (recursion_level == 0) {
+        highest_message_id = 0;
+        memset(message_lengths, 0, sizeof(message_lengths));
+    }
 
 
     // Start main header
@@ -138,7 +148,9 @@ bool MAVLinkXMLParser::generate()
                                 MAVLinkXMLParser includeParser(incFilePath, topLevelOutputDirName, this);
                                 connect(&includeParser, SIGNAL(parseState(QString)), this, SIGNAL(parseState(QString)));
                                 // Generate and write
+                                recursion_level++;
                                 includeParser.generate();
+                                recursion_level--;
                                 mainHeader += "\n#include \"../" + pureIncludeFileName + "/" + pureIncludeFileName + ".h\"\n";
 
 
@@ -340,7 +352,7 @@ bool MAVLinkXMLParser::generate()
                                         QString decodeLines;
                                         QString sendArguments;
                                         QString commentLines;
-
+                                        unsigned message_length = 0;
 
 
                                         // Get the message fields
@@ -437,6 +449,40 @@ bool MAVLinkXMLParser::generate()
                                                 }
 
 
+                                                // message length calculation
+                                                unsigned element_multiplier = 1;
+                                                unsigned element_length = 0;
+                                                const struct {
+                                                    const char *prefix;
+                                                    unsigned length;
+                                                } length_map[] = {
+                                                        { "array",  1 },
+                                                        { "char",   1 },
+                                                        { "uint8",  1 },
+                                                        { "int8",   1 },
+                                                        { "uint16", 2 },
+                                                        { "int16",  2 },
+                                                        { "uint32", 4 },
+                                                        { "int32",  4 },
+                                                        { "uint64", 8 },
+                                                        { "int64",  8 },
+                                                        { "float",  4 },
+                                                        { "double", 8 },
+                                                };
+                                                if (fieldType.contains("[")) {
+                                                    element_multiplier = fieldType.split("[").at(1).split("]").first().toInt();
+                                                }
+                                                for (unsigned i=0; i<sizeof(length_map)/sizeof(length_map[0]); i++) {
+                                                    if (fieldType.startsWith(length_map[i].prefix)) {
+                                                        element_length = length_map[i].length * element_multiplier;
+                                                        break;
+                                                    }
+                                                }
+                                                if (element_length == 0) {
+                                                    emit parseState(tr("<font color=\"red\">ERROR: Unable to calculate length for %2 near line %1\nAbort.</font>").arg(QString::number(e.lineNumber()), fieldType));
+                                                }
+                                                message_length += element_length;
+
                                                 //
                                                 //                                                QString unpackingCode;
 
@@ -489,6 +535,11 @@ bool MAVLinkXMLParser::generate()
                                             f = f.nextSibling();
                                         }
 
+                                        if (messageId > highest_message_id) {
+                                            highest_message_id = messageId;
+                                        }
+                                        message_lengths[messageId] = message_length;
+
                                         cStruct = cStruct.arg(cStructName, cStructLines);
                                         lcmStructDefs.append("\n").append(cStruct).append("\n");
                                         pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines);
@@ -540,6 +591,15 @@ bool MAVLinkXMLParser::generate()
         mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first);
     }
 
+    mainHeader += "\n\n// MESSAGE LENGTHS\n\n";
+    mainHeader += "#undef MAVLINK_MESSAGE_LENGTHS\n";
+    mainHeader += "#define MAVLINK_MESSAGE_LENGTHS { ";
+    for (int i=0; i<highest_message_id; i++) {
+        mainHeader += QString::number(message_lengths[i]);
+        if (i < highest_message_id-1) mainHeader += ", ";
+    }
+    mainHeader += " }\n\n";
+
     mainHeader += "#ifdef __cplusplus\n}\n#endif\n";
     mainHeader += "#endif";
     // Newline to make compiler happy