diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 0c43dbd6e07039c4f96c598fc85b42f50f3c8600..69ee72af4d437d2e56a7eee212d545e22eb9ac3b 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -182,6 +182,14 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
         unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status);
 
         if (decodeState == 1) {
+#ifdef MAVLINK_MESSAGE_LENGTHS
+	    const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
+	    if (message.msgid >= sizeof(message_lengths) ||
+		message.len != message_lengths[message.msgid]) {
+                    qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")";
+		    continue;
+	    }
+#endif
             // Log data
             if (m_loggingEnabled && m_logfile) {
                 const int len = MAVLINK_MAX_PACKET_LEN+sizeof(quint64);