diff --git a/flight/uavobjects/uavobjectmanager.c b/flight/uavobjects/uavobjectmanager.c index 7f229bd2c..d350a9d53 100644 --- a/flight/uavobjects/uavobjectmanager.c +++ b/flight/uavobjects/uavobjectmanager.c @@ -1678,8 +1678,7 @@ xSemaphoreGiveRecursive(mutex); /** * Send a triggered event to all event queues registered on the object. */ -static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, - UAVObjEventType triggered_event) +static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, UAVObjEventType triggered_event) { /* Set up the message that will be sent to all registered listeners */ UAVObjEvent msg = { @@ -1692,14 +1691,13 @@ static int32_t sendEvent(struct UAVOBase *obj, uint16_t instId, struct ObjectEventEntry *event; LL_FOREACH(obj->next_event, event) { - if (event->eventMask == 0 - || (event->eventMask & triggered_event) != 0) { + if (event->eventMask == 0 || (event->eventMask & triggered_event) != 0) { // Send to queue if a valid queue is registered if (event->queue) { // will not block if (xQueueSend(event->queue, &msg, 0) != pdTRUE) { - stats.lastQueueErrorID = UAVObjGetID(obj); ++stats.eventQueueErrors; + stats.lastQueueErrorID = UAVObjGetID(obj); } } diff --git a/flight/uavtalk/inc/uavtalk_priv.h b/flight/uavtalk/inc/uavtalk_priv.h index 9225179d9..e853c6002 100644 --- a/flight/uavtalk/inc/uavtalk_priv.h +++ b/flight/uavtalk/inc/uavtalk_priv.h @@ -72,7 +72,6 @@ typedef struct { UAVTalkStats stats; UAVTalkInputProcessor iproc; uint8_t *rxBuffer; - uint32_t txSize; uint8_t *txBuffer; } UAVTalkConnectionData; diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 848b34e87..16bf7a029 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -156,7 +156,7 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) statsOut->txObjects += connection->stats.txObjects; statsOut->rxObjects += connection->stats.rxObjects; statsOut->txErrors += connection->stats.txErrors; - statsOut->txErrors += connection->stats.txErrors; + statsOut->rxErrors += connection->stats.rxErrors; // Release lock xSemaphoreGiveRecursive(connection->lock); @@ -195,7 +195,6 @@ void UAVTalkGetLastTimestamp(UAVTalkConnection connectionHandle, uint16_t *times *timestamp = iproc->timestamp; } - /** * Request an update for the specified object, on success the object data would have been * updated by the GCS. @@ -519,7 +518,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connectionHandle break; } - if (iproc->rxPacketLength != (iproc->packet_size + 1)) { + if (iproc->rxPacketLength != (iproc->packet_size + UAVTALK_CHECKSUM_LENGTH)) { // packet error - mismatched packet size connection->stats.rxErrors++; iproc->state = UAVTALK_STATE_ERROR; @@ -595,53 +594,53 @@ UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkC // Setup type outConnection->txBuffer[1] = inIproc->type; // 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 - outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); - outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); - dataOffset = 10; - } + // 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 + outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); + outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); + int32_t headerLength = 10; // Add timestamp when the transaction type is appropriate if (inIproc->type & UAVTALK_TIMESTAMPED) { portTickType time = xTaskGetTickCount(); - outConnection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; + outConnection->txBuffer[10] = (uint8_t)(time & 0xFF); + outConnection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF); + headerLength += 2; } // Copy data (if any) - memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length); + if (inIproc->length > 0) { + memcpy(&outConnection->txBuffer[headerLength], inConnection->rxBuffer, inIproc->length); + } // Store the packet length - outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF); - outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF); + outConnection->txBuffer[2] = (uint8_t)((headerLength + inIproc->length) & 0xFF); + outConnection->txBuffer[3] = (uint8_t)(((headerLength + inIproc->length) >> 8) & 0xFF); // Copy the checksum - outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs; + outConnection->txBuffer[headerLength + inIproc->length] = inIproc->cs; // Send the buffer. - int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength); + int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH); // Update stats outConnection->stats.txBytes += rc; + // evaluate return value before releasing the lock + UAVTalkRxState rxState = 0; + if (rc != (int32_t) (headerLength + inIproc->length + UAVTALK_CHECKSUM_LENGTH)) { + rxState = -1; + } + // Release lock xSemaphoreGiveRecursive(outConnection->lock); // Done - if (rc != inIproc->rxPacketLength) { - return -1; - } - - return 0; + return rxState; } /** @@ -712,8 +711,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, // Lock xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - // Get the handle to the Object. - // Will be zero if object does not exist. + // Get the handle to the object. Will be null 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... @@ -724,7 +722,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, switch (type) { case UAVTALK_TYPE_OBJ: case UAVTALK_TYPE_OBJ_TS: - // All instances, not allowed for OBJ messages + // 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! if (UAVObjUnpack(obj, instId, data) == 0) { @@ -743,7 +741,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, case UAVTALK_TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK_TS: - // All instances, not allowed for OBJ_ACK messages + // All instances not allowed for OBJ_ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Unpack object, if the instance does not exist it will be created! if (UAVObjUnpack(obj, instId, data) == 0) { @@ -788,7 +786,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, break; case UAVTALK_TYPE_ACK: - // All instances, not allowed for ACK messages + // All instances not allowed for ACK messages if (obj && (instId != UAVOBJ_ALL_INSTANCES)) { // Check if an ACK is pending updateAck(connection, type, objId, instId); @@ -816,9 +814,16 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, */ static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId) { - if ((connection->respType == type) && (connection->respObjId == objId) && ((connection->respInstId == instId) || (connection->respInstId == UAVOBJ_ALL_INSTANCES))) { - xSemaphoreGive(connection->respSema); - connection->respObjId = 0; + if ((connection->respObjId == objId) && (connection->respType == type)) { + if ((connection->respInstId == UAVOBJ_ALL_INSTANCES) && (instId == 0)) { + // last instance received, complete transaction + xSemaphoreGive(connection->respSema); + connection->respObjId = 0; + } + else if (connection->respInstId == instId) { + xSemaphoreGive(connection->respSema); + connection->respObjId = 0; + } } } @@ -852,7 +857,7 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 // Send all instances in reverse order // This allows the receiver to detect when the last object has been received (i.e. when instance 0 is received) for (n = 0; n < numInst; ++n) { - if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) < 0) { + if (sendSingleObject(connection, type, objId, numInst - n - 1, obj) == -1) { return -1; } } @@ -886,9 +891,6 @@ static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint3 */ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj) { - int32_t length; - int32_t dataOffset; - // IMPORTANT : obj can be null (when type is NACK for example) if (!connection->outStream) { @@ -908,17 +910,18 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, // Setup instance ID connection->txBuffer[8] = (uint8_t)(instId & 0xFF); connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); - dataOffset = 10; + int32_t headerLength = 10; // Add timestamp when the transaction type is appropriate if (type & UAVTALK_TIMESTAMPED) { portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; + connection->txBuffer[10] = (uint8_t)(time & 0xFF); + connection->txBuffer[11] = (uint8_t)((time >> 8) & 0xFF); + headerLength += 2; } // Determine data length + int32_t length; if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK || type == UAVTALK_TYPE_NACK) { length = 0; } else { @@ -926,32 +929,41 @@ static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, } // Check length - if (length >= UAVTALK_MAX_PAYLOAD_LENGTH) { + if (length > UAVOBJECTS_LARGEST) { + connection->stats.txErrors++; return -1; } // Copy data (if any) if (length > 0) { - if (UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0) { + if (UAVObjPack(obj, instId, &connection->txBuffer[headerLength]) == -1) { + connection->stats.txErrors++; return -1; } } // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset + length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset + length) >> 8) & 0xFF); + connection->txBuffer[2] = (uint8_t)((headerLength + length) & 0xFF); + connection->txBuffer[3] = (uint8_t)(((headerLength + length) >> 8) & 0xFF); - // Calculate checksum - connection->txBuffer[dataOffset + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset + length); + // Calculate and store checksum + connection->txBuffer[headerLength + length] = PIOS_CRC_updateCRC(0, connection->txBuffer, headerLength + length); - uint16_t tx_msg_len = dataOffset + length + UAVTALK_CHECKSUM_LENGTH; + // Send object + uint16_t tx_msg_len = headerLength + length + UAVTALK_CHECKSUM_LENGTH; int32_t rc = (*connection->outStream)(connection->txBuffer, tx_msg_len); + // Update stats if (rc == tx_msg_len) { - // Update stats ++connection->stats.txObjects; - connection->stats.txBytes += tx_msg_len; connection->stats.txObjectBytes += length; + connection->stats.txBytes += tx_msg_len; + } + else { + connection->stats.txErrors++; + // TDOD rc == -1 connection not open, -2 buffer full should retry + connection->stats.txBytes += (rc > 0) ? rc : 0; + return -1; } // Done diff --git a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp index a9f316ff3..e8a67f53d 100644 --- a/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp +++ b/ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp @@ -354,7 +354,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte) { UAVObject *rxObj = objMngr->getObject(rxObjId); if (rxObj == NULL && rxType != TYPE_OBJ_REQ) { - qWarning() << "UAVTalk - error : missing object" << rxObjId; + qWarning() << "UAVTalk - error : unknown object" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; break; @@ -420,13 +420,13 @@ bool UAVTalk::processInputByte(quint8 rxbyte) if (rxCS != rxCSPacket) { // packet error - faulty CRC - qWarning() << "UAVTalk - error : faulty CRC" << rxObjId; + qWarning() << "UAVTalk - error : failed CRC check" << rxObjId; stats.rxErrors++; rxState = STATE_SYNC; break; } - if (rxPacketLength != packetSize + 1) { + if (rxPacketLength != packetSize + CHECKSUM_LENGTH) { // packet error - mismatched packet size qWarning() << "UAVTalk - error : mismatched packet size" << rxObjId; stats.rxErrors++; @@ -773,6 +773,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U // Check length if (length >= MAX_PAYLOAD_LENGTH) { qWarning() << "UAVTalk - error transmitting : object exceeds max payload length" << obj->toStringBrief(); + ++stats.txErrors; return false; } @@ -780,6 +781,7 @@ bool UAVTalk::transmitSingleObject(quint8 type, quint32 objId, quint16 instId, U if (length > 0) { if (!obj->pack(&txBuffer[HEADER_LENGTH])) { qWarning() << "UAVTalk - error transmitting : failed to pack object" << obj->toStringBrief(); + ++stats.txErrors; return false; } }