37 #include "hwtaulink.h"
38 #include "objectpersistence.h"
44 #ifdef TELEMETRY_DEBUG
45 #define TELEMETRY_QXTLOG_DEBUG(...) qDebug() << (__VA_ARGS__)
46 #else // TELEMETRY_DEBUG
47 #define TELEMETRY_QXTLOG_DEBUG(...)
48 #endif // TELEMETRY_DEBUG
93 this->objMngr = objMngr;
96 const int objSize = objs.size();
97 for (
int objidx = 0; objidx < objSize; ++objidx) {
98 registerObject(objs[objidx][0]);
107 gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
109 timeToNextUpdateMs = 0;
110 updateTimer =
new QTimer(
this);
111 connect(updateTimer, &QTimer::timeout,
this, &Telemetry::processPeriodicUpdates);
112 updateTimer->start(1000);
120 for (QMap<TransactionKey, ObjectTransactionInfo *>::iterator itr = transMap.begin();
121 itr != transMap.end(); ++itr) {
129 void Telemetry::registerObject(
UAVObject *obj)
135 updateObject(obj, EV_NONE);
141 void Telemetry::addObject(
UAVObject *obj)
144 const QVector<ObjectTimeInfo>::iterator iterEnd = objList.end();
145 for (QVector<ObjectTimeInfo>::const_iterator
iter = objList.constBegin();
iter != iterEnd;
154 ObjectTimeInfo timeInfo;
156 timeInfo.timeToNextUpdateMs = 0;
157 timeInfo.updatePeriodMs = 0;
158 objList.append(timeInfo);
164 void Telemetry::setUpdatePeriod(
UAVObject *obj, qint32 periodMs)
167 const quint32 objID = obj->
getObjID();
169 const QVector<ObjectTimeInfo>::iterator iterEnd = objList.end();
170 for (QVector<ObjectTimeInfo>::iterator
iter = objList.begin();
iter != iterEnd; ++
iter) {
171 if (
iter->obj->getObjID() == objID) {
172 iter->updatePeriodMs = periodMs;
173 iter->timeToNextUpdateMs = quint32((
float)periodMs * (
float)qrand()
182 void Telemetry::connectToObjectInstances(
UAVObject *obj, quint32 eventMask)
185 int objsSize = objs.size();
186 for (
int n = 0;
n < objsSize; ++
n) {
188 objs[
n]->disconnect(
this);
190 if ((eventMask & EV_UNPACKED) != 0) {
193 if ((eventMask & EV_UPDATED) != 0) {
196 if ((eventMask & EV_UPDATED_MANUAL) != 0) {
198 &Telemetry::objectUpdatedManual);
200 if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
202 &Telemetry::objectUpdatedPeriodic);
204 if ((eventMask & EV_UPDATE_REQ) != 0) {
207 &Telemetry::updateAllInstancesRequested);
221 void Telemetry::updateObject(
UAVObject *obj, quint32 eventType)
231 setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
233 eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
234 eventMask |= EV_UNPACKED;
235 connectToObjectInstances(obj, eventMask);
238 setUpdatePeriod(obj, 0);
240 eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
241 eventMask |= EV_UNPACKED;
242 connectToObjectInstances(obj, eventMask);
245 if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
247 if (eventType == EV_NONE)
248 setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
250 eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
255 eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
257 eventMask |= EV_UNPACKED;
258 connectToObjectInstances(obj, eventMask);
261 setUpdatePeriod(obj, 0);
263 eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
264 eventMask |= EV_UNPACKED;
265 connectToObjectInstances(obj, eventMask);
274 void Telemetry::transactionSuccess(
UAVObject *obj)
276 if (updateTransactionMap(obj,
false)) {
277 TELEMETRY_QXTLOG_DEBUG(
278 QString(
"[telemetry.cpp] Transaction succeeded:%0 Instance:%1")
280 + QString(QString(
" 0x") + QString::number(obj->
getObjID(), 16).toUpper()))
285 TELEMETRY_QXTLOG_DEBUG(
286 QString(
"[telemetry.cpp] Received an ACK we were not expecting object:%0")
290 processObjectQueue();
300 void Telemetry::transactionFailure(
UAVObject *obj)
303 if (sender() == this->utalk)
307 if (updateTransactionMap(obj,
true) || updateTransactionMap(obj,
false)) {
308 TELEMETRY_QXTLOG_DEBUG(
309 QString(
"[telemetry.cpp] Transaction failed:%0 Instance:%1")
311 + QString(QString(
" 0x") + QString::number(obj->
getObjID(), 16).toUpper()))
316 TELEMETRY_QXTLOG_DEBUG(
317 QString(
"[telemetry.cpp] Received a NACK we were not expecting for object %0")
321 processObjectQueue();
329 void Telemetry::transactionRequestCompleted(
UAVObject *obj)
331 if (updateTransactionMap(obj,
true)) {
332 TELEMETRY_QXTLOG_DEBUG(
333 QString(
"[telemetry.cpp] Transaction succeeded:%0 Instance:%1")
335 + QString(QString(
" 0x") + QString::number(obj->
getObjID(), 16).toUpper()))
340 TELEMETRY_QXTLOG_DEBUG(
341 QString(
"[telemetry.cpp] Received a ACK we were not expecting for object %0")
345 processObjectQueue();
357 bool Telemetry::updateTransactionMap(
UAVObject *obj,
bool request)
360 QMap<TransactionKey, ObjectTransactionInfo *>::iterator itr = transMap.find(key);
361 if (itr != transMap.end()) {
364 transInfo->
timer->stop();
365 transMap.remove(key);
377 transInfo->
timer->stop();
381 qInfo() << QString(
"[telemetry.cpp] Transaction timeout:%0 Instance:%1 Retrying remaining=%3")
383 + QString(QString(
" 0x")
384 + QString::number(transInfo->
obj->
getObjID(), 16).toUpper()))
387 processObjectTransaction(transInfo);
390 qInfo() << QString(
"[telemetry.cpp] Transaction timeout:%0 Instance:%1 no more retries. FAILED "
393 + QString(QString(
" 0x")
394 + QString::number(transInfo->
obj->
getObjID(), 16).toUpper()))
396 transInfo->
timer->stop();
397 transactionFailure(transInfo->
obj);
409 quint32 curOffset = 0;
411 QByteArray *result =
new QByteArray();
413 quint32 sizeGuess = 32 * 1024;
415 if (maxSize < sizeGuess) {
419 result->reserve(maxSize);
421 bool newReqNeeded =
false;
422 bool completed =
false;
423 int inactivityCount = 0;
429 timeStep.setSingleShot(
false);
432 connect(&timeStep, &QTimer::timeout, &loop, &QEventLoop::quit);
435 [&](quint32 recvFileId, quint32 offset, quint8 *
data, quint32 dataLen,
436 bool eof,
bool lastInSeq) {
437 if (recvFileId != fileId) {
441 if (offset != curOffset) {
450 result->append((
const char *) data, dataLen);
452 curOffset += dataLen;
457 }
else if (lastInSeq) {
466 progressCb(curOffset);
471 while (curOffset < maxSize && (!completed)) {
474 newReqNeeded =
false;
478 if ((inactivityCount++) > 10) {
479 qDebug() <<
"Retrying file transfer because of inactivity";
486 }
while (curOffset < maxSize && (!completed) && (!newReqNeeded));
489 qDebug() <<
"Aborting file transfer";
512 transInfo->
timer->start(REQ_TIMEOUT_MS);
524 void Telemetry::processObjectUpdates(
UAVObject *obj, EventMask event,
bool allInstances,
528 ObjectQueueInfo objInfo;
530 objInfo.event = event;
531 objInfo.allInstances = allInstances;
533 if (objPriorityQueue.length() < MAX_QUEUE_SIZE) {
534 objPriorityQueue.enqueue(objInfo);
539 qWarning() << QString(tr(
"Telemetry: priority event queue is full, event lost (%1)")
543 if (objQueue.length() < MAX_QUEUE_SIZE) {
544 objQueue.enqueue(objInfo);
549 qWarning() << QString(tr(
"Telemetry: event queue is full, event lost (%1)")
554 processObjectQueue();
560 void Telemetry::processObjectQueue()
562 if (objQueue.length() > 1) {
563 TELEMETRY_QXTLOG_DEBUG(
564 "[telemetry.cpp] **************** Object Queue above 1 in backlog ****************");
567 ObjectQueueInfo objInfo;
568 if (!objPriorityQueue.isEmpty()) {
569 objInfo = objPriorityQueue.dequeue();
570 }
else if (!objQueue.isEmpty()) {
571 objInfo = objQueue.dequeue();
578 GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
579 if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
581 if (objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) {
582 objInfo.obj->emitTransactionCompleted(
false);
583 objInfo.obj->emitTransactionCompleted(
false,
false);
589 UAVObject::Metadata metadata = objInfo.obj->getMetadata();
591 if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC)
594 if (transMap.contains(
TransactionKey(objInfo.obj, objInfo.event == EV_UPDATE_REQ))) {
595 TELEMETRY_QXTLOG_DEBUG(QString(
"[telemetry.cpp] Warning: Got request for %0 for which "
596 "a request is already in progress. Not doing it")
597 .arg(objInfo.obj->getName()));
600 UAVObject::Metadata metadata = objInfo.obj->getMetadata();
602 transInfo->
obj = objInfo.obj;
606 if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL
607 || objInfo.event == EV_UPDATED_PERIODIC) {
609 }
else if (objInfo.event == EV_UPDATE_REQ) {
612 transInfo->
telem =
this;
615 transMap.insert(key, transInfo);
616 processObjectTransaction(transInfo);
623 if (metaobj != NULL) {
626 updateObject(objInfo.obj, objInfo.event);
631 if (objInfo.event == EV_UNPACKED) {
634 TELEMETRY_QXTLOG_DEBUG(
635 QString(
"[telemetry.cpp] EV_UNPACKED %0 Instance:%1")
636 .arg(objInfo.obj->getName()
637 + QString(QString(
"0x")
638 + QString::number(objInfo.obj->getObjID(), 16).toUpper()))
639 .arg(objInfo.obj->getInstID()));
640 transactionRequestCompleted(objInfo.obj);
642 processObjectQueue();
650 void Telemetry::processPeriodicUpdates()
657 qint32 minDelay = MAX_UPDATE_PERIOD_MS;
658 qint32 elapsedMs = 0;
662 const QVector<ObjectTimeInfo>::iterator objInfoEnd = objList.end();
663 for (QVector<ObjectTimeInfo>::iterator objinfo = objList.begin(); objinfo != objInfoEnd;
669 if (objinfo->updatePeriodMs > 0) {
670 objinfo->timeToNextUpdateMs -= timeToNextUpdateMs;
672 if (objinfo->timeToNextUpdateMs <= 0) {
674 offset = (-objinfo->timeToNextUpdateMs) % objinfo->updatePeriodMs;
675 objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
678 processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC,
true,
false);
679 elapsedMs = time.elapsed();
681 timeToNextUpdateMs += elapsedMs;
684 if (objinfo->timeToNextUpdateMs < minDelay) {
685 minDelay = objinfo->timeToNextUpdateMs;
691 if (minDelay < MIN_UPDATE_PERIOD_MS) {
692 minDelay = MIN_UPDATE_PERIOD_MS;
696 timeToNextUpdateMs = minDelay;
699 updateTimer->start(timeToNextUpdateMs);
726 void Telemetry::objectUpdatedAuto(
UAVObject *obj)
728 processObjectUpdates(obj, EV_UPDATED,
false,
true);
731 void Telemetry::objectUpdatedManual(
UAVObject *obj)
733 processObjectUpdates(obj, EV_UPDATED_MANUAL,
false,
true);
736 void Telemetry::objectUpdatedPeriodic(
UAVObject *obj)
738 processObjectUpdates(obj, EV_UPDATED_PERIODIC,
false,
true);
741 void Telemetry::objectUnpacked(
UAVObject *obj)
743 processObjectUpdates(obj, EV_UNPACKED,
false,
true);
746 void Telemetry::updateRequested(
UAVObject *obj)
748 processObjectUpdates(obj, EV_UPDATE_REQ,
false,
true);
751 void Telemetry::updateAllInstancesRequested(
UAVObject *obj)
753 processObjectUpdates(obj, EV_UPDATE_REQ,
true,
true);
756 void Telemetry::newObject(
UAVObject *obj)
761 void Telemetry::newInstance(
UAVObject *obj)
770 allInstances =
false;
776 timer =
new QTimer(
this);
778 connect(
timer, &QTimer::timeout,
this, &ObjectTransactionInfo::timeout);
785 timer->deleteLater();
788 void ObjectTransactionInfo::timeout()
791 telem->transactionTimeout(
this);
QVector< UAVObject * > getObjectInstancesVector(const QString &name)
void emitTransactionCompleted(bool success)
QPointer< class Telemetry > telem
void objectUpdatedPeriodic(UAVObject *obj)
objectUpdatedPeriodic: not used anywhere ?
void objectUpdatedAuto(UAVObject *obj)
objectUpdatedAuto: triggered on "setData" only (Object data updated by changing the data structure) ...
QVector< QVector< UAVObject * > > getObjectsVector()
UAVObject * getParentObject()
void newInstance(UAVObject *obj)
void objectUnpacked(UAVObject *obj)
objectUnpacked: triggered whenever an object is unpacked (i.e. arrives from the telemetry link) ...
static UpdateMode GetGcsTelemetryUpdateMode(const Metadata &meta)
bool operator<(const TransactionKey &rhs) const
bool sendObject(UAVObject *obj, bool acked, bool allInstances)
bool operator==(const TransactionKey &rhs) const
QByteArray * downloadFile(quint32 fileId, quint32 maxSize, std::function< void(quint32)>progressCb=nullptr)
virtual Metadata getMetadata()=0
void fileDataReceived(quint32 fileId, quint32 offset, quint8 *data, quint32 dataLen, bool eof, bool lastInSeq)
ObjectTransactionInfo(QObject *parent)
void updateRequested(UAVObject *obj)
updateRequested
TransactionKey(UAVObject *obj, bool req)
void ackReceived(UAVObject *obj)
void updateAllInstancesRequested(UAVObject *obj)
updateAllInstancesRequested
void transactionTimeout(ObjectTransactionInfo *info)
TransactionKey(quint32 objId, quint32 instId, bool req)
bool requestFile(quint32 fileId, quint32 offset)
TelemetryStats getStats()
The TransactionKey class A key for the QMap to track transactions.
void objectUpdatedManual(UAVObject *obj)
objectUpdatedManual: triggered only from the "updated" slot in uavobject The telemetry manager listen...
Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
static quint8 GetGcsTelemetryAcked(const Metadata &meta)
bool sendObjectRequest(UAVObject *obj, bool allInstances)
void nackReceived(UAVObject *obj)
void newObject(UAVObject *obj)