Implemented NACKs on UAVTalk on the ground implementation.

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

@@ -50,7 +50,7 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr)
     connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*)));
     connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
     // Listen to transaction completions
-    connect(utalk, SIGNAL(transactionCompleted(UAVObject*)), this, SLOT(transactionCompleted(UAVObject*)));
+    connect(utalk, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool)));
     // Get GCS stats object
     gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
     // Setup transaction timer
@@ -207,7 +207,7 @@ void Telemetry::updateObject(UAVObject* obj)
 /**
  * Called when a transaction is successfully completed (uavtalk event)
  */
-void Telemetry::transactionCompleted(UAVObject* obj)
+void Telemetry::transactionCompleted(UAVObject* obj, bool success)
 {
     // Check if there is a pending transaction and the objects match
     if ( transPending && transInfo.obj->getObjID() == obj->getObjID() )
@@ -217,7 +217,7 @@ void Telemetry::transactionCompleted(UAVObject* obj)
         transTimer->stop();
         transPending = false;
         // Send signal
-        obj->emitTransactionCompleted(true);
+        obj->emitTransactionCompleted(success);
         // Process new object updates from queue
         processObjectQueue();
     } else

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

@@ -68,7 +68,7 @@ private slots:
     void newObject(UAVObject* obj);
     void newInstance(UAVObject* obj);
     void processPeriodicUpdates();
-    void transactionCompleted(UAVObject* obj);
+    void transactionCompleted(UAVObject* obj, bool success);
     void transactionTimeout();
 
 private:

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

@@ -273,7 +273,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
             rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
             {
                 UAVObject *rxObj = objMngr->getObject(rxObjId);
-                if (rxObj == NULL)
+                if (rxObj == NULL && rxType != TYPE_NACK)
                 {
                     stats.rxErrors++;
                     rxState = STATE_SYNC;
@@ -281,7 +281,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
                 }
 
                 // Determine data length
-                if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
+                if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK)
                     rxLength = 0;
                 else
                     rxLength = rxObj->getNumBytes();
@@ -303,7 +303,14 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
                 }
 
                 // Check if this is a single instance object (i.e. if the instance ID field is coming next)
-                if (rxObj->isSingleInstance())
+                if (rxType == TYPE_NACK)
+                {
+                   // If this is a NACK, just skip to checksum
+                   rxState   = STATE_CS;
+                   rxInstId = 0;
+                   rxCount = 0;
+                }
+                else if (rxObj->isSingleInstance())
                 {
                     // If there is a payload get it, otherwise receive checksum
                     if (rxLength > 0)
@@ -395,7 +402,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
 
 /**
  * Receive an object. This function process objects received through the telemetry stream.
- * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK)
+ * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
  * \param[in] obj Handle of the received object
  * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
  * \param[in] data Data buffer
@@ -471,9 +478,29 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8*
         }
         else
         {
+            // Object was not found, transmit a NACK with the
+            // objId which was not found.
+            transmitNack(objId);
             error = true;
         }
         break;
+    case TYPE_NACK:
+        // All instances, not allowed for NACK messages
+        if (!allInstances)
+        {
+            // Get object
+            obj = objMngr->getObject(objId, instId);
+            // Check if object exists:
+            if (obj != NULL)
+            {
+                updateNack(obj);
+            }
+            else
+            {
+             error = true;
+            }
+        }
+        break;
     case TYPE_ACK:
         // All instances, not allowed for ACK messages
         if (!allInstances)
@@ -539,6 +566,19 @@ UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data)
     }
 }
 
+/**
+ * Check if a transaction is pending and if yes complete it.
+ */
+void UAVTalk::updateNack(UAVObject* obj)
+{
+    if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
+    {
+        respObj = NULL;
+        emit transactionCompleted(obj, false);
+    }
+}
+
+
 /**
  * Check if a transaction is pending and if yes complete it.
  */
@@ -547,10 +587,11 @@ void UAVTalk::updateAck(UAVObject* obj)
     if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
     {
         respObj = NULL;
-        emit transactionCompleted(obj);
+        emit transactionCompleted(obj, true);
     }
 }
 
+
 /**
  * Send an object through the telemetry link.
  * \param[in] obj Object to send
@@ -607,6 +648,39 @@ 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)
+{
+    txBuffer[0] = SYNC_VAL;
+    txBuffer[1] = TYPE_NACK;
+    qToLittleEndian<quint16>(0x0, &txBuffer[2]); // Length
+    qToLittleEndian<quint32>(objId, &txBuffer[4]);
+
+    // Calculate checksum
+    txBuffer[8] = updateCRC(0, txBuffer, 8);
+
+    // Send buffer, check that the transmit backlog does not grow above limit
+    if ( io->bytesToWrite() < TX_BUFFER_SIZE )
+    {
+        io->write((const char*)txBuffer, 8+CHECKSUM_LENGTH);
+    }
+    else
+    {
+        ++stats.txErrors;
+        return false;
+    }
+
+    // Update stats
+    stats.txBytes += 8+CHECKSUM_LENGTH;
+
+    // Done
+    return true;
+
+}
+
 
 /**
  * Send an object through the telemetry link.

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

@@ -59,7 +59,7 @@ public:
     void resetStats();
 
 signals:
-    void transactionCompleted(UAVObject* obj);
+    void transactionCompleted(UAVObject* obj, bool success);
 
 private slots:
     void processInputStream(void);
@@ -72,6 +72,7 @@ private:
     static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
     static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
     static const int TYPE_ACK = (TYPE_VER | 0x03);
+    static const int TYPE_NACK = (TYPE_VER | 0x04);
 
     static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4)
     static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
@@ -83,6 +84,8 @@ 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];
 
@@ -117,6 +120,8 @@ private:
     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(UAVObject* obj, quint8 type, bool allInstances);
     bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
     quint8 updateCRC(quint8 crc, const quint8 data);