Merge pull request #4128 from DonLakeFlyer/Mav2Payload

Correct address of payload for Mav2

src/ui/MAVLinkDecoder.cc

@@ -69,6 +69,11 @@ void MAVLinkDecoder::run()
 
 void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t message)
 {
+    if (message.msgid >= cMessageIds) {
+        // No support for messag ids above 255
+        return;
+    }
+
     Q_UNUSED(link);
     memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
 
@@ -91,7 +96,8 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
 
         // See if first value is a time value and if it is, use that as the arrival time for this data.
         uint8_t fieldid = 0;
-        uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
+        uint8_t* m = (uint8_t*)&((mavlink_message_t*)(receivedMessages+msgid))->payload64[0];
+
         if (QString(msgInfo->fields[fieldid].name) == QString("time_boot_ms") && msgInfo->fields[fieldid].type == MAVLINK_TYPE_UINT32_T)
         {
             time = *((quint32*)(m+msgInfo->fields[fieldid].wire_offset));
@@ -220,7 +226,7 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     if (messageFilter.contains(msgid)) return;
     QString fieldName(msgInfo->fields[fieldid].name);
     QString fieldType;
-    uint8_t* m = ((uint8_t*)(receivedMessages+msgid))+8;
+    uint8_t* m = (uint8_t*)&((mavlink_message_t*)(receivedMessages+msgid))->payload64[0];
     QString name("%1.%2");
     QString unit("");
 

src/ui/MAVLinkDecoder.h

@@ -25,15 +25,16 @@ protected:
     /** @brief Shift a timestamp in Unix time if necessary */
     quint64 getUnixTimeFromMs(int systemID, quint64 time);
 
-    mavlink_message_t receivedMessages[256]; ///< Available / known messages
-    QMap<uint16_t, bool> messageFilter;               ///< Message/field names not to emit
-    QMap<uint16_t, bool> textMessageFilter;           ///< Message/field names not to emit in text mode
-    int componentID[256];                             ///< Multi component detection
-    bool componentMulti[256];                         ///< Multi components detected
-    quint64 onboardTimeOffset[256];                   ///< Offset of onboard time from Unix epoch (of the receiving GCS)
-    qint64 onboardToGCSUnixTimeOffsetAndDelay[256];   ///< Offset of onboard time and GCS Unix time
-    quint64 firstOnboardTime[256];                    ///< First seen onboard time
-
+    static const size_t cMessageIds = 256;
+
+    mavlink_message_t receivedMessages[cMessageIds];        ///< Available / known messages
+    QMap<uint16_t, bool> messageFilter;                     ///< Message/field names not to emit
+    QMap<uint16_t, bool> textMessageFilter;                 ///< Message/field names not to emit in text mode
+    int componentID[cMessageIds];                           ///< Multi component detection
+    bool componentMulti[cMessageIds];                       ///< Multi components detected
+    quint64 onboardTimeOffset[cMessageIds];                 ///< Offset of onboard time from Unix epoch (of the receiving GCS)
+    qint64 onboardToGCSUnixTimeOffsetAndDelay[cMessageIds]; ///< Offset of onboard time and GCS Unix time
+    quint64 firstOnboardTime[cMessageIds];                  ///< First seen onboard time
 };
 
 #endif // MAVLINKDECODER_H

src/ui/QGCMAVLinkInspector.cc

@@ -441,8 +441,7 @@ void QGCMAVLinkInspector::updateField(mavlink_message_t* msg, const mavlink_mess
         return;
     }
 
-    uint8_t* m = ((uint8_t*)uasMessage)+8;
-
+    uint8_t* m = (uint8_t*)&uasMessage->payload64[0];
 
     switch (msgInfo->fields[fieldid].type)
     {