128 lines
2.9 KiB
C++
Executable File
128 lines
2.9 KiB
C++
Executable File
/**
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
*/
|
|
|
|
#include "mainthread.h"
|
|
|
|
#include "tetra_if_impl.h"
|
|
#include "trimble_if.h"
|
|
|
|
#include <QMutexLocker>
|
|
#include <QTimer>
|
|
#include <DDS/Core/pacemaker.h>
|
|
|
|
namespace DifferentialGPS
|
|
{
|
|
MainThread::MainThread(QObject *parent)
|
|
: QObject(parent)
|
|
, m_logger(getLogger("MainThread"))
|
|
, m_ddsHandler(nullptr)
|
|
, m_dgpsTimer(new QTimer(this))
|
|
, m_tetraIfNum(0)
|
|
, m_trimbleIfNum(0)
|
|
, m_broadcastTime(QTime::currentTime())
|
|
, m_broadcastCycle_ms(10000)
|
|
{
|
|
m_tetraIF[0] = new TetraImpl;
|
|
m_tetraIF[1] = new TetraImpl;
|
|
|
|
m_trimbleIF[0] = new TrimbleIF;
|
|
m_trimbleIF[1] = new TrimbleIF;
|
|
|
|
connect(this, &MainThread::newDataAvailable,
|
|
m_tetraIF[m_tetraIfNum], &TetraImpl::onNewDataAvailable,
|
|
Qt::QueuedConnection);
|
|
|
|
}
|
|
|
|
MainThread::~MainThread()
|
|
{
|
|
m_ddsHandler = nullptr;
|
|
|
|
m_tetraIF[0]->quit();
|
|
m_tetraIF[1]->quit();
|
|
m_trimbleIF[0]->quit();
|
|
m_trimbleIF[1]->quit();
|
|
|
|
m_tetraIF[0]->wait();
|
|
m_tetraIF[1]->wait();
|
|
m_trimbleIF[0]->wait();
|
|
m_trimbleIF[1]->wait();
|
|
|
|
delete m_tetraIF[0];
|
|
delete m_tetraIF[1];
|
|
delete m_trimbleIF[0];
|
|
delete m_trimbleIF[1];
|
|
}
|
|
|
|
|
|
bool MainThread::init(int domainId)
|
|
{
|
|
m_ddsHandler.reset(new DdsHandler(domainId));
|
|
if (!m_ddsHandler->init())
|
|
{
|
|
ERROR_LOG(m_logger) << "Initialisation DdsHandler failed.";
|
|
m_ddsHandler->pacemaker()->setNewState(Core::RunningState::StartingError);
|
|
return false;
|
|
}
|
|
|
|
m_trimbleIF[0]->startThread(0, this);
|
|
m_trimbleIF[1]->startThread(1, this);
|
|
|
|
m_tetraIF[m_tetraIfNum]->startThread(m_tetraIfNum, this);
|
|
|
|
m_ddsHandler->pacemaker()->setNewState(Core::RunningState::Running);
|
|
return true;
|
|
}
|
|
|
|
void MainThread::addNewData(const QByteArray &ba, int trimbleIfNum)
|
|
{
|
|
const int cycle_ms = m_broadcastCycle_ms + (m_broadcastCycle_ms / 5); // Zykluszeit + 20%
|
|
QTime currentTime = QTime::currentTime();
|
|
|
|
bool overtake = (trimbleIfNum == m_trimbleIfNum);
|
|
if (!overtake)
|
|
{
|
|
if (m_broadcastTime.msecsTo(currentTime) > cycle_ms)
|
|
{
|
|
overtake = true;
|
|
m_trimbleIfNum = trimbleIfNum; // Lese ab jetzt bevorzugt von diesem IF
|
|
}
|
|
}
|
|
|
|
if (overtake)
|
|
{
|
|
QMutexLocker l(&m_mutex);
|
|
m_dgpsData = ba;
|
|
if (m_tetraIF[m_tetraIfNum]->isRunning())
|
|
emit newDataAvailable();
|
|
else
|
|
{
|
|
disconnect(this, &MainThread::newDataAvailable,
|
|
m_tetraIF[m_tetraIfNum], &TetraImpl::onNewDataAvailable);
|
|
m_tetraIfNum = (m_tetraIfNum + 1) % 2;
|
|
connect(this, &MainThread::newDataAvailable,
|
|
m_tetraIF[m_tetraIfNum], &TetraImpl::onNewDataAvailable,
|
|
Qt::QueuedConnection);
|
|
|
|
m_tetraIF[m_tetraIfNum]->startThread(m_tetraIfNum, this);
|
|
}
|
|
|
|
m_broadcastTime = currentTime; // Die Broadcast-Zeit merken
|
|
}
|
|
|
|
}
|
|
|
|
bool MainThread::getNewData(QByteArray &ba)
|
|
{
|
|
QMutexLocker l(&m_mutex);
|
|
if (!m_dgpsData.size())
|
|
return false;
|
|
ba = m_dgpsData;
|
|
|
|
return true;
|
|
}
|
|
} // namespace DifferentialGPS
|