From f234651d9cc496c11e42504023744e98f1f4bcc2 Mon Sep 17 00:00:00 2001 From: elafargue Date: Sat, 30 Apr 2011 16:52:07 +0200 Subject: [PATCH] Implemented NACKs on UAVTalk on the ground implementation. --- .../src/plugins/uavtalk/telemetry.cpp | 6 +- .../src/plugins/uavtalk/telemetry.h | 2 +- .../src/plugins/uavtalk/uavtalk.cpp | 84 +++++++++++++++++-- .../src/plugins/uavtalk/uavtalk.h | 7 +- 4 files changed, 89 insertions(+), 10 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp index 50366d9f7..186233ca0 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp +++ b/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 diff --git a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h b/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h index 23053e297..62210e8da 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/telemetry.h +++ b/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: diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 784048b92..0ad6f1ce0 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -273,7 +273,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) rxObjId = (qint32)qFromLittleEndian(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(0x0, &txBuffer[2]); // Length + qToLittleEndian(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. diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index f9f436fe7..b642c9a89 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/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);