76 lines
1.5 KiB
C++
76 lines
1.5 KiB
C++
#include "rtkprocess.h"
|
|
|
|
rtkprocess::rtkprocess(QObject *parent) : ThreadTemplet(parent)
|
|
{
|
|
setRunFrq(10);//10hz
|
|
}
|
|
|
|
rtkprocess::~rtkprocess()
|
|
{
|
|
qDebug() << "stop rtk" << QThread::currentThreadId();
|
|
}
|
|
|
|
void rtkprocess::process()//线程函数
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|