Added support for multiple components emitting the same message

src/comm/MAVLinkSimulationLink.cc

@@ -358,19 +358,19 @@ void MAVLinkSimulationLink::mainloop()
     if (rate10hzCounter == 1000 / rate / 9) {
         rate10hzCounter = 1;
 
-        float lastX = x;
-        float lastY = y;
-        float lastZ = z;
-        float hackDt = 0.1f; // 100 ms
+        double lastX = x;
+        double lastY = y;
+        double lastZ = z;
+        double hackDt = 0.1f; // 100 ms
 
         // Move X Position
         x = 12.0*sin(((double)circleCounter)/200.0);
         y = 5.0*cos(((double)circleCounter)/200.0);
         z = 1.8 + 1.2*sin(((double)circleCounter)/200.0);
 
-        float xSpeed = (x - lastX)/hackDt;
-        float ySpeed = (y - lastY)/hackDt;
-        float zSpeed = (z - lastZ)/hackDt;
+        double xSpeed = (x - lastX)/hackDt;
+        double ySpeed = (y - lastY)/hackDt;
+        double zSpeed = (z - lastZ)/hackDt;
 
 
 
@@ -414,12 +414,12 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
-//        // GLOBAL POSITION VEHICLE 2
-//        mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
-//        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
-//        //add data into datastream
-//        memcpy(stream+streampointer,buffer, bufferlength);
-//        streampointer += bufferlength;
+        // GLOBAL POSITION VEHICLE 2
+        mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
 
 //        // ATTITUDE VEHICLE 2
 //        mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
@@ -427,7 +427,7 @@ void MAVLinkSimulationLink::mainloop()
 
 
 //        // GLOBAL POSITION VEHICLE 3
-//        mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
+//        mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
 //        bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
 //        //add data into datastream
 //        memcpy(stream+streampointer,buffer, bufferlength);
@@ -565,6 +565,15 @@ void MAVLinkSimulationLink::mainloop()
         memcpy(stream+streampointer,buffer, bufferlength);
         streampointer += bufferlength;
 
+        // Pack message and get size of encoded byte string
+        messageSize = mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
+        // Allocate buffer with packet data
+        bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
+        //qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
+        //add data into datastream
+        memcpy(stream+streampointer,buffer, bufferlength);
+        streampointer += bufferlength;
+
 
         // Send controller states
 

src/uas/UAS.cc

@@ -82,6 +82,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
     isGlobalPositionKnown(false),
     systemIsArmed(false)
 {
+    for (unsigned int i = 0; i<255;++i)
+    {
+        componentID[i] = -1;
+        componentMulti[i] = false;
+    }
+
     color = UASInterface::getNextColor();
     setBatterySpecs(QString("9V,9.5V,12.6V"));
     connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
@@ -210,6 +216,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
         QString uasState;
         QString stateDescription;
 
+        bool multiComponentSourceDetected = false;
+        bool wrongComponent = false;
+
+        // Store component ID
+        if (componentID[message.msgid] == -1)
+        {
+            componentID[message.msgid] = message.compid;
+        }
+        else
+        {
+            // Got this message already
+            if (componentID[message.msgid] != message.compid)
+            {
+                componentMulti[message.msgid] = true;
+                wrongComponent = true;
+            }
+        }
+
+        if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true;
+
+
         switch (message.msgid)
         {
         case MAVLINK_MSG_ID_HEARTBEAT:
@@ -336,6 +363,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_SYS_STATUS:
         {
+                if (multiComponentSourceDetected && message.compid != MAV_COMP_ID_IMU_2)
+                {
+                    break;
+                }
                 mavlink_sys_status_t state;
                 mavlink_msg_sys_status_decode(&message, &state);
 
@@ -371,6 +402,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_ATTITUDE:
             {
+                if (wrongComponent) break;
+
                 mavlink_attitude_t attitude;
                 mavlink_msg_attitude_decode(&message, &attitude);
                 quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
@@ -499,7 +532,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
                     if (!isnan(alt) && !isinf(alt))
                     {
                         alt = 0;
-                        emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
+                        //emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
                     }
                     // FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
                     // Smaller than threshold and not NaN

src/uas/UAS.h

@@ -544,6 +544,8 @@ protected:
     quint64 getUnixTimeFromMs(quint64 time);
     /** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
     quint64 getUnixReferenceTime(quint64 time);
+    int componentID[256];
+    bool componentMulti[256];
 
 protected slots:
     /** @brief Write settings to disk */

src/ui/MAVLinkDecoder.cc

@@ -7,6 +7,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
     mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
     memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);
     memset(receivedMessages, 0, sizeof(mavlink_message_t)*256);
+    for (unsigned int i = 0; i<255;++i)
+    {
+        componentID[i] = -1;
+        componentMulti[i] = false;
+    }
 
     // Fill filter
     messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false);
@@ -69,6 +74,26 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
 
 void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
 {
+    bool multiComponentSourceDetected = false;
+    bool wrongComponent = false;
+
+    // Store component ID
+    if (componentID[msg->msgid] == -1)
+    {
+        componentID[msg->msgid] = msg->compid;
+    }
+    else
+    {
+        // Got this message already
+        if (componentID[msg->msgid] != msg->compid)
+        {
+            componentMulti[msg->msgid] = true;
+            wrongComponent = true;
+        }
+    }
+
+    if (componentMulti[msg->msgid] == true) multiComponentSourceDetected = true;
+
     // Add field tree widget item
     uint8_t msgid = msg->msgid;
     if (messageFilter.contains(msgid)) return;
@@ -78,6 +103,8 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
     QString name("%1.%2");
     QString unit("");
     name = name.arg(messageInfo[msgid].name, fieldName);
+    if (multiComponentSourceDetected) name.prepend(QString("C%1:").arg(msg->compid));
+    name.prepend(QString("M%1:").arg(msg->sysid));
     switch (messageInfo[msgid].fields[fieldid].type)
     {
     case MAVLINK_TYPE_CHAR:

src/ui/MAVLinkDecoder.h

@@ -30,6 +30,8 @@ protected:
     mavlink_message_info_t messageInfo[256]; ///< Message information
     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
 
 };
 

src/ui/QGCMAVLinkInspector.cc

@@ -82,7 +82,7 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
         {
             msgHz = 1;
         }
-        float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1);
+        float newHz = 0.05f*msgHz+0.95f*messagesHz.value(message.msgid, 1);
         messagesHz.insert(message.msgid, newHz);
     }