From 87d277ff7c3433802fc07767c47e36d11c72933d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 16 Apr 2011 14:17:39 +1000
Subject: [PATCH] mavlink: report bad message lengths to the console

---
 src/comm/MAVLinkProtocol.cc | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 0c43dbd6e..69ee72af4 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);
-- 
2.22.0