mavlink: report bad message lengths to the console

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);