保存程序

MavLinkNode/mavlinknode.cpp

@@ -18,7 +18,13 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
     //初始化buff
     initbuff();
 
+    //初始化ID
+    int Current_sysID = 0xF1;
+    int Current_CompID = MAV_COMP_ID_MISSIONPLANNER;
+
+
     Mission = new MissionProcess();
+    Mission->setGCSID(Current_sysID,Current_CompID);
     connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
 
@@ -31,6 +37,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 
 
     Parameter = new ParameterProcess();
+    Parameter->setGCSID(Current_sysID,Current_CompID);
     connect(Parameter,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
 
@@ -39,6 +46,7 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 
 
     Commander = new commandprocess();
+    Commander->setGCSID(Current_sysID,Current_CompID);
     connect(Commander,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
             this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
 
@@ -242,16 +250,13 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
     mavlink_message_t msg;
     mavlink_status_t status;
 
-    //qDebug() << "msg parsed";
-
     for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
     {
         if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
         {
-
             emit recievemsg(msg);                    //将信息广播出去
             MAVLinkRcv_Handler(msg);                 //接收完一帧数据并处理
-            //qDebug() << "msg parsed";
+
         }
         else
         {
@@ -300,10 +305,10 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
                  //qDebug() << "MAVLINK_PARSE_STATE_GOT_CRC1";
             }break;
             case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:{
-                 //qDebug() << "MAVLINK_PARSE_STATE_GOT_BAD_CRC1";
+                 qDebug() << "MAVLINK_PARSE_STATE_GOT_BAD_CRC1";
             }break;
             case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:{
-                 //qDebug() << "MAVLINK_PARSE_STATE_SIGNATURE_WAIT";
+                 qDebug() << "MAVLINK_PARSE_STATE_SIGNATURE_WAIT";
             }break;
             }
         }