协同dlink和mavlink

dlink/dlink.cpp

@@ -7,11 +7,16 @@
 
 DLink::DLink(QObject *parent) : QObject(parent)
 {
+     qDebug() << "Dlink " <<  QThread::currentThreadId();
+
+     mavlinknode = new MavLinkNode();
+
 
 }
 DLink::~DLink()
 {
-
+    mavlinknode->stop();
+    delete mavlinknode;
 }
 
 
@@ -20,8 +25,11 @@ int DLink::SendMessageTo(uint8_t, uint8_t *msg, uint16_t len)
     //mavlinknode::bytes_num_out += len;
     if (DLink::Clientsock)
     {
-        DLink::Clientsock->writeDatagram((const char *)msg, len,
-                                   DLink::clientSockets.last().addr, DLink::clientSockets.last().port);
+        foreach(Node node,clientSockets)
+        {
+            DLink::Clientsock->writeDatagram((const char *)msg, len,
+                                       node.addr, node.port);
+        }
     }
 
     if (DLink::serialPort)
@@ -41,7 +49,7 @@ void DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity p
         serialPort->close();
         delete serialPort;
     }
-    serialPort = new QSerialPort(port, this);
+    serialPort = new QSerialPort(port,this);
 
     if (serialPort->open(QIODevice::ReadWrite))
     {
@@ -50,7 +58,11 @@ void DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity p
         serialPort->setDataBits(QSerialPort::Data8);
         serialPort->setStopBits(QSerialPort::OneStop);
 
-        connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagrams()));
+        mavlinknode->start();
+
+        connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagramsSerialPort()));
+
+        qDebug() << "Serial Port Open Success";
     }
     else
     {
@@ -74,8 +86,7 @@ void DLink::stopPort()
 
 
 void DLink::setupClient(const QHostAddress &remote_addr, int remote_port, int local_port)
-{
-
+{    
     Clientsock = new QUdpSocket(this);
     if (Clientsock->bind(QHostAddress::Any, local_port))
     {
@@ -87,11 +98,40 @@ void DLink::setupClient(const QHostAddress &remote_addr, int remote_port, int lo
             node.sock = Clientsock;
             node.addr = remote_addr;
             node.port = remote_port;
+
             clientSockets.append(node);
-            qDebug() << "sock open:"
+            qDebug() << "UdpSocket open:"
                      << remote_addr
                      << local_port
                      << remote_port;
+
+            mavlinknode->start();
+
+
+            //想办法只让这个UDP打开一次
+            /*
+            foreach(Node nodex,clientSockets)
+            {
+                qDebug() << "in";
+                //如果不存在就添加进去
+                if((nodex.sock == node.sock)&&
+                   (nodex.addr == node.addr)&&
+                   (nodex.port == node.port))
+                {
+                    continue;
+                }
+                else {
+                    clientSockets.append(node);
+                    qDebug() << "sock open:"
+                             << remote_addr
+                             << local_port
+                             << remote_port;
+                }
+            }
+            */
+
+
+
         }
         else
         {
@@ -112,40 +152,35 @@ void DLink::readPendingDatagramsSerialPort(void)
 {
     QByteArray datagram = serialPort->readAll();
 
-    //static mavlink_message_t msg;
-    //static mavlink_status_t  status;
-    uint8_t ret = '\0';
-
-
-    SendMessageTo(ret,(uint8_t *)datagram.data(),datagram.size());
+    mavlinknode->setbuff(SourceType::s_port,datagram);
 
+/*
     for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
     {
-        /*
+
         ret = mavlink_parse_char(MAVLINK_COMM_0,*i,&msg,&status);
         if(MAVLINK_FRAMING_OK == ret)
         {
             MAVLinkRcv_Handler(msg);                 //接收完一帧数据并处理
         }
-        */
+
     }
+*/
+
 }
 
 void DLink::readPendingDatagramsClient(void)
 {
-
-
-
+   //轮番查询
     while (Clientsock->hasPendingDatagrams()) {
         QByteArray datagram;
         datagram.resize(Clientsock->pendingDatagramSize());
         Clientsock->readDatagram(datagram.data(), datagram.size());
 
+        mavlinknode->setbuff(SourceType::c_sock,datagram);
+
          //SendMessageTo(ret,(uint8_t *)datagram.data(),datagram.size());
 
-
-        //qDebug() << "Client Read All" << datagram;
-
         /*
         for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
         {
@@ -159,3 +194,14 @@ void DLink::readPendingDatagramsClient(void)
     }
 
 }
+
+
+
+
+
+
+
+
+
+
+