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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       uavtalk.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 #include "uavtalk.h"
  28 #include <extensionsystem/pluginmanager.h>
  29 #include <coreplugin/generalsettings.h>
  30 #include <utils/crc.h>
  31
  32 #include <QtEndian>
  33 #include <QDebug>
  34 #include <QEventLoop>
  35
  36 #ifdef VERBOSE_UAVTALK
  37 // uncomment and adapt the following lines to filter verbose logging to include specific object(s) only
  38 // #include "flighttelemetrystats.h"
  39 // #define VERBOSE_FILTER(objId) if (objId == FlightTelemetryStats::OBJID)
  40 #endif
  41 #ifndef VERBOSE_FILTER
  42 #define VERBOSE_FILTER(objId)
  43 #endif
  44
  45 #define SYNC_VAL 0x3C
  46
  47 using namespace Utils;
  48
  49 /**
  50  * Constructor
  51  */
  52 UAVTalk::UAVTalk(QIODevice *iodev, UAVObjectManager *objMngr) : io(iodev), objMngr(objMngr), mutex(QMutex::Recursive)
  53 {
  54     rxState = STATE_SYNC;
  55     rxPacketLength = 0;
  56
  57     memset(&stats, 0, sizeof(ComStats));
  58
  59     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  60     Core::Internal::GeneralSettings *settings = pm->getObject<Core::Internal::GeneralSettings>();
  61     useUDPMirror = settings->useUDPMirror();
  62     qDebug() << "USE UDP:::::::::::." << useUDPMirror;
  63     if (useUDPMirror) {
  64         udpSocketTx = new QUdpSocket(this);
  65         udpSocketRx = new QUdpSocket(this);
  66         udpSocketTx->bind(9000);
  67         udpSocketRx->connectToHost(QHostAddress::LocalHost, 9000);
  68         connect(udpSocketTx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead()));
  69         connect(udpSocketRx, SIGNAL(readyRead()), this, SLOT(dummyUDPRead()));
  70     }
  71 }
  72
  73 UAVTalk::~UAVTalk()
  74 {
  75     // According to Qt, it is not necessary to disconnect upon object deletion.
  76     // disconnect(io, SIGNAL(readyRead()), worker, SLOT(processInputStream()));
  77
  78     closeAllTransactions();
  79 }
  80
  81 /**
  82  * Reset the statistics counters
  83  */
  84 void UAVTalk::resetStats()
  85 {
  86     QMutexLocker locker(&mutex);
  87
  88     memset(&stats, 0, sizeof(ComStats));
  89 }
  90
  91 /**
  92  * Get the statistics counters
  93  */
  94 UAVTalk::ComStats UAVTalk::getStats()
  95 {
  96     QMutexLocker locker(&mutex);
  97
  98     return stats;
  99 }
 100
 101 void UAVTalk::dummyUDPRead()
 102 {
 103     QUdpSocket *socket = qobject_cast<QUdpSocket *>(sender());
 104     QByteArray junk;
 105
 106     while (socket->hasPendingDatagrams()) {
 107         junk.resize(socket->pendingDatagramSize());
 108         socket->readDatagram(junk.data(), junk.size());
 109     }
 110 }
 111
 112 /**
 113  * Send the specified object through the telemetry link.
 114  * \param[in] obj Object to send
 115  * \param[in] acked Selects if an ack is required
 116  * \param[in] allInstances If set true then all instances will be updated
 117  * \return Success (true), Failure (false)
 118  */
 119 bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
 120 {
 121     QMutexLocker locker(&mutex);
 122
 123     quint16 instId = 0;
 124
 125     if (allInstances) {
 126         instId = ALL_INSTANCES;
 127     } else if (obj) {
 128         instId = obj->getInstID();
 129     }
 130     bool success = false;
 131     if (acked) {
 132         success = objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj);
 133     } else {
 134         success = objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj);
 135     }
 136
 137     return success;
 138 }
 139
 140 /**
 141  * Request an update for the specified object, on success the object data would have been
 142  * updated by the GCS.
 143  * \param[in] obj Object to update
 144  * \param[in] allInstances If set true then all instances will be updated
 145  * \return Success (true), Failure (false)
 146  */
 147 bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
 148 {
 149     QMutexLocker locker(&mutex);
 150
 151     quint16 instId = 0;
 152
 153     if (allInstances) {
 154         instId = ALL_INSTANCES;
 155     } else if (obj) {
 156         instId = obj->getInstID();
 157     }
 158     return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj);
 159 }
 160
 161 /**
 162  * Cancel a pending transaction
 163  */
 164 void UAVTalk::cancelTransaction(UAVObject *obj)
 165 {
 166     QMutexLocker locker(&mutex);
 167
 168     if (io.isNull()) {
 169         return;
 170     }
 171     Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
 172     if (trans != NULL) {
 173         closeTransaction(trans);
 174     }
 175 }
 176
 177 /**
 178  * Execute the requested transaction on an object.
 179  * \param[in] obj Object
 180  * \param[in] type Transaction type
 181  *                        TYPE_OBJ: send object,
 182  *                        TYPE_OBJ_REQ: request object update
 183  *                        TYPE_OBJ_ACK: send object with an ack
 184  * \param[in] allInstances If set true then all instances will be updated
 185  * \return Success (true), Failure (false)
 186  */
 187 bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 188 {
 189     Q_ASSERT(obj);
 190     // Send object depending on if a response is needed
 191     // transactions of TYPE_OBJ_REQ are acked by the response
 192     if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
 193         if (transmitObject(type, objId, instId, obj)) {
 194             openTransaction(type, objId, instId);
 195             return true;
 196         } else {
 197             return false;
 198         }
 199     } else if (type == TYPE_OBJ) {
 200         return transmitObject(type, objId, instId, obj);
 201     } else {
 202         return false;
 203     }
 204 }
 205
 206 /**
 207  * Called each time there are data in the input buffer
 208  */
 209 void UAVTalk::processInputStream()
 210 {
 211     quint8 tmp;
 212
 213     if (io && io->isReadable()) {
 214         while (io->bytesAvailable() > 0) {
 215             int ret = io->read((char *)&tmp, 1);
 216             if (ret != -1) {
 217                 processInputByte(tmp);
 218             } else {
 219                 // TODOD
 220             }
 221             if (rxState == STATE_COMPLETE) {
 222                 mutex.lock();
 223                 if (receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength)) {
 224                     stats.rxObjectBytes += rxLength;
 225                     stats.rxObjects++;
 226                 } else {
 227                     // TODO...
 228                 }
 229                 mutex.unlock();
 230
 231                 if (useUDPMirror) {
 232                     // it is safe to do this outside of the above critical section as the rxDataArray is
 233                     // accessed from this thread only
 234                     udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
 235                 }
 236             }
 237         }
 238     }
 239 }
 240
 241 /**
 242  * Process an byte from the telemetry stream.
 243  * \param[in] rxbyte Received byte
 244  * \return Success (true), Failure (false)
 245  */
 246 bool UAVTalk::processInputByte(quint8 rxbyte)
 247 {
 248     if (rxState == STATE_COMPLETE || rxState == STATE_ERROR) {
 249         rxState = STATE_SYNC;
 250
 251         if (useUDPMirror) {
 252             rxDataArray.clear();
 253         }
 254     }
 255
 256     // Update stats
 257     stats.rxBytes++;
 258
 259     // update packet byte count
 260     rxPacketLength++;
 261
 262     if (useUDPMirror) {
 263         rxDataArray.append(rxbyte);
 264     }
 265
 266     // Receive state machine
 267     switch (rxState) {
 268     case STATE_SYNC:
 269
 270         if (rxbyte != SYNC_VAL) {
 271             // continue until sync byte is matched
 272             stats.rxSyncErrors++;
 273             break;
 274         }
 275
 276         // Initialize and update CRC
 277         rxCS = Crc::updateCRC(0, rxbyte);
 278
 279         rxPacketLength = 1;
 280
 281         // case local byte counter, don't forget to zero it after use.
 282         rxCount = 0;
 283
 284         rxState = STATE_TYPE;
 285         break;
 286
 287     case STATE_TYPE:
 288
 289         // Update CRC
 290         rxCS = Crc::updateCRC(rxCS, rxbyte);
 291
 292         if ((rxbyte & TYPE_MASK) != TYPE_VER) {
 293             qWarning() << "UAVTalk - error : bad type";
 294             stats.rxErrors++;
 295             rxState = STATE_ERROR;
 296             break;
 297         }
 298
 299         rxType     = rxbyte;
 300
 301         packetSize = 0;
 302
 303         rxState    = STATE_SIZE;
 304         break;
 305
 306     case STATE_SIZE:
 307
 308         // Update CRC
 309         rxCS = Crc::updateCRC(rxCS, rxbyte);
 310
 311         if (rxCount == 0) {
 312             packetSize += rxbyte;
 313             rxCount++;
 314             break;
 315         }
 316         packetSize += (quint32)rxbyte << 8;
 317         rxCount     = 0;
 318
 319
 320         if (packetSize < HEADER_LENGTH || packetSize > HEADER_LENGTH + MAX_PAYLOAD_LENGTH) {
 321             // incorrect packet size
 322             qWarning() << "UAVTalk - error : incorrect packet size";
 323             stats.rxErrors++;
 324             rxState = STATE_ERROR;
 325             break;
 326         }
 327
 328         rxState = STATE_OBJID;
 329         break;
 330
 331     case STATE_OBJID:
 332
 333         // Update CRC
 334         rxCS = Crc::updateCRC(rxCS, rxbyte);
 335
 336         rxTmpBuffer[rxCount++] = rxbyte;
 337         if (rxCount < 4) {
 338             break;
 339         }
 340         rxCount  = 0;
 341
 342         rxObjId  = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
 343
 344         // Message always contain an instance ID
 345         rxInstId = 0;
 346         rxState  = STATE_INSTID;
 347         break;
 348
 349     case STATE_INSTID:
 350
 351         // Update CRC
 352         rxCS = Crc::updateCRC(rxCS, rxbyte);
 353
 354         rxTmpBuffer[rxCount++] = rxbyte;
 355         if (rxCount < 2) {
 356             break;
 357         }
 358         rxCount  = 0;
 359
 360         rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
 361
 362         // Search for object, if not found reset state machine
 363         {
 364             UAVObject *rxObj = objMngr->getObject(rxObjId);
 365             if (rxObj == NULL && rxType != TYPE_OBJ_REQ) {
 366                 qWarning() << "UAVTalk - error : unknown object" << rxObjId;
 367                 stats.rxErrors++;
 368                 rxState = STATE_ERROR;
 369                 break;
 370             }
 371
 372             // Determine data length
 373             if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) {
 374                 rxLength = 0;
 375             } else {
 376                 if (rxObj) {
 377                     rxLength = rxObj->getNumBytes();
 378                 } else {
 379                     rxLength = packetSize - rxPacketLength;
 380                 }
 381             }
 382
 383             // Check length and determine next state
 384             if (rxLength >= MAX_PAYLOAD_LENGTH) {
 385                 // packet error - exceeded payload max length
 386                 qWarning() << "UAVTalk - error : exceeded payload max length" << rxObjId;
 387                 stats.rxErrors++;
 388                 rxState = STATE_ERROR;
 389                 break;
 390             }
 391
 392             // Check the lengths match
 393             if ((rxPacketLength + rxLength) != packetSize) {
 394                 // packet error - mismatched packet size
 395                 qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
 396                 stats.rxErrors++;
 397                 rxState = STATE_ERROR;
 398                 break;
 399             }
 400         }
 401
 402         // If there is a payload get it, otherwise receive checksum
 403         if (rxLength > 0) {
 404             rxState = STATE_DATA;
 405         } else {
 406             rxState = STATE_CS;
 407         }
 408         break;
 409
 410     case STATE_DATA:
 411
 412         // Update CRC
 413         rxCS = Crc::updateCRC(rxCS, rxbyte);
 414
 415         rxBuffer[rxCount++] = rxbyte;
 416         if (rxCount < rxLength) {
 417             break;
 418         }
 419         rxCount = 0;
 420
 421         rxState = STATE_CS;
 422         break;
 423
 424     case STATE_CS:
 425
 426         // The CRC byte
 427         rxCSPacket = rxbyte;
 428
 429         if (rxCS != rxCSPacket) {
 430             // packet error - faulty CRC
 431             qWarning() << "UAVTalk - error : failed CRC check" << rxObjId;
 432             stats.rxCrcErrors++;
 433             rxState = STATE_ERROR;
 434             break;
 435         }
 436
 437         if (rxPacketLength != packetSize + CHECKSUM_LENGTH) {
 438             // packet error - mismatched packet size
 439             qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId;
 440             stats.rxErrors++;
 441             rxState = STATE_ERROR;
 442             break;
 443         }
 444
 445         rxState = STATE_COMPLETE;
 446         break;
 447
 448     default:
 449
 450         qWarning() << "UAVTalk - error : bad state";
 451         rxState = STATE_ERROR;
 452         break;
 453     }
 454
 455     // Done
 456     return true;
 457 }
 458
 459 /**
 460  * Receive an object. This function process objects received through the telemetry stream.
 461  *
 462  * Parser errors are considered as transmission errors and are not NACKed.
 463  * Some senders (GCS) can timeout and retry if the message is not answered by an ack or nack.
 464  *
 465  * Object handling errors are considered as application errors and are NACked.
 466  * In that case we want to nack as there is no point in the sender retrying to send invalid objects.
 467  *
 468  * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
 469  * \param[in] obj Handle of the received object
 470  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
 471  * \param[in] data Data buffer
 472  * \param[in] length Buffer length
 473  * \return Success (true), Failure (false)
 474  */
 475 bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length)
 476 {
 477     Q_UNUSED(length);
 478
 479     UAVObject *obj    = NULL;
 480     bool error        = false;
 481     bool allInstances = (instId == ALL_INSTANCES);
 482
 483     // Process message type
 484     switch (type) {
 485     case TYPE_OBJ:
 486         // All instances, not allowed for OBJ messages
 487         if (!allInstances) {
 488             // Get object and update its data
 489             obj = updateObject(objId, instId, data);
 490 #ifdef VERBOSE_UAVTALK
 491             VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 492 #endif
 493             if (obj != NULL) {
 494                 // Check if this object acks a pending OBJ_REQ message
 495                 // any OBJ message can ack a pending OBJ_REQ message
 496                 // even one that was not sent in response to the OBJ_REQ message
 497                 updateAck(type, objId, instId, obj);
 498             } else {
 499                 error = true;
 500             }
 501         } else {
 502             error = true;
 503         }
 504         break;
 505
 506     case TYPE_OBJ_ACK:
 507         // All instances, not allowed for OBJ_ACK messages
 508         if (!allInstances) {
 509             // Get object and update its data
 510             obj = updateObject(objId, instId, data);
 511 #ifdef VERBOSE_UAVTALK
 512             VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 513 #endif
 514             if (obj != NULL) {
 515                 // Object updated or created, transmit ACK
 516                 error = !transmitObject(TYPE_ACK, objId, instId, obj);
 517             } else {
 518                 error = true;
 519             }
 520         } else {
 521             error = true;
 522         }
 523         if (error) {
 524             // failed to update object, transmit NACK
 525             transmitObject(TYPE_NACK, objId, instId, NULL);
 526         }
 527         break;
 528
 529     case TYPE_OBJ_REQ:
 530         // Check if requested object exists
 531         if (allInstances) {
 532             // All instances, so get instance zero
 533             obj = objMngr->getObject(objId);
 534         } else {
 535             obj = objMngr->getObject(objId, instId);
 536         }
 537 #ifdef VERBOSE_UAVTALK
 538         VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 539 #endif
 540         if (obj != NULL) {
 541             // Object found, transmit it
 542             // The sent object will ack the object request on the receiver side
 543             error = !transmitObject(TYPE_OBJ, objId, instId, obj);
 544         } else {
 545             error = true;
 546         }
 547         if (error) {
 548             // failed to send object, transmit NACK
 549             transmitObject(TYPE_NACK, objId, instId, NULL);
 550         }
 551         break;
 552
 553     case TYPE_ACK:
 554         // All instances, not allowed for ACK messages
 555         if (!allInstances) {
 556             // Get object
 557             obj = objMngr->getObject(objId, instId);
 558 #ifdef VERBOSE_UAVTALK
 559             VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 560 #endif
 561             if (obj != NULL) {
 562                 // Check if an ACK is pending
 563                 updateAck(type, objId, instId, obj);
 564             } else {
 565                 error = true;
 566             }
 567         }
 568         break;
 569
 570     case TYPE_NACK:
 571         // All instances, not allowed for NACK messages
 572         if (!allInstances) {
 573             // Get object
 574             obj = objMngr->getObject(objId, instId);
 575 #ifdef VERBOSE_UAVTALK
 576             VERBOSE_FILTER(objId) qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 577 #endif
 578             if (obj != NULL) {
 579                 // Check if a NACK is pending
 580                 updateNack(objId, instId, obj);
 581             } else {
 582                 error = true;
 583             }
 584         }
 585         break;
 586
 587     default:
 588         error = true;
 589     }
 590     if (error) {
 591         qWarning() << "UAVTalk - !!! error receiving" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 592     }
 593     // Done
 594     return !error;
 595 }
 596
 597 /**
 598  * Update the data of an object from a byte array (unpack).
 599  * If the object instance could not be found in the list, then a
 600  * new one is created.
 601  */
 602 UAVObject *UAVTalk::updateObject(quint32 objId, quint16 instId, quint8 *data)
 603 {
 604     // Get object
 605     UAVObject *obj = objMngr->getObject(objId, instId);
 606
 607     // If the instance does not exist create it
 608     if (obj == NULL) {
 609         // Get the object type
 610         UAVObject *typeObj = objMngr->getObject(objId);
 611         if (typeObj == NULL) {
 612             qWarning() << "UAVTalk - failed to get object, object ID :" << objId;
 613             return NULL;
 614         }
 615         // Make sure this is a data object
 616         UAVDataObject *dataObj = dynamic_cast<UAVDataObject *>(typeObj);
 617         if (dataObj == NULL) {
 618             return NULL;
 619         }
 620         // Create a new instance, unpack and register
 621         UAVDataObject *instObj = dataObj->clone(instId);
 622         if (!objMngr->registerObject(instObj)) {
 623             qWarning() << "UAVTalk - failed to register object " << instObj->toStringBrief();
 624             return NULL;
 625         }
 626         instObj->unpack(data);
 627         return instObj;
 628     } else {
 629         // Unpack data into object instance
 630         obj->unpack(data);
 631         return obj;
 632     }
 633 }
 634
 635 /**
 636  * Check if a transaction is pending and if yes complete it.
 637  */
 638 void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 639 {
 640     Q_ASSERT(obj);
 641     if (!obj) {
 642         return;
 643     }
 644     Transaction *trans = findTransaction(objId, instId);
 645     if (trans && trans->respType == type) {
 646         if (trans->respInstId == ALL_INSTANCES) {
 647             if (instId == 0) {
 648                 // last instance received, complete transaction
 649                 closeTransaction(trans);
 650                 emit transactionCompleted(obj, true);
 651             } else {
 652                 // TODO extend timeout?
 653             }
 654         } else {
 655             closeTransaction(trans);
 656             emit transactionCompleted(obj, true);
 657         }
 658     }
 659 }
 660
 661 /**
 662  * Check if a transaction is pending and if yes complete it.
 663  */
 664 void UAVTalk::updateNack(quint32 objId, quint16 instId, UAVObject *obj)
 665 {
 666     Q_ASSERT(obj);
 667     if (!obj) {
 668         return;
 669     }
 670     Transaction *trans = findTransaction(objId, instId);
 671     if (trans) {
 672         closeTransaction(trans);
 673         emit transactionCompleted(obj, false);
 674     }
 675 }
 676
 677 /**
 678  * Send an object through the telemetry link.
 679  * \param[in] type Transaction type
 680  * \param[in] objId Object ID to send
 681  * \param[in] instId Instance ID to send
 682  * \param[in] obj Object to send (null when type is NACK)
 683  * \return Success (true), Failure (false)
 684  */
 685 bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 686 {
 687     // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
 688
 689     // If all instances are requested on a single instance object it is an error
 690     if ((obj != NULL) && (instId == ALL_INSTANCES) && obj->isSingleInstance()) {
 691         instId = 0;
 692     }
 693     bool allInstances = (instId == ALL_INSTANCES);
 694
 695 #ifdef VERBOSE_UAVTALK
 696     VERBOSE_FILTER(objId) qDebug() << "UAVTalk - transmitting" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 697 #endif
 698
 699     // Process message type
 700     bool ret = false;
 701     if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
 702         if (allInstances) {
 703             // Send all instances in reverse order
 704             // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received)
 705             ret = true;
 706             quint32 numInst = objMngr->getNumInstances(objId);
 707             for (quint32 n = 0; n < numInst; ++n) {
 708                 quint32 i    = numInst - n - 1;
 709                 UAVObject *o = objMngr->getObject(objId, i);
 710                 if (!transmitSingleObject(type, objId, i, o)) {
 711                     ret = false;
 712                     break;
 713                 }
 714             }
 715         } else {
 716             ret = transmitSingleObject(type, objId, instId, obj);
 717         }
 718     } else if (type == TYPE_OBJ_REQ) {
 719         ret = transmitSingleObject(TYPE_OBJ_REQ, objId, instId, NULL);
 720     } else if (type == TYPE_ACK || type == TYPE_NACK) {
 721         if (!allInstances) {
 722             ret = transmitSingleObject(type, objId, instId, NULL);
 723         }
 724     }
 725
 726 #ifdef VERBOSE_UAVTALK
 727     if (!ret) {
 728         VERBOSE_FILTER(objId) qDebug() << "UAVTalk - failed to transmit" << typeToString(type) << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
 729     }
 730 #endif
 731
 732     return ret;
 733 }
 734
 735 /**
 736  * Send an object through the telemetry link.
 737  * \param[in] type Transaction type
 738  * \param[in] objId Object ID to send
 739  * \param[in] instId Instance ID to send
 740  * \param[in] obj Object to send (null when type is NACK)
 741  * \return Success (true), Failure (false)
 742  */
 743 bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 744 {
 745     qint32 length;
 746
 747     // IMPORTANT : obj can be null (when type is NACK for example)
 748
 749     // Setup sync byte
 750     txBuffer[0] = SYNC_VAL;
 751     // Setup type
 752     txBuffer[1] = type;
 753     // next 2 bytes are reserved for data length (inserted here later)
 754     // Setup object ID
 755     qToLittleEndian<quint32>(objId, &txBuffer[4]);
 756     // Setup instance ID
 757     qToLittleEndian<quint16>(instId, &txBuffer[8]);
 758
 759     // Determine data length
 760     if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
 761         length = 0;
 762     } else {
 763         length = obj->getNumBytes();
 764     }
 765
 766     // Check length
 767     if (length >= MAX_PAYLOAD_LENGTH) {
 768         qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief();
 769         ++stats.txErrors;
 770         return false;
 771     }
 772
 773     // Copy data (if any)
 774     if (length > 0) {
 775         if (!obj->pack(&txBuffer[HEADER_LENGTH])) {
 776             qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief();
 777             ++stats.txErrors;
 778             return false;
 779         }
 780     }
 781
 782     // Store the packet length
 783     qToLittleEndian<quint16>(HEADER_LENGTH + length, &txBuffer[2]);
 784
 785     // Calculate checksum
 786     txBuffer[HEADER_LENGTH + length] = Crc::updateCRC(0, txBuffer, HEADER_LENGTH + length);
 787
 788     // Send buffer, check that the transmit backlog does not grow above limit
 789     if (!io.isNull() && io->isWritable()) {
 790         if (io->bytesToWrite() < TX_BUFFER_SIZE) {
 791             io->write((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH);
 792             if (useUDPMirror) {
 793                 udpSocketRx->writeDatagram((const char *)txBuffer, HEADER_LENGTH + length + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
 794             }
 795         } else {
 796             qWarning() << "UAVTalk - error transmitting : io device full";
 797             ++stats.txErrors;
 798             return false;
 799         }
 800     } else {
 801         qWarning() << "UAVTalk - error transmitting : io device not writable";
 802         ++stats.txErrors;
 803         return false;
 804     }
 805
 806     // Update stats
 807     ++stats.txObjects;
 808     stats.txObjectBytes += length;
 809     stats.txBytes += HEADER_LENGTH + length + CHECKSUM_LENGTH;
 810
 811     // Done
 812     return true;
 813 }
 814
 815 UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
 816 {
 817     // Lookup the transaction in the transaction map
 818     QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
 819     if (objTransactions != NULL) {
 820         Transaction *trans = objTransactions->value(instId);
 821         if (trans == NULL) {
 822             // see if there is an ALL_INSTANCES transaction
 823             trans = objTransactions->value(ALL_INSTANCES);
 824         }
 825         return trans;
 826     }
 827     return NULL;
 828 }
 829
 830 void UAVTalk::openTransaction(quint8 type, quint32 objId, quint16 instId)
 831 {
 832     Transaction *trans = new Transaction();
 833
 834     trans->respType   = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
 835     trans->respObjId  = objId;
 836     trans->respInstId = instId;
 837
 838     QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
 839     if (objTransactions == NULL) {
 840         objTransactions = new QMap<quint32, Transaction *>();
 841         transMap.insert(trans->respObjId, objTransactions);
 842     }
 843     objTransactions->insert(trans->respInstId, trans);
 844 }
 845
 846 void UAVTalk::closeTransaction(Transaction *trans)
 847 {
 848     QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
 849     if (objTransactions != NULL) {
 850         objTransactions->remove(trans->respInstId);
 851         // Keep the map even if it is empty
 852         // There are at most 100 different object IDs...
 853         delete trans;
 854     }
 855 }
 856
 857 void UAVTalk::closeAllTransactions()
 858 {
 859     foreach(quint32 objId, transMap.keys()) {
 860         QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
 861         foreach(quint32 instId, objTransactions->keys()) {
 862             Transaction *trans = objTransactions->value(instId);
 863
 864             qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId;
 865             objTransactions->remove(instId);
 866             delete trans;
 867         }
 868         transMap.remove(objId);
 869         delete objTransactions;
 870     }
 871 }
 872
 873 const char *UAVTalk::typeToString(quint8 type)
 874 {
 875     switch (type) {
 876     case TYPE_OBJ:
 877         return "object";
 878
 879         break;
 880
 881     case TYPE_OBJ_ACK:
 882         return "object (acked)";
 883
 884         break;
 885
 886     case TYPE_OBJ_REQ:
 887         return "object request";
 888
 889         break;
 890
 891     case TYPE_ACK:
 892         return "ack";
 893
 894         break;
 895
 896     case TYPE_NACK:
 897         return "nack";
 898
 899         break;
 900     }
 901     return "<error>";
 902 }