From 50a0a4f512a4b3646c41de175c3b9a9b35cd37ec Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Thu, 28 Nov 2013 23:42:08 +0100 Subject: [PATCH] OP-1122 OP-1125 uavtalk - minor OPReview-593 related cleanups --- flight/uavtalk/inc/uavtalk_priv.h | 1 - flight/uavtalk/uavtalk.c | 48 +++++++++---------- .../src/plugins/uavtalk/uavtalk.cpp | 40 +++++++++------- .../src/plugins/uavtalk/uavtalk.h | 3 +- 4 files changed, 50 insertions(+), 42 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 310f9e351..4b3393c9d 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -63,7 +63,6 @@ typedef struct { uint32_t objId; uint16_t instId; uint32_t length; - uint8_t instanceLength; uint8_t timestampLength; uint8_t cs; uint16_t timestamp; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 1dcfd8242..ed184a18b 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -32,6 +32,8 @@ #include "openpilot.h" #include "uavtalk_priv.h" +// Size of instance ID (2 bytes) +#define UAVTALK_INSTANCE_LENGTH 2 // Private functions static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type, UAVObjHandle obj, uint16_t instId, int32_t timeout); @@ -422,23 +424,23 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle 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) { + // packet error - exceeded payload max length connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } // Check the lengths match - if ((iproc->rxPacketLength + iproc->instanceLength + iproc->timestampLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size + if ((iproc->rxPacketLength + UAVTALK_INSTANCE_LENGTH + iproc->timestampLength + iproc->length) != iproc->packet_size) { + // packet error - mismatched packet size connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; break; } + // Message always contain an instance ID iproc->rxCount = 0; iproc->instId = 0; iproc->state = UAVTALK_STATE_INSTID; @@ -585,25 +587,23 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // Lock xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY); - // Setup type and object id fields - outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + // Setup sync byte + outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; + // Setup type outConnection->txBuffer[1] = inIproc->type; - // data length inserted here below - int32_t dataOffset = 8; + // next 2 bytes are reserved for data length (inserted here later) + int32_t dataOffset = 4; if (inIproc->objId != 0) { + // Setup object ID outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF); outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF); outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF); outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF); - // Setup instance ID if one is required - if (inIproc->instanceLength > 0) { - outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); - outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); - dataOffset = 10; - } - } else { - dataOffset = 4; + // Setup instance ID + outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); + outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); + dataOffset = 10; } // Add timestamp when the transaction type is appropriate @@ -860,26 +860,26 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, int32_t length; int32_t dataOffset; - // Important note : obj can be null (when type is NACK for example) so protect all obj dereferences. + // IMPORTANT : obj can be null (when type is NACK for example) if (!connection->outStream) { return -1; } - // Setup type and object id fields - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + // Setup sync byte + connection->txBuffer[0] = UAVTALK_SYNC_VAL; + // Setup type connection->txBuffer[1] = type; - // data length inserted here below + // next 2 bytes are reserved for data length (inserted here later) + // Setup object ID 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; - - // The instance ID is always sent + // Setup instance ID connection->txBuffer[8] = (uint8_t)(instId & 0xFF); connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset += 2; + dataOffset = 10; // Add timestamp when the transaction type is appropriate if (type & UAVTALK_TIMESTAMPED) { diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index 9df149d34..f10f58dc0 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -248,6 +248,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) case STATE_SYNC: if (rxbyte != SYNC_VAL) { + // continue until synch byte is matched UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte, 2, 16) + ")"); break; } @@ -337,14 +338,16 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK) { rxLength = 0; } else { - rxLength = rxObj->getNumBytes(); + if (rxObj) { + rxLength = rxObj->getNumBytes(); + } else { + rxLength = packetSize - rxPacketLength; + } } - // The instance ID is always sent - rxInstanceLength = 2; - // Check length and determine next state if (rxLength >= MAX_PAYLOAD_LENGTH) { + // packet error - exceeded payload max length stats.rxErrors++; rxState = STATE_SYNC; UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)"); @@ -352,7 +355,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) } // Check the lengths match - if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize) { + if ((rxPacketLength + INSTANCE_LENGTH + rxLength) != packetSize) { // packet error - mismatched packet size stats.rxErrors++; rxState = STATE_SYNC; @@ -360,6 +363,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) break; } + // Message always contain an instance ID rxCount = 0; rxInstId = 0; rxState = STATE_INSTID; @@ -707,22 +711,25 @@ bool UAVTalk::transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObje */ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj) { -#ifdef VERBOSE_UAVTALK - qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); -#endif qint32 length; qint32 dataOffset; - // Setup type and object id fields - txBuffer[0] = SYNC_VAL; - txBuffer[1] = type; - // data length inserted here below - qToLittleEndian(objId, &txBuffer[4]); - dataOffset = 8; + #ifdef VERBOSE_UAVTALK + qDebug() << "UAVTalk - transmitting object" << objId << instId << (obj != NULL ? obj->toStringBrief() : ""); +#endif - // The instance ID is always sent + // IMPORTANT : obj can be null (when type is NACK for example) + + // Setup sync byte + txBuffer[0] = SYNC_VAL; + // Setup type + txBuffer[1] = type; + // next 2 bytes are reserved for data length (inserted here later) + // Setup object ID + qToLittleEndian(objId, &txBuffer[4]); + // Setup instance ID qToLittleEndian(instId, &txBuffer[8]); - dataOffset += 2; + dataOffset = 10; // Determine data length if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) { @@ -743,6 +750,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U } } + // Store the packet length qToLittleEndian(dataOffset + length, &txBuffer[2]); // Calculate checksum diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h index c112a008c..8b27fa306 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h @@ -94,6 +94,8 @@ private: static const quint16 ALL_INSTANCES = 0xFFFF; + static const quint8 INSTANCE_LENGTH = 2; + static const int TX_BUFFER_SIZE = 2 * 1024; static const quint8 crc_table[256]; @@ -114,7 +116,6 @@ private: quint16 rxInstId; quint16 rxLength; quint16 rxPacketLength; - quint8 rxInstanceLength; quint8 rxCSPacket, rxCS; qint32 rxCount;