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