fix loss data

MavLinkNode/mavlinknode.cpp

@@ -276,6 +276,7 @@ QByteArray MavLinkNode::readbuff(quint32 src)
 
 void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
 {
+    static int count = 0;
     mavlink_message_t msg;
     mavlink_status_t status;
 
@@ -283,10 +284,25 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
     {
         if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
         {
-            if(msg.compid != Current_CompID)             //过滤地面站发过来的数据
+            //qDebug() << "fifo parse:" << count << msg.msgid;
+
+            count++;
+
+            if(msg.compid == Current_CompID)             //过滤地面站发过来的数据
             {
-                emit recievemsg(msg);                    //将信息广播出去
+                //不能使用这种方法过滤，不正确
+                //qDebug() << msg.msgid << "msg come from groundstation";
+            }
+
+
+            //if(msg.compid != Current_CompID)             //过滤地面站发过来的数据
+            {
+
                 MAVLinkRcv_Handler(msg);                 //接收完一帧数据并处理
+                //QThread::sleep(5);
+                //qDebug() << "msg.sysid" <<msg.sysid << "msg.compid" << msg.compid << "fifo parse:" << count << msg.msgid;
+
+                emit recievemsg(msg);                    //将信息广播出去
             }
         }
         else
@@ -350,7 +366,7 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
 void MavLinkNode::MAVLinkRcv_Handler(mavlink_message_t msg)
 {
         //用于给参数添加设备，便于读取
-        //qDebug() << "msg.sysid" <<msg.sysid << "msg.compid" << msg.compid;
+
         CheckVehicle(msg.sysid,msg.compid);
 
         vehicle.sysid = msg.sysid;