重改播放

MavLinkNode/mavlinknode.cpp

@@ -9,11 +9,17 @@
 
 MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 {
-    //main thread
+    QDir *temp = new QDir;
+    if(!temp->exists("./Tlog"))
+    {
+        qDebug() << "make dir tlog";
+        temp->mkdir("./Tlog");//如果文件夹不存在就新建
+    }
+
     running_flag = false;
-    Nodethread = new QThread();
-    this->moveToThread(Nodethread);
-    connect(Nodethread, &QThread::started, this, &MavLinkNode::process);
+    thread = new QThread();
+    this->moveToThread(thread);
+    connect(thread, &QThread::started, this, &MavLinkNode::process);
     setRunFrq(10);//50
     //初始化buff
     initbuff();
@@ -22,6 +28,11 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
     int Current_sysID = 0xFB;
     int Current_CompID = MAV_COMP_ID_MISSIONPLANNER;
 
+    replay = new Replay();
+    connect(replay,SIGNAL(readReady()),
+            this,SLOT(readPendingDatagramsReplay()),Qt::DirectConnection);
+
+
     Mission = new MissionProcess();
     Mission->setGCSID(Current_sysID,Current_CompID);
     connect(Mission,SIGNAL(SendMessageTo(quint8,quint8*,quint16)),
@@ -54,12 +65,26 @@ MavLinkNode::MavLinkNode(QObject *parent) : QObject(parent)
 
 
     showMessage(tr("解锁才能有航迹，需要修正,示波器显示时间不对"));
-
-
 }
 
 MavLinkNode::~MavLinkNode()
 {
+    //关闭文件
+    if (mavLogFile)
+    {
+        mavLogFile->close();
+        delete mavLogFile;
+        mavLogFile = NULL;
+    }
+
+    //停止回放
+    if(replay)
+    {
+        replay->stop();
+        delete replay;
+        replay = nullptr;
+    }
+
     //停止任务
     if(Mission)
     {
@@ -70,6 +95,7 @@ MavLinkNode::~MavLinkNode()
             Mission = nullptr;
         }
     }
+
     //停止参数
     if(Parameter)
     {
@@ -91,14 +117,14 @@ MavLinkNode::~MavLinkNode()
         }
     }
 
-    if(Nodethread->isRunning())
+    if(thread->isRunning())
     {
-        Nodethread->quit();
-        Nodethread->wait();
+        thread->quit();
+        thread->wait();
     }
 
-    delete Nodethread;
-    Nodethread = nullptr;
+    delete thread;
+    thread = nullptr;
 
 }
 
@@ -114,16 +140,31 @@ void MavLinkNode::setRunFrq(uint32_t frq)
 
 void MavLinkNode::start()
 {
-    if(!Nodethread->isRunning())
+    if(!thread->isRunning())
     {
+        if (mavLogFile)
+        {
+            mavLogFile->close();
+            delete mavLogFile;
+        }
+
+        QDateTime current = QDateTime::currentDateTime();
+        mavLogFile = new QFile(QString("./Tlog/%1.tlog").arg(current.toString("yyyyMMddHHmmss")));
+        mavLogFile->open(QIODevice::WriteOnly);
+
         //初始化buff
         initbuff();
 
         running_flag = true;
-        Nodethread->start();
+        thread->start();
         qDebug() << "MavLinkNode thread start" << running_flag;
 
 
+        if(replay)
+        {
+            replay->start();
+        }
+
         if(Mission)
         {
             Mission->start();
@@ -148,8 +189,15 @@ void MavLinkNode::start()
 
 void MavLinkNode::stop()
 {
-    if(Nodethread->isRunning())
+    if(thread->isRunning())
     {
+        if (mavLogFile)
+        {
+            mavLogFile->close();
+            delete mavLogFile;
+            mavLogFile = NULL;
+        }
+
         running_flag = false;
         qDebug() << "thread stop" << running_flag;
     }
@@ -189,9 +237,11 @@ void MavLinkNode::process()//线程函数
          }
 
     }
+    running_flag = false;
     //退出线程
-    Nodethread->quit();
-    Nodethread->wait();
+    disconnect(thread, nullptr, nullptr, nullptr);
+    thread->quit();
+    thread->wait();
 }
 
 void MavLinkNode::initbuff(void)
@@ -292,12 +342,19 @@ void MavLinkNode::Mavlinkparse(quint32 src,QByteArray datagram)
     mavlink_message_t msg;
     mavlink_status_t status;
 
-    //qDebug() << datagram;
-
     for (QByteArray::const_iterator i = datagram.cbegin(); i != datagram.cend(); ++i)
     {
         if(MAVLINK_FRAMING_OK == mavlink_parse_char(src,*i,&msg,&status))
         {
+            uint8_t buff[MAVLINK_MAX_PACKET_LEN+sizeof(quint64)];
+            quint64 currentTimestamp = ((quint64)QDateTime::currentMSecsSinceEpoch()) * 1000;
+            qToBigEndian(currentTimestamp, buff);
+            uint16_t len = mavlink_msg_to_send_buffer(buff+sizeof(quint64), &msg);
+            if (mavLogFile)
+            {
+                mavLogFile->write((const char *)buff, len+sizeof(quint64));
+            }
+
             count++;
             if(msg.sysid < 250)             //过滤地面站发过来的数据
             {
@@ -534,3 +591,19 @@ void MavLinkNode::setCurrentSelected(int sysid,int compid)
 
 }
 
+void MavLinkNode::readPendingDatagramsReplay(void)
+{
+    if(replay)
+    {
+        QByteArray datagram = replay->readAll();
+        setbuff(SourceType::c_sock,datagram);
+    }
+}
+
+void MavLinkNode::setLogfile(QString file)
+{
+    if(replay)
+    {
+       replay->setLogfile(file);
+    }
+}