#include "rtkprocess.h"

rtkprocess::rtkprocess(QObject *parent) : ThreadTemplet(parent)
{
    setRunFrq(0.001);//10hz
}

rtkprocess::~rtkprocess()
{
    qDebug() << "stop rtk" << QThread::currentThreadId();
}

void rtkprocess::process()//线程函数
{
    QThread::msleep(5000);//5s后再发送
    uint64_t lastTime = 0;
    QByteArray datagram = nullptr;

    uint8_t count = 0;

    while (true)
    {
         count ++;

         QThread::msleep(1000/frq());

          //解码从串口来的
          datagram.clear();
          datagram = readbuff(SourceType::s_port);//每次全部读取
          if(datagram.size() > 0)
          {
              //QApplication::processEvents();
               rtkrawdata.append(datagram);
          }

          quint64 currentTimestamp = (quint64)QDateTime::currentMSecsSinceEpoch();

          if((currentTimestamp - lastTime) > 66)
          {
              lastTime = currentTimestamp;
              rtkupdate();
          }

          if(isInterruptionRequested())//退出
          {
              break;
          }
         // QThread::yieldCurrentThread();
    }
}


void rtkprocess::rtkupdate(void)
{
    if (rtkrawdata.size() >= 180)
    {
        mavlink_message_t msg;
        mavlink_gps_rtcm_data_t rtcm;

        rtcm.flags = 0;
        rtcm.len = 180;

        for (uint i=0u;i<180u;++i)
        {
            rtcm.data[i] = rtkrawdata[i];
        }

        mavlink_msg_gps_rtcm_data_encode(GCS_SysID,GCS_CompID,&msg,&rtcm);
        rtkrawdata = rtkrawdata.mid(180);
        Send(msg);
    }
}