OP-1122 OP-1125 uavtalk - fixed a number of issues concerning acked multi instance messages :

- all messages now systematically include an instance ID (fixes parsing of messages containing unknown object)
- added missing NACK of object request on fight side
- added more logging and warnings
- misc fixes and cleanups
this commit affects both the firmware and GCS (need to flash new version of firmware)

flight/uavtalk/uavtalk.c

@@ -34,10 +34,9 @@
 
 
 // Private functions
-static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
+static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout);
-static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
+static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
-static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
+static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
-static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId);
 static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length);
 static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
 
@@ -213,7 +212,8 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandl
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
-    return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout);
+
+    return objectTransaction(connection, UAVTALK_TYPE_OBJ_REQ, obj, instId, timeout);
 }
 
 /**
@@ -231,11 +231,12 @@ int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj,
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
+
     // Send object
     if (acked == 1) {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK, obj, instId, timeoutMs);
     } else {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ, obj, instId, timeoutMs);
     }
 }
 
@@ -254,27 +255,29 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH
     UAVTalkConnectionData *connection;
 
     CHECKCONHANDLE(connectionHandle, connection, return -1);
+
     // Send object
     if (acked == 1) {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK_TS, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_ACK_TS, obj, instId, timeoutMs);
     } else {
-        return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_TS, timeoutMs);
+        return objectTransaction(connection, UAVTALK_TYPE_OBJ_TS, obj, instId, timeoutMs);
     }
 }
 
 /**
  * Execute the requested transaction on an object.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object
- * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
  * \param[in] type Transaction type
  *                        UAVTALK_TYPE_OBJ: send object,
  *                        UAVTALK_TYPE_OBJ_REQ: request object update
  *                        UAVTALK_TYPE_OBJ_ACK: send object with an ack
+ * \param[in] obj Object
+ * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
+ * \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
+static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs)
 {
     int32_t respReceived;
 
@@ -286,7 +289,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
         connection->respObj    = obj;
         connection->respInstId = instId;
-        sendObject(connection, obj, instId, type);
+        sendObject(connection, type, UAVObjGetID(obj), instId, obj);
         xSemaphoreGiveRecursive(connection->lock);
         // Wait for response (or timeout)
         respReceived = xSemaphoreTake(connection->respSema, timeoutMs / portTICK_RATE_MS);
@@ -305,7 +308,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
         }
     } else if (type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_TS) {
         xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
-        sendObject(connection, obj, instId, type);
+        sendObject(connection, type, UAVObjGetID(obj), instId, obj);
         xSemaphoreGiveRecursive(connection->lock);
         return 0;
     } else {
@@ -326,6 +329,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
     CHECKCONHANDLE(connectionHandle, connection, return -1);
 
     UAVTalkInputProcessor *iproc = &connection->iproc;
+
     ++connection->stats.rxBytes;
 
     if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) {
@@ -338,6 +342,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
     // Receive state machine
     switch (iproc->state) {
     case UAVTALK_STATE_SYNC:
+
         if (rxbyte != UAVTALK_SYNC_VAL) {
             break;
         }
@@ -408,19 +413,18 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
         // Determine data length
         if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) {
             iproc->length = 0;
-            iproc->instanceLength = 0;
         } else {
             if (iproc->obj) {
                 iproc->length = UAVObjGetNumBytes(iproc->obj);
-                iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2);
             } else {
-                // We don't know if it's a multi-instance object, so just assume it's 0.
-                iproc->instanceLength = 0;
                 iproc->length = iproc->packet_size - iproc->rxPacketLength;
             }
             iproc->timestampLength = (iproc->type & UAVTALK_TIMESTAMPED) ? 2 : 0;
         }
 
+        // Message always contain an instance ID
+        iproc->instanceLength = 2;
+
         // Check length and determine next state
         if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) {
             connection->stats.rxErrors++;
@@ -435,28 +439,9 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle
             break;
         }
 
-        iproc->instId = 0;
-        if (iproc->type == UAVTALK_TYPE_NACK) {
-            // If this is a NACK, we skip to Checksum
-            iproc->state = UAVTALK_STATE_CS;
-        }
-        // Check if this is a single instance object (i.e. if the instance ID field is coming next)
-        else if ((iproc->obj != 0) && !UAVObjIsSingleInstance(iproc->obj)) {
-            iproc->state = UAVTALK_STATE_INSTID;
-        }
-        // Check if this is a single instance and has a timestamp in it
-        else if ((iproc->obj != 0) && (iproc->type & UAVTALK_TIMESTAMPED)) {
-            iproc->timestamp = 0;
-            iproc->state     = UAVTALK_STATE_TIMESTAMP;
-        } else {
-            // If there is a payload get it, otherwise receive checksum
-            if (iproc->length > 0) {
-                iproc->state = UAVTALK_STATE_DATA;
-            } else {
-                iproc->state = UAVTALK_STATE_CS;
-            }
-        }
         iproc->rxCount = 0;
+        iproc->instId = 0;
+        iproc->state = UAVTALK_STATE_INSTID;
 
         break;
 
@@ -713,8 +698,12 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
     // Lock
     xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
 
-    // Get the handle to the Object. Will be zero
+    // Get the handle to the Object.
-    // if object does not exist.
+    // Will be zero if object does not exist.
+    // Warning :
+    // Here we ask for instance ID 0 without taking into account the provided instId
+    // The provided instId will be used later when packing, unpacking, etc...
+    // TODO the above should be fixed as it is cumbersome and error prone
     obj = UAVObjGetByID(objId);
 
     // Process message type
@@ -724,9 +713,15 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
         // All instances, not allowed for OBJ messages
         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
             // Unpack object, if the instance does not exist it will be created!
-            UAVObjUnpack(obj, instId, data);
+            if (UAVObjUnpack(obj, instId, data)) {
-            // Check if an ack is pending
+                // Check if an ack is pending OBJ_ACK below
-            updateAck(connection, obj, instId);
+                // TODO is it necessary to do that check?
+                // TODO if yes, why is the same check not done for OBJ_ACK below?
+                updateAck(connection, obj, instId);
+            }
+            else {
+                ret = -1;
+            }
         } else {
             ret = -1;
         }
@@ -738,22 +733,30 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
             // Unpack object, if the instance does not exist it will be created!
             if (UAVObjUnpack(obj, instId, data) == 0) {
-                // Transmit ACK
+                // Object updated or created, transmit ACK
-                sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
+                sendObject(connection, UAVTALK_TYPE_ACK, objId, instId, NULL);
             } else {
                 ret = -1;
             }
         } else {
             ret = -1;
         }
+        if (ret == -1) {
+            // failed to update object, transmit NACK
+            sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
+        }
         break;
     case UAVTALK_TYPE_OBJ_REQ:
-        // Send requested object if message is of type OBJ_REQ
+        // Check if requested object exists
-        if (obj == 0) {
+        if (obj && ((instId == UAVOBJ_ALL_INSTANCES) || instId < UAVObjGetNumInstances(obj))) {
-            // Transmit NACK
+            // Object found, transmit it
-            sendNack(connection, objId);
+            sendObject(connection, UAVTALK_TYPE_OBJ, objId, instId, obj);
         } else {
-            sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
+            ret = -1;
+        }
+        if (ret == -1) {
+            // failed to send object, transmit NACK
+            sendObject(connection, UAVTALK_TYPE_NACK, objId, instId, NULL);
         }
         break;
     case UAVTALK_TYPE_NACK:
@@ -762,7 +765,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
     case UAVTALK_TYPE_ACK:
         // All instances, not allowed for ACK messages
         if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
-            // Check if an ack is pending
+            // Check if an ACK is pending
             updateAck(connection, obj, instId);
         } else {
             ret = -1;
@@ -796,19 +799,22 @@ static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint1
 /**
  * Send an object through the telemetry link.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object handle to send
- * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
  * \param[in] type Transaction type
+ * \param[in] objId The object ID
+ * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
+ * \param[in] obj Object handle to send (null when type is NACK)
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
+static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 {
     uint32_t numInst;
     uint32_t n;
 
+    // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
+
     // If all instances are requested and this is a single instance object, force instance ID to zero
-    if (instId == UAVOBJ_ALL_INSTANCES && UAVObjIsSingleInstance(obj)) {
+    if ((obj != NULL) && (instId == UAVOBJ_ALL_INSTANCES) && UAVObjIsSingleInstance(obj)) {
         instId = 0;
     }
 
@@ -819,17 +825,17 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u
             numInst = UAVObjGetNumInstances(obj);
             // Send all instances
             for (n = 0; n < numInst; ++n) {
-                sendSingleObject(connection, obj, n, type);
+                sendSingleObject(connection, type, objId, n, obj);
             }
             return 0;
         } else {
-            return sendSingleObject(connection, obj, instId, type);
+            return sendSingleObject(connection, type, objId, instId, obj);
         }
     } else if (type == UAVTALK_TYPE_OBJ_REQ) {
-        return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ);
+        return sendSingleObject(connection, UAVTALK_TYPE_OBJ_REQ, objId, instId, obj);
-    } else if (type == UAVTALK_TYPE_ACK) {
+    } else if (type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
         if (instId != UAVOBJ_ALL_INSTANCES) {
-            return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
+            return sendSingleObject(connection, type, objId, instId, obj);
         } else {
             return -1;
         }
@@ -841,24 +847,26 @@ static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, u
 /**
  * Send an object through the telemetry link.
  * \param[in] connection UAVTalkConnection to be used
- * \param[in] obj Object handle to send
- * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead)
  * \param[in] type Transaction type
+ * \param[in] objId The object ID
+ * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use
+() instead)
+ * \param[in] obj Object handle to send (null when type is NACK)
  * \return 0 Success
  * \return -1 Failure
  */
-static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
+static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj)
 {
     int32_t length;
     int32_t dataOffset;
-    uint32_t objId;
+
+    // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
 
     if (!connection->outStream) {
         return -1;
     }
 
     // Setup type and object id fields
-    objId = UAVObjGetID(obj);
     connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
     connection->txBuffer[1] = type;
     // data length inserted here below
@@ -866,15 +874,12 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle
     connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
     connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
     connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
+    dataOffset = 8;
 
-    // Setup instance ID if one is required
+    // The instance ID is always sent
-    if (UAVObjIsSingleInstance(obj)) {
+    connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
-        dataOffset = 8;
+    connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
-    } else {
+    dataOffset += 2;
-        connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
-        connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
-        dataOffset = 10;
-    }
 
     // Add timestamp when the transaction type is appropriate
     if (type & UAVTALK_TIMESTAMPED) {
@@ -885,7 +890,7 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle
     }
 
     // Determine data length
-    if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK) {
+    if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) {
         length = 0;
     } else {
         length = UAVObjGetNumBytes(obj);
@@ -924,50 +929,6 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle
     return 0;
 }
 
-/**
- * Send a NACK through the telemetry link.
- * \param[in] connection UAVTalkConnection to be used
- * \param[in] objId Object ID to send a NACK for
- * \return 0 Success
- * \return -1 Failure
- */
-static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
-{
-    int32_t dataOffset;
-
-    if (!connection->outStream) {
-        return -1;
-    }
-
-    connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
-    connection->txBuffer[1] = UAVTALK_TYPE_NACK;
-    // data length inserted here below
-    connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
-    connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
-    connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
-    connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
-
-    dataOffset = 8;
-
-    // Store the packet length
-    connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
-    connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
-
-    // Calculate checksum
-    connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset);
-
-    uint16_t tx_msg_len = dataOffset + UAVTALK_CHECKSUM_LENGTH;
-    int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len);
-
-    if (rc == tx_msg_len) {
-        // Update stats
-        connection->stats.txBytes += tx_msg_len;
-    }
-
-    // Done
-    return 0;
-}
-
 /**
  * @}
  * @}

ground/openpilotgcs/src/plugins/uavobjects/uavobject.cpp

@@ -436,6 +436,7 @@ QString UAVObject::toString()
     QString sout;
 
     sout.append(toStringBrief());
+    sout.append('\n');
     sout.append(toStringData());
     return sout;
 }
@@ -447,12 +448,12 @@ QString UAVObject::toStringBrief()
 {
     QString sout;
 
-    sout.append(QString("%1 (ID: %2, InstID: %3, NumBytes: %4, SInst: %5)\n")
+    sout.append(QString("%1 (ID: %2:%3, %4bytes, %5)")
                 .arg(getName())
                 .arg(getObjID())
                 .arg(getInstID())
                 .arg(getNumBytes())
-                .arg(isSingleInstance()));
+                .arg(isSingleInstance() ? "single" : "multiple"));
     return sout;
 }
 

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

@@ -65,9 +65,8 @@ Telemetry::Telemetry(UAVTalk *utalk, UAVObjectManager *objMngr)
 
 Telemetry::~Telemetry()
 {
-    for (QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.begin(); itr != transMap.end(); ++itr) {
+    closeAllTransactions();
-        delete itr.value();
+    
-    }
 }
 
 /**
@@ -122,6 +121,7 @@ void Telemetry::setUpdatePeriod(UAVObject *obj, qint32 periodMs)
  */
 void Telemetry::connectToObjectInstances(UAVObject *obj, quint32 eventMask)
 {
+ 	// TODO why connect systematically to all instances?
     QList<UAVObject *> objs = objMngr->getObjectInstances(obj->getObjID());
     for (int n = 0; n < objs.length(); ++n) {
         // Disconnect all
@@ -211,21 +211,25 @@ void Telemetry::updateObject(UAVObject *obj, quint32 eventType)
 void Telemetry::transactionCompleted(UAVObject *obj, bool success)
 {
     // Lookup the transaction in the transaction map.
-    quint32 objId = obj->getObjID();
+    ObjectTransactionInfo *transInfo = findTransaction(obj);
-
+    if (transInfo) {
-    QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objId);
-    if (itr != transMap.end()) {
-        ObjectTransactionInfo *transInfo = itr.value();
         // Remove this transaction as it's complete.
         transInfo->timer->stop();
-        transMap.remove(objId);
+        closeTransaction(transInfo);
         delete transInfo;
         // Send signal
+        if (success) {
+#ifdef VERBOSE_TELEMETRY
+            qDebug() << "Telemetry - transaction successful for object" << obj->toStringBrief();
+#endif
+        } else {
+            qWarning() << "Telemetry - !!! transaction failed for object" << obj->toStringBrief();
+        }
         obj->emitTransactionCompleted(success);
         // Process new object updates from queue
         processObjectQueue();
     } else {
-        qDebug() << "Error: received a transaction completed when did not expect it.";
+        qWarning() << "Telemetry - Error: received a transaction completed when did not expect it for" << obj->toStringBrief();
     }
 }
 
@@ -237,6 +241,9 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
     transInfo->timer->stop();
     // Check if more retries are pending
     if (transInfo->retriesRemaining > 0) {
+#ifdef VERBOSE_TELEMETRY
+        qDebug() << QString("Telemetry - transaction timed out for object %1, retrying...").arg(transInfo->obj->toStringBrief());
+#endif
         --transInfo->retriesRemaining;
         processObjectTransaction(transInfo);
         ++txRetries;
@@ -246,9 +253,11 @@ void Telemetry::transactionTimeout(ObjectTransactionInfo *transInfo)
         // Terminate transaction
         utalk->cancelTransaction(transInfo->obj);
         // Send signal
+        qWarning() << QString("Telemetry - !!! transaction timed out for object %1").arg(transInfo->obj->toStringBrief());
         transInfo->obj->emitTransactionCompleted(false);
         // Remove this transaction as it's complete.
-        transMap.remove(transInfo->obj->getObjID());
+        // FIXME : also remove transaction from UAVTalk transaction map
+        closeTransaction(transInfo);
         delete transInfo;
         // Process new object updates from queue
         processObjectQueue();
@@ -263,8 +272,14 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
 {
     // Initiate transaction
     if (transInfo->objRequest) {
+#ifdef VERBOSE_TELEMETRY
+        qDebug() << QString("Telemetry - sending object request for %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : "");
+#endif
         utalk->sendObjectRequest(transInfo->obj, transInfo->allInstances);
     } else {
+#ifdef VERBOSE_TELEMETRY
+        qDebug() << QString("Telemetry - sending object %1 - %2").arg(transInfo->obj->toStringBrief()).arg(transInfo->acked ? "acked" : "");
+#endif
         utalk->sendObject(transInfo->obj, transInfo->acked, transInfo->allInstances);
     }
     // Start timer if a response is expected
@@ -272,7 +287,7 @@ void Telemetry::processObjectTransaction(ObjectTransactionInfo *transInfo)
         transInfo->timer->start(REQ_TIMEOUT_MS);
     } else {
         // Otherwise, remove this transaction as it's complete.
-        transMap.remove(transInfo->obj->getObjID());
+        closeTransaction(transInfo);
         delete transInfo;
     }
 }
@@ -293,14 +308,15 @@ void Telemetry::processObjectUpdates(UAVObject *obj, EventMask event, bool allIn
             objPriorityQueue.enqueue(objInfo);
         } else {
             ++txErrors;
+            qWarning() << QString("Telemetry - !!! priority event queue is full, event lost (%1)").arg(obj->toStringBrief());
             obj->emitTransactionCompleted(false);
-            qDebug() << tr("Telemetry: priority event queue is full, event lost (%1)").arg(obj->getName());
         }
     } else {
         if (objQueue.length() < MAX_QUEUE_SIZE) {
             objQueue.enqueue(objInfo);
         } else {
             ++txErrors;
+            qWarning() << QString("Telemetry - !!! event queue is full, event lost (%1)").arg(obj->toStringBrief());
             obj->emitTransactionCompleted(false);
         }
     }
@@ -340,9 +356,10 @@ void Telemetry::processObjectQueue()
     UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
     UAVObject::UpdateMode updateMode = UAVObject::GetGcsTelemetryUpdateMode(metadata);
     if ((objInfo.event != EV_UNPACKED) && ((objInfo.event != EV_UPDATED_PERIODIC) || (updateMode != UAVObject::UPDATEMODE_THROTTLED))) {
-        QMap<quint32, ObjectTransactionInfo *>::iterator itr = transMap.find(objInfo.obj->getObjID());
+        if (findTransaction(objInfo.obj)) {
-        if (itr != transMap.end()) {
+            qWarning() << QString("Telemetry - !!! Making request for an object: %1 for which a request is already in progress").arg(objInfo.obj->toStringBrief());
-            qDebug() << "!!!!!! Making request for an object: " << objInfo.obj->getName() << " for which a request is already in progress!!!!!!";
+            objInfo.obj->emitTransactionCompleted(false);
+            return;
         }
         UAVObject::Metadata metadata     = objInfo.obj->getMetadata();
         ObjectTransactionInfo *transInfo = new ObjectTransactionInfo(this);
@@ -357,7 +374,7 @@ void Telemetry::processObjectQueue()
         }
         transInfo->telem = this;
         // Insert the transaction into the transaction map.
-        transMap.insert(objInfo.obj->getObjID(), transInfo);
+        openTransaction(transInfo);
         processObjectTransaction(transInfo);
     }
 
@@ -396,6 +413,7 @@ void Telemetry::processPeriodicUpdates()
     qint32 elapsedMs = 0;
     QTime time;
     qint32 offset;
+    bool allInstances;
     for (int n = 0; n < objList.length(); ++n) {
         objinfo = &objList[n];
         // If object is configured for periodic updates
@@ -408,7 +426,8 @@ void Telemetry::processPeriodicUpdates()
                 objinfo->timeToNextUpdateMs = objinfo->updatePeriodMs - offset;
                 // Send object
                 time.start();
-                processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, true, false);
+                allInstances = !objinfo->obj->isSingleInstance();
+                processObjectUpdates(objinfo->obj, EV_UPDATED_PERIODIC, allInstances, false);
                 elapsedMs = time.elapsed();
                 // Update timeToNextUpdateMs with the elapsed delay of sending the object;
                 timeToNextUpdateMs += elapsedMs;
@@ -496,7 +515,6 @@ void Telemetry::objectUnpacked(UAVObject *obj)
 void Telemetry::updateRequested(UAVObject *obj)
 {
     QMutexLocker locker(mutex);
-
     processObjectUpdates(obj, EV_UPDATE_REQ, false, true);
 }
 
@@ -514,6 +532,51 @@ void Telemetry::newInstance(UAVObject *obj)
     registerObject(obj);
 }
 
+ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
+{
+    // Lookup the transaction in the transaction map
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(obj->getObjID());
+    if (objTransactions != NULL) {
+        return objTransactions->value(obj->getInstID());
+    }
+    return NULL;
+}
+
+void Telemetry::openTransaction(ObjectTransactionInfo *trans)
+{
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(trans->obj->getObjID());
+    if (objTransactions == NULL) {
+        objTransactions = new QMap<quint32, ObjectTransactionInfo *>();
+        transMap.insert(trans->obj->getObjID(), objTransactions);
+    }
+    objTransactions->insert(trans->obj->getInstID(), trans);
+}
+
+void Telemetry::closeTransaction(ObjectTransactionInfo *trans)
+{
+    QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(trans->obj->getObjID());
+    if (objTransactions != NULL) {
+        objTransactions->remove(trans->obj->getInstID());
+        // Keep the map even if it is empty
+        // There are at most 100 different object IDs...
+    }
+}
+
+void Telemetry::closeAllTransactions()
+{
+    foreach(quint32 objId, transMap.keys()) {
+        QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(objId);
+        foreach(quint32 instId, objTransactions->keys()) {
+            ObjectTransactionInfo *trans = objTransactions->value(instId);
+            qWarning() << "Telemetry - closing active transaction for object" << trans->obj->toStringBrief();
+            objTransactions->remove(instId);
+            delete trans;
+        }
+        transMap.remove(objId);
+        delete objTransactions;
+    }
+}
+
 ObjectTransactionInfo::ObjectTransactionInfo(QObject *parent) : QObject(parent)
 {
     obj = 0;

ground/openpilotgcs/src/plugins/uavtalk/telemetry.h

@@ -118,7 +118,7 @@ private:
     QList<ObjectTimeInfo> objList;
     QQueue<ObjectQueueInfo> objQueue;
     QQueue<ObjectQueueInfo> objPriorityQueue;
-    QMap<quint32, ObjectTransactionInfo *>transMap;
+    QMap<quint32, QMap<quint32, ObjectTransactionInfo *> *> transMap;
     QMutex *mutex;
     QTimer *updateTimer;
     QTimer *statsTimer;
@@ -136,6 +136,11 @@ private:
     void processObjectTransaction(ObjectTransactionInfo *transInfo);
     void processObjectQueue();
 
+    ObjectTransactionInfo *findTransaction(UAVObject *obj);
+    void openTransaction(ObjectTransactionInfo *trans);
+    void closeTransaction(ObjectTransactionInfo *trans);
+    void closeAllTransactions();
+
 private slots:
     void objectUpdatedAuto(UAVObject *obj);
     void objectUpdatedManual(UAVObject *obj);

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp

@@ -94,6 +94,7 @@ UAVTalk::~UAVTalk()
     // According to Qt, it is not necessary to disconnect upon
     // object deletion.
     // disconnect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
+    closeAllTransactions();
 }
 
 
@@ -154,7 +155,7 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
 {
     QMutexLocker locker(mutex);
 
-    return objectTransaction(obj, TYPE_OBJ_REQ, allInstances);
+    return objectTransaction(TYPE_OBJ_REQ, obj, allInstances);
 }
 
 /**
@@ -169,9 +170,9 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
     QMutexLocker locker(mutex);
 
     if (acked) {
-        return objectTransaction(obj, TYPE_OBJ_ACK, allInstances);
+        return objectTransaction(TYPE_OBJ_ACK, obj, allInstances);
     } else {
-        return objectTransaction(obj, TYPE_OBJ, allInstances);
+        return objectTransaction(TYPE_OBJ, obj, allInstances);
     }
 }
 
@@ -181,15 +182,14 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
 void UAVTalk::cancelTransaction(UAVObject *obj)
 {
     QMutexLocker locker(mutex);
-    quint32 objId = obj->getObjID();
 
     if (io.isNull()) {
         return;
     }
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+    Transaction *trans = findTransaction(obj);
-    if (itr != transMap.end()) {
+    if (trans != NULL) {
-        transMap.remove(objId);
+        closeTransaction(trans);
-        delete itr.value();
+        delete trans;
     }
 }
 
@@ -203,21 +203,24 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
  * \param[in] allInstances If set true then all instances will be updated
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::objectTransaction(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::objectTransaction(quint8 type, UAVObject *obj, bool allInstances)
 {
+	Q_ASSERT(obj);
     // Send object depending on if a response is needed
+    // transactions of TYPE_OBJ_REQ are acked by the response
+    quint16 instId = allInstances ? ALL_INSTANCES : obj->getInstID();
     if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
-        if (transmitObject(obj, type, allInstances)) {
+        if (transmitObject(type, obj->getObjID(), instId, obj)) {
             Transaction *trans = new Transaction();
             trans->obj = obj;
             trans->allInstances = allInstances;
-            transMap.insert(obj->getObjID(), trans);
+            openTransaction(trans);
             return true;
         } else {
             return false;
         }
     } else if (type == TYPE_OBJ) {
-        return transmitObject(obj, TYPE_OBJ, allInstances);
+        return transmitObject(type, obj->getObjID(), instId, obj);
     } else {
         return false;
     }
@@ -233,7 +236,8 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
     // Update stats
     stats.rxBytes++;
 
-    rxPacketLength++; // update packet byte count
+    // update packet byte count
+    rxPacketLength++;
 
     if (useUDPMirror) {
         rxDataArray.append(rxbyte);
@@ -332,12 +336,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
             // Determine data length
             if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) {
                 rxLength = 0;
-                rxInstanceLength = 0;
             } else {
                 rxLength = rxObj->getNumBytes();
-                rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2);
             }
 
+            // The instance ID is always sent
+            rxInstanceLength = 2;
+
             // Check length and determine next state
             if (rxLength >= MAX_PAYLOAD_LENGTH) {
                 stats.rxErrors++;
@@ -347,39 +352,18 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
             }
 
             // Check the lengths match
-            if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { // packet error - mismatched packet size
+            if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) {
+                // packet error - mismatched packet size
                 stats.rxErrors++;
                 rxState = STATE_SYNC;
                 UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)");
                 break;
             }
 
-            // Check if this is a single instance object (i.e. if the instance ID field is coming next)
+            rxCount = 0;
-            if (rxObj == NULL) {
+            rxInstId = 0;
-                // This is a non-existing object, just skip to checksum
+            rxState = STATE_INSTID;
-                // and we'll send a NACK next.
-                rxState  = STATE_CS;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)");
-                rxInstId = 0;
-                rxCount  = 0;
-            } else if (rxObj->isSingleInstance()) {
-                // If there is a payload get it, otherwise receive checksum
-                if (rxLength > 0) {
-                    rxState = STATE_DATA;
-                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)");
-                } else {
-                    rxState = STATE_CS;
-                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum");
-                }
-                rxInstId = 0;
-                rxCount  = 0;
-            } else {
-                rxState = STATE_INSTID;
-                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID");
-                rxCount = 0;
-            }
         }
-
         break;
 
     case STATE_INSTID:
@@ -428,14 +412,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
         // The CRC byte
         rxCSPacket = rxbyte;
 
-        if (rxCS != rxCSPacket) { // packet error - faulty CRC
+        if (rxCS != rxCSPacket) {
+            // packet error - faulty CRC
             stats.rxErrors++;
             rxState = STATE_SYNC;
             UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)");
             break;
         }
 
-        if (rxPacketLength != packetSize + 1) { // packet error - mismatched packet size
+        if (rxPacketLength != packetSize + 1) {
+            // packet error - mismatched packet size
             stats.rxErrors++;
             rxState = STATE_SYNC;
             UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)");
@@ -443,12 +429,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
         }
 
         mutex->lock();
+
         receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
+
         if (useUDPMirror) {
             udpSocketTx->writeDatagram(rxDataArray, QHostAddress::LocalHost, udpSocketRx->localPort());
         }
+
         stats.rxObjectBytes += rxLength;
         stats.rxObjects++;
+
         mutex->unlock();
 
         rxState = STATE_SYNC;
@@ -456,6 +446,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
         break;
 
     default:
+
         rxState = STATE_SYNC;
         stats.rxErrors++;
         UAVTALK_QXTLOG_DEBUG("UAVTalk: \?\?\?->Sync"); // Use the escape character for '?' so that the tripgraph isn't triggered.
@@ -488,8 +479,13 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
         if (!allInstances) {
             // Get object and update its data
             obj = updateObject(objId, instId, data);
-            // Check if an ack is pending
+#ifdef VERBOSE_UAVTALK
+            qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
+                // Check if an ACK is pending
+                // TODO is it necessary to do that check?
+                // TODO if yes, why is the same check not done for OBJ_ACK below?
                 updateAck(obj);
             } else {
                 error = true;
@@ -503,40 +499,55 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
         if (!allInstances) {
             // Get object and update its data
             obj = updateObject(objId, instId, data);
-            // Transmit ACK
+#ifdef VERBOSE_UAVTALK
+            qDebug() << "UAVTalk - received object (acked)" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
-                transmitObject(obj, TYPE_ACK, false);
+                // Object updated or created, transmit ACK
+                transmitObject(TYPE_ACK, objId, instId, obj);
             } else {
                 error = true;
             }
         } else {
             error = true;
         }
+        if (error) {
+            // failed to update object, transmit NACK
+            transmitObject(TYPE_NACK, objId, instId, NULL);
+        }
         break;
     case TYPE_OBJ_REQ:
-        // Get object, if all instances are requested get instance 0 of the object
+        // Check if requested object exists
         if (allInstances) {
+            // All instances, so get instance zero
             obj = objMngr->getObject(objId);
         } else {
             obj = objMngr->getObject(objId, instId);
         }
-        // If object was found transmit it
+#ifdef VERBOSE_UAVTALK
+        qDebug() << "UAVTalk - received object request" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
         if (obj != NULL) {
-            transmitObject(obj, TYPE_OBJ, allInstances);
+            // Object found, transmit it
+            transmitObject(TYPE_OBJ, objId, instId, obj);
         } else {
-            // Object was not found, transmit a NACK with the
-            // objId which was not found.
-            transmitNack(objId);
             error = true;
         }
+        if (error) {
+            // failed to send object, transmit NACK
+            transmitObject(TYPE_NACK, objId, instId, NULL);
+        }
         break;
     case TYPE_NACK:
         // All instances, not allowed for NACK messages
         if (!allInstances) {
             // Get object
             obj = objMngr->getObject(objId, instId);
-            // Check if object exists:
+#ifdef VERBOSE_UAVTALK
+            qDebug() << "UAVTalk - received nack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
+                // Check if a NACK is pending
                 updateNack(obj);
             } else {
                 error = true;
@@ -548,8 +559,11 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
         if (!allInstances) {
             // Get object
             obj = objMngr->getObject(objId, instId);
-            // Check if an ack is pending
+#ifdef VERBOSE_UAVTALK
+            qDebug() << "UAVTalk - received ack" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
             if (obj != NULL) {
+                // Check if an ACK is pending
                 updateAck(obj);
             } else {
                 error = true;
@@ -559,6 +573,9 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
     default:
         error = true;
     }
+    if (error) {
+        qWarning() << "UAVTalk - !!! error receiving object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+    }
     // Done
     return !error;
 }
@@ -608,65 +625,70 @@ void UAVTalk::updateNack(UAVObject *obj)
     if (!obj) {
         return;
     }
-    quint32 objId = obj->getObjID();
+    Transaction *trans = findTransaction(obj);
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+    // TODO handle allInstances
-    if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
+    if (trans != NULL /* || itr.value()->allInstances)*/) {
-        transMap.remove(objId);
+        closeTransaction(trans);
-        delete itr.value();
+        delete trans;
         emit transactionCompleted(obj, false);
     }
 }
 
-
 /**
  * Check if a transaction is pending and if yes complete it.
  */
 void UAVTalk::updateAck(UAVObject *obj)
 {
-    quint32 objId = obj->getObjID();
+    Q_ASSERT(obj);
-
+    if (!obj) {
-    QMap<quint32, Transaction *>::iterator itr = transMap.find(objId);
+        return;
-    if (itr != transMap.end() && (itr.value()->obj->getInstID() == obj->getInstID() || itr.value()->allInstances)) {
+    }
-        transMap.remove(objId);
+    Transaction *trans = findTransaction(obj);
-        delete itr.value();
+    // TODO handle allInstances
+    if (trans != NULL /* || itr.value()->allInstances)*/) {
+        closeTransaction(trans);
+        delete trans;
         emit transactionCompleted(obj, true);
     }
 }
 
-
 /**
  * Send an object through the telemetry link.
- * \param[in] obj Object to send
  * \param[in] type Transaction type
- * \param[in] allInstances True is all instances of the object are to be sent
+ * \param[in] objId Object ID to send
+ * \param[in] instId Instance ID to send
+ * \param[in] obj Object to send (null when type is NACK)
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
+    // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences.
+
     // If all instances are requested on a single instance object it is an error
-    if (allInstances && obj->isSingleInstance()) {
+    if ((obj != NULL) && (instId == ALL_INSTANCES) && obj->isSingleInstance()) {
-        allInstances = false;
+        instId = 0;
     }
+    bool allInstances = (instId == ALL_INSTANCES);
 
     // Process message type
     if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
         if (allInstances) {
             // Get number of instances
-            quint32 numInst = objMngr->getNumInstances(obj->getObjID());
+            quint32 numInst = objMngr->getNumInstances(objId);
             // Send all instances
-            for (quint32 instId = 0; instId < numInst; ++instId) {
+            for (quint32 n = 0; n < numInst; ++n) {
-                UAVObject *inst = objMngr->getObject(obj->getObjID(), instId);
+                UAVObject *o = objMngr->getObject(objId, n);
-                transmitSingleObject(inst, type, false);
+                transmitSingleObject(type, objId, n, o);
             }
             return true;
         } else {
-            return transmitSingleObject(obj, type, false);
+            return transmitSingleObject(type, objId, instId, obj);
         }
     } else if (type == TYPE_OBJ_REQ) {
-        return transmitSingleObject(obj, TYPE_OBJ_REQ, allInstances);
+        return transmitSingleObject(TYPE_OBJ_REQ, objId, instId, obj);
-    } else if (type == TYPE_ACK) {
+    } else if (type == TYPE_ACK || type == TYPE_NACK) {
         if (!allInstances) {
-            return transmitSingleObject(obj, TYPE_ACK, false);
+            return transmitSingleObject(type, objId, instId, obj);
         } else {
             return false;
         }
@@ -675,78 +697,35 @@ bool UAVTalk::transmitObject(UAVObject *obj, quint8 type, bool allInstances)
     }
 }
 
-/**
- * Transmit a NACK through the telemetry link.
- * \param[in] objId the ObjectID we rejected
- */
-bool UAVTalk::transmitNack(quint32 objId)
-{
-    int dataOffset = 8;
-
-    txBuffer[0] = SYNC_VAL;
-    txBuffer[1] = TYPE_NACK;
-    qToLittleEndian<quint32>(objId, &txBuffer[4]);
-
-    // Calculate checksum
-    txBuffer[dataOffset] = updateCRC(0, txBuffer, dataOffset);
-
-    qToLittleEndian<quint16>(dataOffset, &txBuffer[2]);
-
-    // Send buffer, check that the transmit backlog does not grow above limit
-    if (io && io->isWritable() && io->bytesToWrite() < TX_BUFFER_SIZE) {
-        io->write((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH);
-        if (useUDPMirror) {
-            udpSocketRx->writeDatagram((const char *)txBuffer, dataOffset + CHECKSUM_LENGTH, QHostAddress::LocalHost, udpSocketTx->localPort());
-        }
-    } else {
-        ++stats.txErrors;
-        return false;
-    }
-
-    // Update stats
-    stats.txBytes += 8 + CHECKSUM_LENGTH;
-
-    // Done
-    return true;
-}
-
-
 /**
  * Send an object through the telemetry link.
- * \param[in] obj Object handle to send
  * \param[in] type Transaction type
+ * \param[in] objId Object ID to send
+ * \param[in] instId Instance ID to send
+ * \param[in] obj Object to send (null when type is NACK)
  * \return Success (true), Failure (false)
  */
-bool UAVTalk::transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances)
+bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
 {
+#ifdef VERBOSE_UAVTALK
+    qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
+#endif
     qint32 length;
     qint32 dataOffset;
-    quint32 objId;
-    quint16 instId;
-    quint16 allInstId = ALL_INSTANCES;
 
     // Setup type and object id fields
-    objId = obj->getObjID();
     txBuffer[0] = SYNC_VAL;
     txBuffer[1] = type;
+    // data length inserted here below
     qToLittleEndian<quint32>(objId, &txBuffer[4]);
+    dataOffset = 8;
 
-    // Setup instance ID if one is required
+    // The instance ID is always sent
-    if (obj->isSingleInstance()) {
+    qToLittleEndian<quint16>(instId, &txBuffer[8]);
-        dataOffset = 8;
+    dataOffset += 2;
-    } else {
-        // Check if all instances are requested
-        if (allInstances) {
-            qToLittleEndian<quint16>(allInstId, &txBuffer[8]);
-        } else {
-            instId = obj->getInstID();
-            qToLittleEndian<quint16>(instId, &txBuffer[8]);
-        }
-        dataOffset = 10;
-    }
 
     // Determine data length
-    if (type == TYPE_OBJ_REQ || type == TYPE_ACK) {
+    if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) {
         length = 0;
     } else {
         length = obj->getNumBytes();
@@ -811,6 +790,7 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data)
 {
     return crc_table[crc ^ data];
 }
+
 quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length)
 {
     while (length--) {
@@ -818,3 +798,48 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length)
     }
     return crc;
 }
+
+UAVTalk::Transaction *UAVTalk::findTransaction(UAVObject *obj)
+{
+    // Lookup the transaction in the transaction map
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(obj->getObjID());
+    if (objTransactions != NULL) {
+        return objTransactions->value(obj->getInstID());
+    }
+    return NULL;
+}
+
+void UAVTalk::openTransaction(Transaction *trans)
+{
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->obj->getObjID());
+    if (objTransactions == NULL) {
+        objTransactions = new QMap<quint32, Transaction *>();
+        transMap.insert(trans->obj->getObjID(), objTransactions);
+    }
+    objTransactions->insert(trans->obj->getInstID(), trans);
+}
+
+void UAVTalk::closeTransaction(Transaction *trans)
+{
+    QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->obj->getObjID());
+    if (objTransactions != NULL) {
+        objTransactions->remove(trans->obj->getInstID());
+        // Keep the map even if it is empty
+        // There are at most 100 different object IDs...
+    }
+}
+
+void UAVTalk::closeAllTransactions()
+{
+    foreach(quint32 objId, transMap.keys()) {
+        QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
+        foreach(quint32 instId, objTransactions->keys()) {
+            Transaction *trans = objTransactions->value(instId);
+            qWarning() << "UAVTalk - closing active transaction for object" << trans->obj->toStringBrief();
+            objTransactions->remove(instId);
+            delete trans;
+        }
+        transMap.remove(objId);
+        delete objTransactions;
+    }
+}

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h

@@ -93,7 +93,6 @@ private:
     static const int MAX_PACKET_LENGTH  = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
 
     static const quint16 ALL_INSTANCES  = 0xFFFF;
-    static const quint16 OBJID_NOTFOUND = 0x0000;
 
     static const int TX_BUFFER_SIZE     = 2 * 1024;
     static const quint8 crc_table[256];
@@ -105,7 +104,7 @@ private:
     QPointer<QIODevice> io;
     UAVObjectManager *objMngr;
     QMutex *mutex;
-    QMap<quint32, Transaction *> transMap;
+    QMap<quint32, QMap<quint32, Transaction *> *> transMap;
     quint8 rxBuffer[MAX_PACKET_LENGTH];
     quint8 txBuffer[MAX_PACKET_LENGTH];
     // Variables used by the receive state machine
@@ -129,17 +128,21 @@ private:
     QByteArray rxDataArray;
 
     // Methods
-    bool objectTransaction(UAVObject *obj, quint8 type, bool allInstances);
+    bool objectTransaction(quint8 type, UAVObject *obj, bool allInstances);
     bool processInputByte(quint8 rxbyte);
     bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *data, qint32 length);
     UAVObject *updateObject(quint32 objId, quint16 instId, quint8 *data);
     void updateAck(UAVObject *obj);
     void updateNack(UAVObject *obj);
-    bool transmitNack(quint32 objId);
+    bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
-    bool transmitObject(UAVObject *obj, quint8 type, bool allInstances);
+    bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
-    bool transmitSingleObject(UAVObject *obj, quint8 type, bool allInstances);
     quint8 updateCRC(quint8 crc, const quint8 data);
     quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
+
+    Transaction *findTransaction(UAVObject *obj);
+    void openTransaction(Transaction *trans);
+    void closeTransaction(Transaction *trans);
+    void closeAllTransactions();
 };
 
 #endif // UAVTALK_H

ground/openpilotgcs/src/plugins/uavtalk/uavtalk.pro

@@ -1,18 +1,29 @@
-QT += network
 TEMPLATE = lib
 TARGET = UAVTalk
+
+QT += network
+
+DEFINES += UAVTALK_LIBRARY
+
+#DEFINES += VERBOSE_TELEMETRY
+#DEFINES += VERBOSE_UAVTALK
+
 include(../../openpilotgcsplugin.pri)
 include(uavtalk_dependencies.pri)
-HEADERS += uavtalk.h \
+
+HEADERS += \
+    uavtalk.h \
     uavtalkplugin.h \
     telemetrymonitor.h \
     telemetrymanager.h \
     uavtalk_global.h \
     telemetry.h
-SOURCES += uavtalk.cpp \
+
+SOURCES += \
+    uavtalk.cpp \
     uavtalkplugin.cpp \
     telemetrymonitor.cpp \
     telemetrymanager.cpp \
     telemetry.cpp
-DEFINES += UAVTALK_LIBRARY
+
 OTHER_FILES += UAVTalk.pluginspec