diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index 3b7f2a2f9..f4616ae7d 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -45,13 +45,15 @@ typedef struct { typedef void* UAVTalkConnection; +typedef enum {UAVTALK_STATE_ERROR=0, UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS, UAVTALK_STATE_COMPLETE} UAVTalkRxState; + // Public functions UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream); int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream); UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection); int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); -int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); +UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); void UAVTalkResetStats(UAVTalkConnection connection); diff --git a/flight/UAVTalk/inc/uavtalk_priv.h b/flight/UAVTalk/inc/uavtalk_priv.h index 77863a6a8..992bf91af 100644 --- a/flight/UAVTalk/inc/uavtalk_priv.h +++ b/flight/UAVTalk/inc/uavtalk_priv.h @@ -55,8 +55,6 @@ typedef uint8_t uavtalk_checksum; #define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH #define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH -typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState; - typedef struct { UAVObjHandle obj; uint8_t type; diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index 844ed5e38..fe4593f93 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -252,10 +252,9 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle * Process an byte from the telemetry stream. * \param[in] connection UAVTalkConnection to be used * \param[in] rxbyte Received byte - * \return 0 Success - * \return -1 Failure + * \return UAVTalkRxState */ -int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) { UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle,connection,return -1); @@ -265,6 +264,9 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if (iproc->rxPacketLength < 0xffff) iproc->rxPacketLength++; // update packet byte count + + if (iproc->state == UAVTALK_STATE_ERROR || iproc->state == UAVTALK_STATE_COMPLETE) + iproc->state = UAVTALK_STATE_SYNC; // Receive state machine switch (iproc->state) @@ -288,7 +290,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) { - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } @@ -316,7 +318,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH) { // incorrect packet size - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } @@ -334,18 +336,8 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if (iproc->rxCount < 4) break; - - // Search for object, if not found reset state machine - // except if we got a OBJ_REQ for an object which does not - // exist, in which case we'll send a NACK iproc->obj = UAVObjGetByID(iproc->objId); - if (iproc->obj == 0 && iproc->type != UAVTALK_TYPE_OBJ_REQ) - { - connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; - break; - } // Determine data length if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) @@ -355,15 +347,24 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx } else { - iproc->length = UAVObjGetNumBytes(iproc->obj); - iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); + 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; + } } // Check length and determine next state if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) { connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } @@ -371,20 +372,19 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if ((iproc->rxPacketLength + iproc->instanceLength + iproc->length) != iproc->packet_size) { // packet error - mismatched packet size connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } iproc->instId = 0; - if (iproc->obj == 0) + if (iproc->type == UAVTALK_TYPE_NACK) { // If this is a NACK, we skip to Checksum iproc->state = UAVTALK_STATE_CS; iproc->rxCount = 0; - } // Check if this is a single instance object (i.e. if the instance ID field is coming next) - else if (UAVObjIsSingleInstance(iproc->obj)) + else if ((iproc->obj == 0) || !UAVObjIsSingleInstance(iproc->obj)) { // If there is a payload get it, otherwise receive checksum if (iproc->length > 0) @@ -441,33 +441,37 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx if (rxbyte != iproc->cs) { // packet error - faulty CRC connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } if (iproc->rxPacketLength != (iproc->packet_size + 1)) { // packet error - mismatched packet size connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; break; } - - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); - connection->stats.rxObjectBytes += iproc->length; - connection->stats.rxObjects++; - xSemaphoreGiveRecursive(connection->lock); - - iproc->state = UAVTALK_STATE_SYNC; + + if (iproc->obj) + { + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + connection->stats.rxObjectBytes += iproc->length; + connection->stats.rxObjects++; + xSemaphoreGiveRecursive(connection->lock); + } + + iproc->state = UAVTALK_STATE_COMPLETE; break; default: connection->stats.rxErrors++; - iproc->state = UAVTALK_STATE_SYNC; + iproc->state = UAVTALK_STATE_ERROR; + break; } // Done - return 0; + return iproc->state; } /**