调整线程模板

MavLinkNode/Terminal.cpp

@@ -73,6 +73,8 @@ void terminal::process()//线程函数
             case RecieveMode  : ReadStateMachine();break;
             case TransmitMode : WriteStateMachine();break;
          }
+
+         QThread::yieldCurrentThread();
     }
 }
 

MavLinkNode/ThreadTemplet.cpp

@@ -1,18 +1,28 @@
 #include "ThreadTemplet.h"
 
-ThreadTemplet::ThreadTemplet(QObject *parent) : QThread(parent)
+ThreadTemplet::ThreadTemplet(QObject *parent) : QObject(parent)
 {
     running_flag = false;
-    moveToThread(this);
+    thread = new QThread();
+    this->moveToThread(thread);
+    connect(thread, &QThread::started, this, &ThreadTemplet::process);
+
+    connect(thread, &QThread::finished, this, &ThreadTemplet::deleteLater);
+
+    setRunFrq(10);//10hz
+    initbuff();
 
     start();
-
 }
 
 ThreadTemplet::~ThreadTemplet()
 {
-    quit();
-    wait();
+    stop();
+
+    if(thread)
+    {
+        delete thread;
+    }
 }
 
 void ThreadTemplet::setRunFrq(uint32_t frq)
@@ -20,29 +30,162 @@ void ThreadTemplet::setRunFrq(uint32_t frq)
     if((frq != 0)||(frq <= 1000))
     {
         running_frq = frq;
-        qDebug() << "set Mavlink Node running frquency:" <<frq <<"Hz";
+        qDebug() << "set running frquency:" <<frq <<"Hz";
     }
 }
 
+void ThreadTemplet::start()
+{
+    if(!thread->isRunning())
+    {
+        running_flag = true;
+        thread->start();
+        qDebug() << "thread start" << running_flag;
+
+    }
+    else
+    {
+        qDebug() << "thread has started";
+    }
+}
+
+void ThreadTemplet::stop()
+{
+    if(thread)
+    {
+        if(!thread->isFinished())
+        {
+            running_flag = false;
+
+            disconnect(thread, nullptr, nullptr, nullptr);
+
+            thread->quit();
+            thread->wait();
+        }
+        else
+        {
+            qDebug() << "thread is not running";
+        }
+    }
+}
 
 void ThreadTemplet::process()//线程函数
 {
-    uint8_t count = 0;
-    while (running_flag)
+    while (isActive())
     {
-         count ++;
          QThread::msleep(1000/running_frq);
 
+
+
+         QThread::yieldCurrentThread();
+    }
+
+}
+
+void ThreadTemplet::setID(int m_sysid,int m_compid)
+{
+    sysid  = (uint8_t)m_sysid;
+    compid = (uint8_t)m_compid;
+}
+
+
+void ThreadTemplet::Send(mavlink_message_t msg)
+{
+    if(isActive())
+    {
+        uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
+        uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
+        emit SendMessageTo(0,buff, len);//使用信号和槽
     }
 }
 
-void ThreadTemplet::run(void)
-{
-    QThread::msleep(1000/running_frq);
 
-    exec();
+void ThreadTemplet::initbuff(void)
+{
+    client_buff.max_size = 10 * 1024 *1024;//10M
+    client_buff.buff[0].clear();
+    client_buff.buff[1].clear();
+    client_buff.select = 0;
+
+    serial_buff.max_size = 10 * 1024 *1024;
+    serial_buff.buff[0].clear();
+    serial_buff.buff[1].clear();
+    serial_buff.select = 0;
+}
+
+void ThreadTemplet::setbuff(quint32 src,QByteArray data)
+{
+    switch (src) {
+    default:
+    case SourceType::c_sock:
+           //当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
+           if(client_buff.buff[client_buff.select].size() >= client_buff.max_size)
+           {
+               client_buff.buff[client_buff.select].clear();
+               qDebug() << "client_buff.buff " << client_buff.select <<" OVERFLOW";
+           }
+           client_buff.buff[client_buff.select].append(data);
+
+        break;
+    case SourceType::s_port:
+        //当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
+           if(serial_buff.buff[serial_buff.select].size() >= serial_buff.max_size)
+           {
+               serial_buff.buff[serial_buff.select].clear();
+               qDebug() << "serial_buff.buff " << serial_buff.select <<" OVERFLOW";
+           }
+           serial_buff.buff[serial_buff.select].append(data);
+        break;
+    }
+}
+
+QByteArray ThreadTemplet::readbuff(quint32 src)
+{
+    QByteArray datagram;
+
+    switch (src) {
+    default:
+    case SourceType::c_sock:
+           if(client_buff.select == 0)
+           {
+               datagram.clear();
+               datagram.append(client_buff.buff[1]);
+               //清除这个未选择的buff
+               client_buff.buff[1].clear();
+               //读取完成，可以往这个内存里面写数了
+               client_buff.select = 1;
+           }
+           else if(client_buff.select == 1)
+           {
+               datagram.clear();
+               datagram.append(client_buff.buff[0]);
+               //清除这个未选择的buff
+               client_buff.buff[0].clear();
+               //读取完成，可以往这个内存里面写数了
+               client_buff.select = 0;
+           }
+        break;
+    case SourceType::s_port:
+           if(serial_buff.select == 0)
+           {
+               datagram.clear();
+               datagram.append(serial_buff.buff[1]);
+               //读取完成，可以往这个内存里面写数了
+               serial_buff.buff[1].clear();
+               serial_buff.select = 1;
+           }
+           else if(serial_buff.select == 1)
+           {
+               datagram.clear();
+               datagram.append(serial_buff.buff[0]);
+               //读取完成，可以往这个内存里面写数了
+               serial_buff.buff[0].clear();
+               serial_buff.select = 0;
+           }
+        break;
+    }
+    return datagram;
 }
 
 
 
-

MavLinkNode/ThreadTemplet.h

@@ -5,11 +5,12 @@
 #include "QThread"
 #include "QFile"
 #include "QDebug"
+#include "mavlink.h"
 
 
 #ifdef QtMavlinkNode
 #include <mavlinknodeglobal.h>
-class MAVLINKNODESHARED_EXPORT ThreadTemplet : public QThread {
+class MAVLINKNODESHARED_EXPORT ThreadTemplet : public QObject {
 #else
 class ThreadTemplet : public QThread
 {
@@ -23,7 +24,10 @@ public:
        QByteArray buff[2];
     }_buffdef;
 
-
+    enum SourceType{
+       c_sock = 0,
+       s_port = 1
+    };
 
     explicit ThreadTemplet(QObject *parent = nullptr);
     ~ThreadTemplet();
@@ -32,24 +36,54 @@ public:
 
 signals:
 
+    void SendMessageTo(quint8 ch, quint8 *data,quint16 len);
+
+    void showMessage(const QString &message,int TimeOut = 0);
 
 public slots:
     //线程对外接口
-    virtual void setRunFrq(uint32_t frq);
-    virtual void run();
-    virtual bool isActive(void)
+
+    void setRunFrq(uint32_t frq);
+    void start();
+    void stop();
+    bool isActive(void)
     {
-        return running_flag;
+        bool flag = running_flag;
+
+/*
+        if(thread)
+        {
+            flag = thread->isRunning();
+            //qDebug() << "running" << flag <<  thread->isRunning();
+        }
+        */
+
+
+
+        return flag;
     }
 
+    quint32 frq(void)
+    {
+        return running_frq;
+    }
 
 
     //缓存对外接口
 
+    void setID(int m_sysid,int m_compid);
+
+    virtual void Send(mavlink_message_t msg);
+
+    void initbuff(void);
+
+    void setbuff(quint32 src,QByteArray data);
+
+    QByteArray readbuff(quint32 src);
 
 private slots:
    //线程私有接口
-   void process();
+   virtual void process();
 
    //缓存私有接口
 
@@ -57,7 +91,20 @@ private slots:
 
 protected:
     bool running_flag = false;
-    quint32 running_frq = 50;//50Hz
+    quint32 running_frq = 10;//50Hz
+
+    QThread *thread = nullptr;
+
+    _buffdef serial_buff;
+    _buffdef client_buff;
+
+
+    int GCS_SysID = 0xF1;
+    int GCS_CompID = MAV_COMP_ID_MISSIONPLANNER;
+
+    //目标id
+    uint8_t sysid;
+    uint8_t compid;
 
 };
 #endif // THREADTEMPLET_H

MavLinkNode/commandprocess.cpp

@@ -77,6 +77,8 @@ void commandprocess::process()//线程函数
             case TransmitMode : WriteStateMachine();break;
          }
 
+         QThread::yieldCurrentThread();
+
     }
 }
 

MavLinkNode/mavlinknode.cpp

@@ -132,12 +132,9 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 
     RC = new rcprocess();
     RC->setGCSID(Current_sysID,Current_CompID);
-   /* connect(RC,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
-            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
-            */
 
     connect(RC,&rcprocess::SendMessageTo,
-            this,&MavLinkNode::SendMessageTo);
+            this,&MavLinkNode::SendMessageTo,Qt::DirectConnection);
 
 
     connect(this,SIGNAL(setCurrentID(int,int)),
@@ -149,9 +146,9 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 
     rtk = new rtkprocess();
     rtk->setGCSID(Current_sysID,Current_CompID);
-    connect(rtk,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
-            this,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),Qt::DirectConnection);
 
+    connect(rtk,&rtkprocess::SendMessageTo,
+            this,&MavLinkNode::SendMessageTo,Qt::DirectConnection);
 
     connect(this,SIGNAL(setCurrentID(int,int)),
             rtk,SLOT(setID(int,int)),Qt::DirectConnection);
@@ -681,6 +678,8 @@ void MavLinkNode::process()//线程函数
               Mavlinkparse(SourceType::s_port,datagram);
          }
 
+         QThread::yieldCurrentThread();
+
     }
 }
 

MavLinkNode/missionprocess.cpp

@@ -84,6 +84,9 @@ void MissionProcess::process()//线程函数
                  }
              }break;
          }
+
+         QThread::yieldCurrentThread();
+
         //qDebug() << count;
     }
 }

MavLinkNode/parameterprocess.cpp

@@ -73,6 +73,8 @@ void ParameterProcess::process()//线程函数
             case TransmitMode : WriteStateMachine();break;
          }
 
+         QThread::yieldCurrentThread();
+
     }
     //退出线程
     //disconnect(thread, nullptr, nullptr, nullptr);

MavLinkNode/rcprocess.cpp

@@ -150,6 +150,8 @@ void rcprocess::process()//线程函数
              lastTime = currentTimestamp;
              update15Hz();
          }
+
+         QThread::yieldCurrentThread();
     }
 
 

MavLinkNode/replay.cpp

@@ -249,6 +249,8 @@ void Replay::process()//线程函数
         {
             QThread::msleep(1000);//如果没有播放，那么每秒钟检测一下是否要播放
         }
+
+        QThread::yieldCurrentThread();
         //mutex.unlock();
     }
     //退出线程

MavLinkNode/rtkprocess.cpp

@@ -2,29 +2,20 @@
 
 rtkprocess::rtkprocess(QObject *parent) : ThreadTemplet(parent)
 {
-    //初始化buff
-
-    setRunFrq(10);
-    initbuff();
-
+    setRunFrq(10);//10hz
 }
 
 rtkprocess::~rtkprocess()
 {
-    //stop();
-
+    stop();
 }
 
-void rtkprocess::setID(int m_sysid,int m_compid)
-{
-    sysid  = (uint8_t)m_sysid;
-    compid = (uint8_t)m_compid;
-}
+
 
 
 void rtkprocess::Send(mavlink_message_t msg)
 {
-    if(running_flag)
+    if(isActive())
     {
         uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
         uint16_t len = mavlink_msg_to_send_buffer(buff, &msg);
@@ -32,7 +23,7 @@ void rtkprocess::Send(mavlink_message_t msg)
     }
 }
 
-//这里一直在解码，一直检查双缓冲里面是否有数据，有就解码，没有就休息
+
 
 
 void rtkprocess::process()//线程函数
@@ -41,22 +32,21 @@ void rtkprocess::process()//线程函数
     QByteArray datagram = nullptr;
 
     uint8_t count = 0;
-    while (running_flag)
+
+    while (true)
     {
          count ++;
-         QThread::msleep(1000/running_frq);
+         QThread::msleep(1000/frq());
 
           //解码从串口来的
           datagram.clear();
           datagram = readbuff(SourceType::s_port);//每次全部读取
           if(datagram.size() > 0)
           {
-               parse(SourceType::s_port,datagram);
-
-
-
+               rtkrawdata.append(datagram);
           }
 
+          //qDebug() << "rtk run";
 
           quint64 currentTimestamp = (quint64)QDateTime::currentMSecsSinceEpoch();
 
@@ -66,104 +56,16 @@ void rtkprocess::process()//线程函数
               update15Hz();
           }
 
+          QThread::yieldCurrentThread();
+
+          if(!isActive())//退出
+          {
+              break;
+          }
     }
 }
 
 
-void rtkprocess::initbuff(void)
-{
-    client_buff.max_size = 10 * 1024 *1024;//10M
-    client_buff.buff[0].clear();
-    client_buff.buff[1].clear();
-    client_buff.select = 0;
-
-    serial_buff.max_size = 10 * 1024 *1024;
-    serial_buff.buff[0].clear();
-    serial_buff.buff[1].clear();
-    serial_buff.select = 0;
-}
-
-void rtkprocess::setbuff(quint32 src,QByteArray data)
-{
-    switch (src) {
-    default:
-    case SourceType::c_sock:
-           //当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
-           if(client_buff.buff[client_buff.select].size() >= client_buff.max_size)
-           {
-               client_buff.buff[client_buff.select].clear();
-               qDebug() << "client_buff.buff " << client_buff.select <<" OVERFLOW";
-           }
-           client_buff.buff[client_buff.select].append(data);
-
-        break;
-    case SourceType::s_port:
-        //当前的buff超过10M字节之后就清除，防爆机制，不然buff太大后容易卡死
-           if(serial_buff.buff[serial_buff.select].size() >= serial_buff.max_size)
-           {
-               serial_buff.buff[serial_buff.select].clear();
-               qDebug() << "serial_buff.buff " << serial_buff.select <<" OVERFLOW";
-           }
-           serial_buff.buff[serial_buff.select].append(data);
-        break;
-    }
-}
-
-QByteArray rtkprocess::readbuff(quint32 src)
-{
-    QByteArray datagram;
-
-    switch (src) {
-    default:
-    case SourceType::c_sock:
-           if(client_buff.select == 0)
-           {
-               datagram.clear();
-               datagram.append(client_buff.buff[1]);
-               //清除这个未选择的buff
-               client_buff.buff[1].clear();
-               //读取完成，可以往这个内存里面写数了
-               client_buff.select = 1;
-           }
-           else if(client_buff.select == 1)
-           {
-               datagram.clear();
-               datagram.append(client_buff.buff[0]);
-               //清除这个未选择的buff
-               client_buff.buff[0].clear();
-               //读取完成，可以往这个内存里面写数了
-               client_buff.select = 0;
-           }
-        break;
-    case SourceType::s_port:
-           if(serial_buff.select == 0)
-           {
-               datagram.clear();
-               datagram.append(serial_buff.buff[1]);
-               //读取完成，可以往这个内存里面写数了
-               serial_buff.buff[1].clear();
-               serial_buff.select = 1;
-           }
-           else if(serial_buff.select == 1)
-           {
-               datagram.clear();
-               datagram.append(serial_buff.buff[0]);
-               //读取完成，可以往这个内存里面写数了
-               serial_buff.buff[0].clear();
-               serial_buff.select = 0;
-           }
-        break;
-    }
-    return datagram;
-}
-
-
-void rtkprocess::parse(quint32 src,QByteArray datagram)
-{
-   rtkrawdata.append(datagram);
-}
-
-
 void rtkprocess::update15Hz(void)
 {
     if (rtkrawdata.size() >= 180)

MavLinkNode/rtkprocess.h

@@ -46,13 +46,7 @@ signals:
 
 public slots:
 
-
-
-
-
     //缓存对外接口
-    void setID(int m_sysid, int m_compid);
-    void setbuff(quint32 src, QByteArray data);
 
 
 
@@ -63,16 +57,11 @@ private slots:
 
 
    //缓存私有接口
-   void initbuff(void);
-   QByteArray readbuff(quint32 src);
-
 
    void update15Hz(void);
 
 
    //解析
-   void parse(quint32 src,QByteArray datagram);
-
    void process();
 
 

MavLinkNode/statusprocess.cpp

@@ -86,6 +86,8 @@ void statusprocess::process()//线程函数
                  //qDebug() << "heartbeat";
              }
          }
+
+         QThread::yieldCurrentThread();
     }
     //退出线程
     //disconnect(thread, nullptr, nullptr, nullptr);

dlink/dlink.cpp

@@ -49,8 +49,21 @@ DLink::DLink(QObject *parent) : QObject(parent)
 
 }
 DLink::~DLink()
-{
-    stopPort();
+{   
+    foreach (serialNode node, serials) {
+
+        if(node.port)
+        {
+            if(node.port->isOpen())
+            {
+                node.port->close();
+            }
+            delete node.port;
+            node.port = nullptr;
+        }
+    }
+
+
 
     mavlinknode->stop();
     mavlinknode->deleteLater();
@@ -170,6 +183,15 @@ bool DLink::setupPort(const QString port, qint32 baudrate, QSerialPort::Parity p
         connect(serialPort, SIGNAL(readyRead()), this, SLOT(readPendingDatagramsSerialPort()));//本线程内调用
         isSuccess = true;
 
+        serialNode node;
+
+        node.port = serialPort;
+        node.baud = baudrate;
+        node.parity = parity;
+        node.name = port;
+
+        serials.insert(serials.size(),node);
+
         emit showMessage(tr("Serial Port Open Success"));
         qWarning() << "Serial Port Open Success";
     }
@@ -471,6 +493,15 @@ bool DLink::setup_remote(const QString port, qint32 baud, QSerialPort::Parity pa
         serialPort->setDataBits(QSerialPort::Data8);
         connect(serialPort, &QSerialPort::readyRead, this, &DLink::readRemotePendingDatagrams);
         isSuccess = true;
+
+        serialNode node;
+
+        node.port = serialPort;
+        node.baud = baud;
+        node.parity = parity;
+        node.name = port;
+
+        serials.insert(serials.size(),node);
     }
     else
     {
@@ -493,6 +524,15 @@ bool DLink::setup_rtk(const QString port, qint32 baudrate, QSerialPort::Parity p
         serialPort->setDataBits(QSerialPort::Data8);
         connect(serialPort, &QSerialPort::readyRead, this, &DLink::readRTKPendingDatagrams);
         isSuccess = true;
+
+        serialNode node;
+
+        node.port = serialPort;
+        node.baud = baudrate;
+        node.parity = parity;
+        node.name = port;
+
+        serials.insert(serials.size(),node);
     }
     else
     {

dlink/dlink.h

@@ -128,6 +128,7 @@ protected:
     QSerialPort *serialPortRTK = nullptr;
 
     QList<Node> clientSockets;
+    QList<serialNode> serials;
 
 
     QUdpSocket * multicast_sock=NULL;