ground/gcs/src/plugins/uavtalk/telemetry.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       telemetry.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup UAVTalkPlugin UAVTalk Plugin
   9  * @{
  10  * @brief The UAVTalk protocol plugin
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include "telemetry.h"
  29 #include "oplinksettings.h"
  30 #include "objectpersistence.h"
  31 #include <QTime>
  32 #include <QtGlobal>
  33 #include <stdlib.h>
  34 #include <QDebug>
  35
  36 /**
  37  * Constructor
  38  */
  39 Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr) : objMngr(objMngr), utalk(utalk)
  40 {
  41     mutex = new QMutex(QMutex::Recursive);
  42
  43     // Register all objects in the list
  44     foreach(QList<UAVObject *> instances, objMngr->getObjects()) {
  45         foreach(UAVObject * object, instances) {
  46             // make sure we 'forget' all objects before we request it from the flight side
  47             object->setIsKnown(false);
  48         }
  49         // we only need to register one instance per object type
  50         registerObject(instances.first());
  51     }
  52
  53     // Listen to new object creations
  54     // connection must be direct, if not, it is not possible to create and send (or request) an object in one go
  55     connect(objMngr, SIGNAL(newObject(UAVObject *)), this, SLOT(newObject(UAVObject *)), Qt::DirectConnection);
  56     connect(objMngr, SIGNAL(newInstance(UAVObject *)), this, SLOT(newInstance(UAVObject *)), Qt::DirectConnection);
  57
  58     // Listen to transaction completions
  59     // these slots will be executed in the telemetry thread
  60     // TODO should send a status (SUCCESS, FAILED, TIMEOUT)
  61     connect(utalk, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(transactionCompleted(UAVObject *, bool)));
  62
  63     // Get GCS stats object
  64     gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
  65
  66     // Setup and start the periodic timer
  67     timeToNextUpdateMs = 0;
  68     updateTimer = new QTimer(this);
  69     connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
  70     updateTimer->start(1000);
  71
  72     // Setup and start the stats timer
  73     txErrors  = 0;
  74     txRetries = 0;
  75 }
  76
  77 Telemetry::~Telemetry()
  78 {
  79     closeAllTransactions();
  80     foreach(QList<UAVObject *> instances, objMngr->getObjects()) {
  81         foreach(UAVObject * object, instances) {
  82             // 'forget' all objects
  83             object->setIsKnown(false);
  84         }
  85     }
  86 }
  87
  88 /**
  89  * Register a new object for periodic updates (if enabled)
  90  */
  91 void Telemetry::registerObject(UAVObject *obj)
  92 {
  93     // Setup object for periodic updates
  94     addObject(obj);
  95
  96     // Setup object for telemetry updates
  97     updateObject(obj, EV_NONE);
  98 }
  99
 100 /**
 101  * Add an object in the list used for periodic updates
 102  */
 103 void Telemetry::addObject(UAVObject *obj)
 104 {
 105     // Check if object type is already in the list
 106     for (int n = 0; n < objList.length(); ++n) {
 107         if (objList[n].obj->getObjID() == obj->getObjID()) {
 108             // Object type (not instance!) is already in the list, do nothing
 109             return;
 110         }
 111     }
 112
 113     // If this point is reached, then the object type is new, let's add it
 114     ObjectTimeInfo timeInfo;
 115     timeInfo.obj = obj;
 116     timeInfo.timeToNextUpdateMs = 0;
 117     timeInfo.updatePeriodMs     = 0;
 118     objList.append(timeInfo);
 119 }
 120
 121 /**
 122  * Update the object's timers
 123  */
 124 void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
 125 {
 126     // Find object type (not instance!) and update its period
 127     for (int n = 0; n < objList.length(); ++n) {
 128         if (objList[n].obj->getObjID() == obj->getObjID()) {
 129             objList[n].updatePeriodMs     = periodMs;
 130             objList[n].timeToNextUpdateMs = quint32((float)periodMs * (float)qrand() / (float)RAND_MAX); // avoid bunching of updates
 131         }
 132     }
 133 }
 134
 135 /**
 136  * Connect to all instances of an object depending on the event mask specified
 137  */
 138 void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask)
 139 {
 140     // TODO why connect systematically to all instances?
 141     // It is probably not needed to connect to always connect to all instances.
 142     QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
 143     for (int n = 0; n < objs.length(); ++n) {
 144         connectToObject(objs[n], eventMask);
 145     }
 146 }
 147
 148 /**
 149  * Connect to all instances of an object depending on the event mask specified
 150  */
 151 void Telemetry::connectToObject(UAVObject *obj, quint32 eventMask)
 152 {
 153     // Disconnect all
 154     obj->disconnect(this);
 155     // Connect only the selected events
 156     if ((eventMask & EV_UNPACKED) != 0) {
 157         connect(obj, SIGNAL(objectUnpacked(UAVObject *)), this, SLOT(objectUnpacked(UAVObject *)));
 158     }
 159     if ((eventMask & EV_UPDATED) != 0) {
 160         connect(obj, SIGNAL(objectUpdatedAuto(UAVObject *)), this, SLOT(objectUpdatedAuto(UAVObject *)));
 161     }
 162     if ((eventMask & EV_UPDATED_MANUAL) != 0) {
 163         connect(obj, SIGNAL(objectUpdatedManual(UAVObject *, bool)), this, SLOT(objectUpdatedManual(UAVObject *, bool)));
 164     }
 165     if ((eventMask & EV_UPDATED_PERIODIC) != 0) {
 166         connect(obj, SIGNAL(objectUpdatedPeriodic(UAVObject *)), this, SLOT(objectUpdatedPeriodic(UAVObject *)));
 167     }
 168     if ((eventMask & EV_UPDATE_REQ) != 0) {
 169         connect(obj, SIGNAL(updateRequested(UAVObject *, bool)), this, SLOT(updateRequested(UAVObject *, bool)));
 170     }
 171 }
 172
 173 /**
 174  * Update an object based on its metadata properties
 175  */
 176 void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
 177 {
 178     // Get metadata
 179     UAVObject::Metadata metadata     = obj->getMetadata();
 180     UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
 181
 182     // Setup object depending on update mode
 183     qint32 eventMask;
 184
 185     if (updateMode == UAVObject::UPDATEMODE_PERIODIC) {
 186         // Set update period
 187         setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
 188         // Connect signals
 189         eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
 190         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
 191             // we also need to act on remote updates (unpack events)
 192             eventMask |= EV_UNPACKED;
 193         }
 194         connectToObjectInstances(obj, eventMask);
 195     } else if (updateMode == UAVObject::UPDATEMODE_ONCHANGE) {
 196         // Set update period
 197         setUpdatePeriod(obj, 0);
 198         // Connect signals
 199         eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
 200         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
 201             // we also need to act on remote updates (unpack events)
 202             eventMask |= EV_UNPACKED;
 203         }
 204         connectToObjectInstances(obj, eventMask);
 205     } else if (updateMode == UAVObject::UPDATEMODE_THROTTLED) {
 206         // If we received a periodic update, we can change back to update on change
 207         if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) {
 208             // Set update period
 209             if (eventType == EV_NONE) {
 210                 setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
 211             }
 212             // Connect signals
 213             eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ | EV_UPDATED_PERIODIC;
 214         } else {
 215             // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates
 216             // Connect signals
 217             eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
 218         }
 219         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
 220             // we also need to act on remote updates (unpack events)
 221             eventMask |= EV_UNPACKED;
 222         }
 223         connectToObjectInstances(obj, eventMask);
 224     } else if (updateMode == UAVObject::UPDATEMODE_MANUAL) {
 225         // Set update period
 226         setUpdatePeriod(obj, 0);
 227         // Connect signals
 228         eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
 229         if (dynamic_cast<UAVMetaObject *>(obj) != NULL) {
 230             // we also need to act on remote updates (unpack events)
 231             eventMask |= EV_UNPACKED;
 232         }
 233         connectToObjectInstances(obj, eventMask);
 234     }
 235 }
 236
 237 /**
 238  * Called when a transaction is successfully completed (uavtalk event)
 239  */
 240 void Telemetry::transactionCompleted(UAVObject *obj, bool success)
 241 {
 242     // Lookup the transaction in the transaction map.
 243     ObjectTransactionInfo *transInfo = findTransaction(obj);
 244
 245     if (transInfo) {
 246         if (success) {
 247             // We now know that the flight side knows of this object.
 248             obj->setIsKnown(true);
 249 #ifdef VERBOSE_TELEMETRY
 250             qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief();
 251 #endif
 252         } else {
 253             obj->setIsKnown(false);
 254             qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief();
 255         }
 256
 257         // Remove this transaction as it's complete.
 258         closeTransaction(transInfo);
 259
 260         // Send signal
 261         obj->emitTransactionCompleted(success);
 262
 263         // Process new object updates from queue
 264         processObjectQueue();
 265     } else {
 266         qWarning() << "Telemetry - Error: received a transaction completed when did not expect it for" << obj->toStringBrief();
 267     }
 268 }
 269
 270 /**
 271  * Called when a transaction is not completed within the timeout period (timer event)
 272  */
 273 void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
 274 {
 275     // Check if more retries are pending
 276     if (transInfo->retriesRemaining > 0) {
 277 #ifdef VERBOSE_TELEMETRY
 278         qDebug().nospace() << "Telemetry - transaction timed out for object " << transInfo->obj->toStringBrief() << ", retrying...";
 279 #endif
 280         ++txRetries;
 281         --transInfo->retriesRemaining;
 282
 283         // Retry the transaction
 284         processObjectTransaction(transInfo);
 285     } else {
 286         qWarning().nospace() << "Telemetry - !!! transaction timed out for object " << transInfo->obj->toStringBrief();
 287
 288         ++txErrors;
 289
 290         // Terminate transaction
 291         utalk->cancelTransaction(transInfo->obj);
 292
 293         // Remove this transaction as it's complete.
 294         UAVObject *obj = transInfo->obj;
 295         closeTransaction(transInfo);
 296
 297         // Send signal
 298         obj->emitTransactionCompleted(false);
 299
 300         // Process new object updates from queue
 301         processObjectQueue();
 302     }
 303 }
 304
 305 /**
 306  * Start an object transaction with UAVTalk, all information is stored in transInfo
 307  */
 308 void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
 309 {
 310     // Initiate transaction
 311     bool sent = false;
 312
 313     if (transInfo->objRequest) {
 314 #ifdef VERBOSE_TELEMETRY
 315         qDebug().nospace() << "Telemetry - sending request for object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
 316 #endif
 317         sent = utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
 318     } else {
 319 #ifdef VERBOSE_TELEMETRY
 320         qDebug().nospace() << "Telemetry - sending object " << transInfo->obj->toStringBrief() << ", " << (transInfo->allInstances ? "all" : "single") << " " << (transInfo->acked ? "acked" : "");
 321 #endif
 322         sent = utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
 323     }
 324     // Check if a response is needed now or will arrive asynchronously
 325     if (transInfo->objRequest || transInfo->acked) {
 326         if (sent) {
 327             // Start timer if a response is expected
 328             transInfo->timer->start(REQ_TIMEOUT_MS);
 329         } else {
 330             // message was not sent, the transaction will not complete and will timeout
 331             // there is no need to wait to close the transaction and notify of completion failure
 332             // transactionCompleted(transInfo->obj, false);
 333         }
 334     } else {
 335         // not transacted, so just close the transaction with no notification of completion
 336         closeTransaction(transInfo);
 337     }
 338 }
 339
 340 /**
 341  * Process the event received from an object
 342  */
 343 void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allInstances, bool priority)
 344 {
 345     // Push event into queue
 346     ObjectQueueInfo objInfo;
 347
 348     objInfo.obj   = obj;
 349     objInfo.event = event;
 350     objInfo.allInstances = allInstances;
 351     if (priority) {
 352         if (objPriorityQueue.length() < MAX_QUEUE_SIZE) {
 353             objPriorityQueue.enqueue(objInfo);
 354         } else {
 355             ++txErrors;
 356             qWarning().nospace() << "Telemetry - !!! priority event queue is full, event lost " << obj->toStringBrief();
 357             obj->emitTransactionCompleted(false);
 358         }
 359     } else {
 360         if (objQueue.length() < MAX_QUEUE_SIZE) {
 361             objQueue.enqueue(objInfo);
 362         } else {
 363             ++txErrors;
 364             qWarning().nospace() << "Telemetry - !!! event queue is full, event lost " << obj->toStringBrief();
 365             obj->emitTransactionCompleted(false);
 366         }
 367     }
 368
 369     // Process the transaction
 370     processObjectQueue();
 371 }
 372
 373 /**
 374  * Process events from the object queue
 375  */
 376 void Telemetry::processObjectQueue()
 377 {
 378     // Get object information from queue (first the priority and then the regular queue)
 379     ObjectQueueInfo objInfo;
 380
 381     if (!objPriorityQueue.isEmpty()) {
 382         objInfo = objPriorityQueue.dequeue();
 383     } else if (!objQueue.isEmpty()) {
 384         objInfo = objQueue.dequeue();
 385     } else {
 386         return;
 387     }
 388
 389     // Check if a connection has been established, only process GCSTelemetryStats updates
 390     // (used to establish the connection)
 391     GCSTelemetryStats::DataFields gcsStats = gcsStatsObj->getData();
 392     if (gcsStats.Status != GCSTelemetryStats::STATUS_CONNECTED) {
 393         objQueue.clear();
 394         if ((objInfo.obj->getObjID() != GCSTelemetryStats::OBJID) &&
 395             (objInfo.obj->getObjID() != OPLinkSettings::OBJID) &&
 396             (objInfo.obj->getObjID() != ObjectPersistence::OBJID)) {
 397             objInfo.obj->emitTransactionCompleted(false);
 398             return;
 399         }
 400     }
 401
 402     // Setup transaction (skip if unpack event)
 403     UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
 404     UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
 405     if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
 406         // Check if a transaction for that object already exists
 407         // It is allowed to have multiple transaction on the same object ID provided that the instance IDs are different
 408         // If an "all instances" transaction is running, then it is not allowed to start another transaction with same object ID
 409         // If a single instance transaction is running, then starting an "all instance" transaction is not allowed
 410         // TODO make the above logic a reality...
 411         if (findTransaction(objInfo.obj)) {
 412             qWarning().nospace() << "Telemetry - !!! Making request for an object " << objInfo.obj->toStringBrief() << " for which a request is already in progress";
 413             // objInfo.obj->emitTransactionCompleted(false);
 414             return;
 415         }
 416         UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
 417         ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this);
 418         transInfo->obj   = objInfo.obj;
 419         transInfo->allInstances = objInfo.allInstances;
 420         transInfo->retriesRemaining = MAX_RETRIES;
 421         transInfo->acked = UAVObject::GetGcsTelemetryAcked(metadata);
 422         if (objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL || objInfo.event == EV_UPDATED_PERIODIC) {
 423             transInfo->objRequest = false;
 424         } else if (objInfo.event == EV_UPDATE_REQ) {
 425             transInfo->objRequest = true;
 426         }
 427         transInfo->telem = this;
 428         // Insert the transaction into the transaction map.
 429         openTransaction(transInfo);
 430         processObjectTransaction(transInfo);
 431     }
 432
 433     // If this is a metaobject then make necessary telemetry updates
 434     UAVMetaObject *metaobj = dynamic_cast<UAVMetaObject *>(objInfo.obj);
 435     if (metaobj != NULL) {
 436         updateObject(metaobj->getParentObject(), EV_NONE);
 437     } else if (updateMode != UAVObject::UPDATEMODE_THROTTLED) {
 438         updateObject(objInfo.obj, objInfo.event);
 439     }
 440
 441     // The fact we received an unpacked event does not mean that
 442     // we do not have additional objects still in the queue,
 443     // so we have to reschedule queue processing to make sure they are not
 444     // stuck:
 445     if (objInfo.event == EV_UNPACKED) {
 446         processObjectQueue();
 447     }
 448 }
 449
 450 /**
 451  * Check is any objects are pending for periodic updates
 452  * TODO: Clean-up
 453  */
 454 void Telemetry::processPeriodicUpdates()
 455 {
 456     QMutexLocker locker(mutex);
 457
 458     // Stop timer
 459     updateTimer->stop();
 460
 461     // Iterate through each object and update its timer, if zero then transmit object.
 462     // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs)
 463     qint32 minDelay  = MAX_UPDATE_PERIOD_MS;
 464     ObjectTimeInfo *objinfo;
 465     qint32 elapsedMs = 0;
 466     QTime time;
 467     qint32 offset;
 468     bool allInstances;
 469     for (int n = 0; n < objList.length(); ++n) {
 470         objinfo = &objList[n];
 471         // If object is configured for periodic updates
 472         if (objinfo->updatePeriodMs > 0) {
 473             objinfo->timeToNextUpdateMs -= timeToNextUpdateMs;
 474             // Check if time for the next update
 475             if (objinfo->timeToNextUpdateMs <= 0) {
 476                 // Reset timer
 477                 offset = (-objinfo->timeToNextUpdateMs) % objinfo->updatePeriodMs;
 478                 objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
 479                 // Send object
 480                 time.start();
 481                 allInstances = !objinfo->obj->isSingleInstance();
 482                 processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false);
 483                 elapsedMs    = time.elapsed();
 484                 // Update timeToNextUpdateMs with the elapsed delay of sending the object;
 485                 timeToNextUpdateMs += elapsedMs;
 486             }
 487             // Update minimum delay
 488             if (objinfo->timeToNextUpdateMs < minDelay) {
 489                 minDelay = objinfo->timeToNextUpdateMs;
 490             }
 491         }
 492     }
 493
 494     // Check if delay for the next update is too short
 495     if (minDelay < MIN_UPDATE_PERIOD_MS) {
 496         minDelay = MIN_UPDATE_PERIOD_MS;
 497     }
 498
 499     // Done
 500     timeToNextUpdateMs = minDelay;
 501
 502     // Restart timer
 503     updateTimer->start(timeToNextUpdateMs);
 504 }
 505
 506 Telemetry::TelemetryStats Telemetry::getStats()
 507 {
 508     QMutexLocker locker(mutex);
 509
 510     // Get UAVTalk stats
 511     UAVTalk::ComStats utalkStats = utalk->getStats();
 512
 513     // Update stats
 514     TelemetryStats stats;
 515
 516     stats.txBytes       = utalkStats.txBytes;
 517     stats.txObjectBytes = utalkStats.txObjectBytes;
 518     stats.txObjects     = utalkStats.txObjects;
 519     stats.txErrors      = utalkStats.txErrors + txErrors;
 520     stats.txRetries     = txRetries;
 521
 522     stats.rxBytes       = utalkStats.rxBytes;
 523     stats.rxObjectBytes = utalkStats.rxObjectBytes;
 524     stats.rxObjects     = utalkStats.rxObjects;
 525     stats.rxErrors      = utalkStats.rxErrors;
 526     stats.rxSyncErrors  = utalkStats.rxSyncErrors;
 527     stats.rxCrcErrors   = utalkStats.rxCrcErrors;
 528
 529     // Done
 530     return stats;
 531 }
 532
 533 void Telemetry::resetStats()
 534 {
 535     QMutexLocker locker(mutex);
 536
 537     utalk->resetStats();
 538     txErrors  = 0;
 539     txRetries = 0;
 540 }
 541
 542 void Telemetry::objectUpdatedAuto(UAVObject *obj)
 543 {
 544     QMutexLocker locker(mutex);
 545
 546     processObjectUpdates(obj, EV_UPDATED, false, true);
 547 }
 548
 549 void Telemetry::objectUpdatedManual(UAVObject *obj, bool all)
 550 {
 551     QMutexLocker locker(mutex);
 552
 553     bool allInstances = obj->isSingleInstance() ? false : all;
 554
 555     processObjectUpdates(obj, EV_UPDATED_MANUAL, allInstances, true);
 556 }
 557
 558 void Telemetry::objectUpdatedPeriodic(UAVObject *obj)
 559 {
 560     QMutexLocker locker(mutex);
 561
 562     processObjectUpdates(obj, EV_UPDATED_PERIODIC, false, true);
 563 }
 564
 565 void Telemetry::objectUnpacked(UAVObject *obj)
 566 {
 567     QMutexLocker locker(mutex);
 568
 569     processObjectUpdates(obj, EV_UNPACKED, false, true);
 570 }
 571
 572 void Telemetry::updateRequested(UAVObject *obj, bool all)
 573 {
 574     QMutexLocker locker(mutex);
 575
 576     bool allInstances = obj->isSingleInstance() ? false : all;
 577
 578     processObjectUpdates(obj, EV_UPDATE_REQ, allInstances, true);
 579 }
 580
 581 void Telemetry::newObject(UAVObject *obj)
 582 {
 583     QMutexLocker locker(mutex);
 584
 585 #ifdef VERBOSE_TELEMETRY
 586     qDebug() << "Telemetry - new object" << obj->toStringBrief();
 587 #endif
 588
 589     registerObject(obj);
 590 }
 591
 592 void Telemetry::newInstance(UAVObject *obj)
 593 {
 594     QMutexLocker locker(mutex);
 595
 596 #ifdef VERBOSE_TELEMETRY
 597     qDebug() << "Telemetry - new object instance" << obj->toStringBrief();
 598 #endif
 599
 600     registerObject(obj);
 601 }
 602
 603 ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
 604 {
 605     quint32 objId  = obj->getObjID();
 606     quint16 instId = obj->getInstID();
 607
 608     // Lookup the transaction in the transaction map
 609     QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId, NULL);
 610     if (objTransactions != NULL) {
 611         ObjectTransactionInfo *trans = objTransactions->value(instId, NULL);
 612         if (trans == NULL) {
 613             // see if there is an ALL_INSTANCES transaction
 614             trans = objTransactions->value(UAVTalk::ALL_INSTANCES, NULL);
 615         }
 616         return trans;
 617     }
 618     return NULL;
 619 }
 620
 621 void Telemetry::openTransaction(ObjectTransactionInfo *trans)
 622 {
 623     quint32 objId  = trans->obj->getObjID();
 624     quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
 625
 626     QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId, NULL);
 627     if (objTransactions == NULL) {
 628         objTransactions = new QMap<quint32, ObjectTransactionInfo *>();
 629         transMap.insert(objId, objTransactions);
 630     }
 631     objTransactions->insert(instId, trans);
 632 }
 633
 634 void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
 635 {
 636     quint32 objId  = trans->obj->getObjID();
 637     quint16 instId = trans->allInstances ? UAVTalk::ALL_INSTANCES : trans->obj->getInstID();
 638
 639     QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId, NULL);
 640     if (objTransactions != NULL) {
 641         objTransactions->remove(instId);
 642         // Keep the map even if it is empty
 643         // There are at most 100 different object IDs...
 644     }
 645     delete trans;
 646 }
 647
 648 void Telemetry::closeAllTransactions()
 649 {
 650     foreach(quint32 objId, transMap.keys()) {
 651         QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId, NULL);
 652         foreach(quint32 instId, objTransactions->keys()) {
 653             ObjectTransactionInfo *trans = objTransactions->value(instId, NULL);
 654
 655             qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief();
 656             objTransactions->remove(instId);
 657             delete trans;
 658         }
 659         transMap.remove(objId);
 660         delete objTransactions;
 661     }
 662 }
 663
 664 ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
 665 {
 666     obj = 0;
 667     allInstances     = false;
 668     objRequest       = false;
 669     retriesRemaining = 0;
 670     acked = false;
 671     telem = 0;
 672     // Setup transaction timer
 673     timer = new QTimer(this);
 674     timer->setSingleShot(true);
 675     connect(timer, SIGNAL(timeout()), this, SLOT(timeout()));
 676 }
 677
 678 ObjectTransactionInfo::~ObjectTransactionInfo()
 679 {
 680     telem = 0;
 681     timer->stop();
 682     delete timer;
 683 }
 684
 685 void ObjectTransactionInfo::timeout()
 686 {
 687     if (!telem.isNull()) {
 688         telem->transactionTimeout(this);
 689     }
 690 }