rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

Added returning of UAVTalk state from UAVTalkProcessInputStream

Browse Source
This commit is contained in:
Brian Webb 2012-04-16 20:21:00 -07:00
parent 4936cd8fd2
commit 78c7416bda
3 changed files with 41 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/UAVTalk/inc/uavtalk.h
View File

@ -45,13 +45,15 @@ typedef struct {
typedef void* UAVTalkConnection; 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 // Public functions
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream); UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream);
int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream); int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection); UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); 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 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 UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkResetStats(UAVTalkConnection connection); void UAVTalkResetStats(UAVTalkConnection connection);

2
flight/UAVTalk/inc/uavtalk_priv.h
View File

@ -55,8 +55,6 @@ typedef uint8_t uavtalk_checksum;
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH #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 #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 { typedef struct {
UAVObjHandle obj; UAVObjHandle obj;
uint8_t type; uint8_t type;

54
flight/UAVTalk/uavtalk.c
View File

@ -252,10 +252,9 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle
* Process an byte from the telemetry stream. * Process an byte from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used * \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte * \param[in] rxbyte Received byte
* \return 0 Success * \return UAVTalkRxState
* \return -1 Failure
*/ */
int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
{ {
UAVTalkConnectionData *connection; UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1); CHECKCONHANDLE(connectionHandle,connection,return -1);
@ -266,6 +265,9 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
if (iproc->rxPacketLength < 0xffff) if (iproc->rxPacketLength < 0xffff)
iproc->rxPacketLength++; // update packet byte count 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 // Receive state machine
switch (iproc->state) switch (iproc->state)
{ {
@ -288,7 +290,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER) if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER)
{ {
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; 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) if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH)
{ // incorrect packet size { // incorrect packet size
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; break;
} }
@ -335,17 +337,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
if (iproc->rxCount < 4) if (iproc->rxCount < 4)
break; 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); 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 // Determine data length
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK) if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK)
@ -354,16 +346,25 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
iproc->instanceLength = 0; iproc->instanceLength = 0;
} }
else else
{
if (iproc->obj)
{ {
iproc->length = UAVObjGetNumBytes(iproc->obj); iproc->length = UAVObjGetNumBytes(iproc->obj);
iproc->instanceLength = (UAVObjIsSingleInstance(iproc->obj) ? 0 : 2); 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 // Check length and determine next state
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH) if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH)
{ {
connection->stats.rxErrors++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; break;
} }
@ -371,20 +372,19 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
if ((iproc->rxPacketLength + iproc->instanceLength + iproc->length) != iproc->packet_size) if ((iproc->rxPacketLength + iproc->instanceLength + iproc->length) != iproc->packet_size)
{ // packet error - mismatched packet size { // packet error - mismatched packet size
connection->stats.rxErrors++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; break;
} }
iproc->instId = 0; iproc->instId = 0;
if (iproc->obj == 0) if (iproc->type == UAVTALK_TYPE_NACK)
{ {
// If this is a NACK, we skip to Checksum // If this is a NACK, we skip to Checksum
iproc->state = UAVTALK_STATE_CS; iproc->state = UAVTALK_STATE_CS;
iproc->rxCount = 0; iproc->rxCount = 0;
} }
// Check if this is a single instance object (i.e. if the instance ID field is coming next) // 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 there is a payload get it, otherwise receive checksum
if (iproc->length > 0) if (iproc->length > 0)
@ -441,33 +441,37 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rx
if (rxbyte != iproc->cs) if (rxbyte != iproc->cs)
{ // packet error - faulty CRC { // packet error - faulty CRC
connection->stats.rxErrors++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; break;
} }
if (iproc->rxPacketLength != (iproc->packet_size + 1)) if (iproc->rxPacketLength != (iproc->packet_size + 1))
{ // packet error - mismatched packet size { // packet error - mismatched packet size
connection->stats.rxErrors++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break; break;
} }
if (iproc->obj)
{
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
connection->stats.rxObjectBytes += iproc->length; connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++; connection->stats.rxObjects++;
xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->lock);
}
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_COMPLETE;
break; break;
default: default:
connection->stats.rxErrors++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_ERROR;
break;
} }
// Done // Done
return 0; return iproc->state;
} }
/** /**