rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

UAVTalk: Make UAVTalk object oriented, allowing multiple UAVTalk instances

Browse Source
This commit is contained in:
Corvus Corax 2011-06-16 16:35:37 +02:00
parent 144f36dfb7
commit 926246b402
3 changed files with 277 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/Modules/Telemetry/telemetry.c
View File

@ -66,6 +66,7 @@ static uint32_t txErrors;
static uint32_t txRetries; static uint32_t txRetries;
static TelemetrySettingsData settings; static TelemetrySettingsData settings;
static uint32_t timeOfLastObjectUpdate; static uint32_t timeOfLastObjectUpdate;
static UAVTalkConnection uavTalkCon;
// Private functions // Private functions
static void telemetryTxTask(void *parameters); static void telemetryTxTask(void *parameters);
@ -102,7 +103,7 @@ int32_t TelemetryInitialize(void)
updateSettings(); updateSettings();
// Initialise UAVTalk // Initialise UAVTalk
UAVTalkInitialize(&transmitData); UAVTalkInitialize(&uavTalkCon, &transmitData);
// Process all registered objects and connect queue for updates // Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject); UAVObjIterate(&registerObject);
@ -221,7 +222,7 @@ static void processObjEvent(UAVObjEvent * ev)
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) { if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
// Send update to GCS (with retries) // Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout success = UAVTalkSendObject(&uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries; ++retries;
} }
// Update stats // Update stats
@ -232,7 +233,7 @@ static void processObjEvent(UAVObjEvent * ev)
} else if (ev->event == EV_UPDATE_REQ) { } else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries) // Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout success = UAVTalkSendObjectRequest(&uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries; ++retries;
} }
// Update stats // Update stats
@ -309,7 +310,7 @@ static void telemetryRxTask(void *parameters)
// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
len = PIOS_COM_ReceiveBufferUsed(inputPort); len = PIOS_COM_ReceiveBufferUsed(inputPort);
for (int32_t n = 0; n < len; ++n) { for (int32_t n = 0; n < len; ++n) {
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort)); UAVTalkProcessInputStream(&uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort));
} }
vTaskDelay(5); // <- remove when blocking calls are implemented vTaskDelay(5); // <- remove when blocking calls are implemented
@ -403,8 +404,8 @@ static void updateTelemetryStats()
uint32_t timeNow; uint32_t timeNow;
// Get stats // Get stats
UAVTalkGetStats(&utalkStats); UAVTalkGetStats(&uavTalkCon, &utalkStats);
UAVTalkResetStats(); UAVTalkResetStats(&uavTalkCon);
// Get object data // Get object data
FlightTelemetryStatsGet(&flightStats); FlightTelemetryStatsGet(&flightStats);

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

@ -32,10 +32,21 @@
// Public constants // Public constants
#define UAVTALK_WAITFOREVER -1 #define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0 #define UAVTALK_NOWAIT 0
#define UAVTALK_MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4)
#define UAVTALK_MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects)
#define UAVTALK_CHECKSUM_LENGTH 1
#define UAVTALK_MAX_PAYLOAD_LENGTH 256
#define UAVTALK_MAX_PACKET_LENGTH (UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH + UAVTALK_CHECKSUM_LENGTH)
// Public types // Public types
typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t 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 {
uint32_t txBytes; uint32_t txBytes;
uint32_t rxBytes; uint32_t rxBytes;
@ -47,14 +58,41 @@ typedef struct {
uint32_t rxErrors; uint32_t rxErrors;
} UAVTalkStats; } UAVTalkStats;
typedef struct {
UAVObjHandle obj;
uint8_t type;
uint16_t packet_size;
uint32_t objId;
uint16_t instId;
uint32_t length;
uint8_t cs;
int32_t rxCount;
UAVTalkRxState state;
uint16_t rxPacketLength;
} UAVTalkInputProcessor;
typedef struct {
UAVTalkOutputStream outStream;
xSemaphoreHandle lock;
xSemaphoreHandle transLock;
xSemaphoreHandle respSema;
UAVObjHandle respObj;
uint16_t respInstId;
uint8_t rxBuffer[UAVTALK_MAX_PACKET_LENGTH];
uint8_t txBuffer[UAVTALK_MAX_PACKET_LENGTH];
UAVTalkStats stats;
UAVTalkInputProcessor iproc;
} UAVTalkConnection;
// Public functions // Public functions
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream); int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream);
int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream); int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream);
int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection);
int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(uint8_t rxbyte); int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
void UAVTalkGetStats(UAVTalkStats* stats); int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte);
void UAVTalkResetStats(); void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* stats);
void UAVTalkResetStats(UAVTalkConnection *connection);
#endif // UAVTALK_H #endif // UAVTALK_H
/** /**

436
flight/UAVTalk/uavtalk.c
View File

@ -41,102 +41,119 @@
#define TYPE_ACK (TYPE_VER | 0x03) #define TYPE_ACK (TYPE_VER | 0x03)
#define TYPE_NACK (TYPE_VER | 0x04) #define TYPE_NACK (TYPE_VER | 0x04)
#define MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4)
#define MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects)
#define CHECKSUM_LENGTH 1
#define MAX_PAYLOAD_LENGTH 256
#define MAX_PACKET_LENGTH (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH)
// Private types // Private types
typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState;
// Private variables
static UAVTalkOutputStream outStream;
static xSemaphoreHandle lock;
static xSemaphoreHandle transLock;
static xSemaphoreHandle respSema;
static UAVObjHandle respObj;
static uint16_t respInstId;
static uint8_t rxBuffer[MAX_PACKET_LENGTH];
static uint8_t txBuffer[MAX_PACKET_LENGTH];
static UAVTalkStats stats;
// Private functions // Private functions
static int32_t objectTransaction(UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type); static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendNack(uint32_t objId); static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId);
static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length); static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length);
static void updateAck(UAVObjHandle obj, uint16_t instId); static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId);
/** /**
* Initialize the UAVTalk library * Initialize the UAVTalk library
* \param[in] connection UAVTalkConnection to be used
* \param[in] outputStream Function pointer that is called to send a data buffer * \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream)
{ {
outStream = outputStream; connection->iproc.rxPacketLength = 0;
lock = xSemaphoreCreateRecursiveMutex(); connection->iproc.state = UAVTALK_STATE_SYNC;
transLock = xSemaphoreCreateRecursiveMutex(); connection->outStream = outputStream;
vSemaphoreCreateBinary(respSema); connection->lock = xSemaphoreCreateRecursiveMutex();
xSemaphoreTake(respSema, 0); // reset to zero connection->transLock = xSemaphoreCreateRecursiveMutex();
UAVTalkResetStats(); vSemaphoreCreateBinary(connection->respSema);
xSemaphoreTake(connection->respSema, 0); // reset to zero
UAVTalkResetStats(connection);
return 0; return 0;
} }
/** /**
* Get communication statistics counters * Set the communication output stream
* @param[out] statsOut Statistics counters * \param[in] connection UAVTalkConnection to be used
* \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success
* \return -1 Failure
*/ */
void UAVTalkGetStats(UAVTalkStats* statsOut) int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream)
{ {
// Lock // Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Copy stats // set output stream
memcpy(statsOut, &stats, sizeof(UAVTalkStats)); connection->outStream = outputStream;
// Release lock // Release lock
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
return 0;
}
/**
* Get current output stream
* \param[in] connection UAVTalkConnection to be used
* @return UAVTarlkOutputStream the output stream used
*/
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection)
{
return connection->outStream;
}
/**
* Get communication statistics counters
* \param[in] connection UAVTalkConnection to be used
* @param[out] statsOut Statistics counters
*/
void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut)
{
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Copy stats
memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats));
// Release lock
xSemaphoreGiveRecursive(connection->lock);
} }
/** /**
* Reset the statistics counters. * Reset the statistics counters.
* \param[in] connection UAVTalkConnection to be used
*/ */
void UAVTalkResetStats() void UAVTalkResetStats(UAVTalkConnection *connection)
{ {
// Lock // Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Clear stats // Clear stats
memset(&stats, 0, sizeof(UAVTalkStats)); memset(&connection->stats, 0, sizeof(UAVTalkStats));
// Release lock // Release lock
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
} }
/** /**
* Request an update for the specified object, on success the object data would have been * Request an update for the specified object, on success the object data would have been
* updated by the GCS. * updated by the GCS.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object to update * \param[in] obj Object to update
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] timeout Time to wait for the response, when zero it will return immediately * \param[in] timeout Time to wait for the response, when zero it will return immediately
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeout) int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeout)
{ {
return objectTransaction(obj, instId, TYPE_OBJ_REQ, timeout); return objectTransaction(connection, obj, instId, TYPE_OBJ_REQ, timeout);
} }
/** /**
* Send the specified object through the telemetry link. * Send the specified object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object to send * \param[in] obj Object to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required) * \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
@ -144,21 +161,22 @@ int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t time
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkSendObject(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)
{ {
// Send object // Send object
if (acked == 1) if (acked == 1)
{ {
return objectTransaction(obj, instId, TYPE_OBJ_ACK, timeoutMs); return objectTransaction(connection, obj, instId, TYPE_OBJ_ACK, timeoutMs);
} }
else else
{ {
return objectTransaction(obj, instId, TYPE_OBJ, timeoutMs); return objectTransaction(connection, obj, instId, TYPE_OBJ, timeoutMs);
} }
} }
/** /**
* Execute the requested transaction on an object. * Execute the requested transaction on an object.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object * \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] type Transaction type * \param[in] type Transaction type
@ -168,7 +186,7 @@ int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int3
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs) static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
{ {
int32_t respReceived; int32_t respReceived;
@ -176,37 +194,37 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
{ {
// Get transaction lock (will block if a transaction is pending) // Get transaction lock (will block if a transaction is pending)
xSemaphoreTakeRecursive(transLock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
// Send object // Send object
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
respObj = obj; connection->respObj = obj;
respInstId = instId; connection->respInstId = instId;
sendObject(obj, instId, type); sendObject(connection, obj, instId, type);
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
// Wait for response (or timeout) // Wait for response (or timeout)
respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS);
// Check if a response was received // Check if a response was received
if (respReceived == pdFALSE) if (respReceived == pdFALSE)
{ {
// Cancel transaction // Cancel transaction
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema) xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
respObj = 0; connection->respObj = 0;
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
xSemaphoreGiveRecursive(transLock); xSemaphoreGiveRecursive(connection->transLock);
return -1; return -1;
} }
else else
{ {
xSemaphoreGiveRecursive(transLock); xSemaphoreGiveRecursive(connection->transLock);
return 0; return 0;
} }
} }
else if (type == TYPE_OBJ) else if (type == TYPE_OBJ)
{ {
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
sendObject(obj, instId, TYPE_OBJ); sendObject(connection, obj, instId, TYPE_OBJ);
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
return 0; return 0;
} }
else else
@ -217,222 +235,211 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
/** /**
* Process an byte from the telemetry stream. * Process an byte from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte * \param[in] rxbyte Received byte
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
int32_t UAVTalkProcessInputStream(uint8_t rxbyte) int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
{ {
static UAVObjHandle obj; UAVTalkInputProcessor *iproc = &connection->iproc;
static uint8_t type; ++connection->stats.rxBytes;
static uint16_t packet_size;
static uint32_t objId;
static uint16_t instId;
static uint32_t length;
static uint8_t cs, csRx;
static int32_t rxCount;
static RxState state = STATE_SYNC;
static uint16_t rxPacketLength = 0;
++stats.rxBytes; if (iproc->rxPacketLength < 0xffff)
iproc->rxPacketLength++; // update packet byte count
if (rxPacketLength < 0xffff)
rxPacketLength++; // update packet byte count
// Receive state machine // Receive state machine
switch (state) switch (iproc->state)
{ {
case STATE_SYNC: case UAVTALK_STATE_SYNC:
if (rxbyte != SYNC_VAL) if (rxbyte != SYNC_VAL)
break; break;
// Initialize and update the CRC // Initialize and update the CRC
cs = PIOS_CRC_updateByte(0, rxbyte); iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
rxPacketLength = 1; iproc->rxPacketLength = 1;
state = STATE_TYPE; iproc->state = UAVTALK_STATE_TYPE;
break; break;
case STATE_TYPE: case UAVTALK_STATE_TYPE:
// update the CRC // update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & TYPE_MASK) != TYPE_VER) if ((rxbyte & TYPE_MASK) != TYPE_VER)
{ {
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
type = rxbyte; iproc->type = rxbyte;
packet_size = 0; iproc->packet_size = 0;
state = STATE_SIZE; iproc->state = UAVTALK_STATE_SIZE;
rxCount = 0; iproc->rxCount = 0;
break; break;
case STATE_SIZE: case UAVTALK_STATE_SIZE:
// update the CRC // update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (rxCount == 0) if (iproc->rxCount == 0)
{ {
packet_size += rxbyte; iproc->packet_size += rxbyte;
rxCount++; iproc->rxCount++;
break; break;
} }
packet_size += rxbyte << 8; iproc->packet_size += rxbyte << 8;
if (packet_size < MIN_HEADER_LENGTH || packet_size > MAX_HEADER_LENGTH + 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
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
rxCount = 0; iproc->rxCount = 0;
objId = 0; iproc->objId = 0;
state = STATE_OBJID; iproc->state = UAVTALK_STATE_OBJID;
break; break;
case STATE_OBJID: case UAVTALK_STATE_OBJID:
// update the CRC // update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
objId += rxbyte << (8*(rxCount++)); iproc->objId += rxbyte << (8*(iproc->rxCount++));
if (rxCount < 4) if (iproc->rxCount < 4)
break; break;
// Search for object, if not found reset state machine // Search for object, if not found reset state machine
// except if we got a OBJ_REQ for an object which does not // except if we got a OBJ_REQ for an object which does not
// exist, in which case we'll send a NACK // exist, in which case we'll send a NACK
obj = UAVObjGetByID(objId); iproc->obj = UAVObjGetByID(iproc->objId);
if (obj == 0 && type != TYPE_OBJ_REQ) if (iproc->obj == 0 && iproc->type != TYPE_OBJ_REQ)
{ {
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
// Determine data length // Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK) if (iproc->type == TYPE_OBJ_REQ || iproc->type == TYPE_ACK || iproc->type == TYPE_NACK)
length = 0; iproc->length = 0;
else else
length = UAVObjGetNumBytes(obj); iproc->length = UAVObjGetNumBytes(iproc->obj);
// Check length and determine next state // Check length and determine next state
if (length >= MAX_PAYLOAD_LENGTH) if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH)
{ {
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
// Check the lengths match // Check the lengths match
if ((rxPacketLength + length) != packet_size) if ((iproc->rxPacketLength + iproc->length) != iproc->packet_size)
{ // packet error - mismatched packet size { // packet error - mismatched packet size
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
instId = 0; iproc->instId = 0;
if (obj == 0) if (iproc->obj == 0)
{ {
// If this is a NACK, we skip to Checksum // If this is a NACK, we skip to Checksum
state = STATE_CS; iproc->state = UAVTALK_STATE_CS;
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(obj)) else if (UAVObjIsSingleInstance(iproc->obj))
{ {
// If there is a payload get it, otherwise receive checksum // If there is a payload get it, otherwise receive checksum
if (length > 0) if (iproc->length > 0)
state = STATE_DATA; iproc->state = UAVTALK_STATE_DATA;
else else
state = STATE_CS; iproc->state = UAVTALK_STATE_CS;
rxCount = 0; iproc->rxCount = 0;
} }
else else
{ {
state = STATE_INSTID; iproc->state = UAVTALK_STATE_INSTID;
rxCount = 0; iproc->rxCount = 0;
} }
break; break;
case STATE_INSTID: case UAVTALK_STATE_INSTID:
// update the CRC // update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
instId += rxbyte << (8*(rxCount++)); iproc->instId += rxbyte << (8*(iproc->rxCount++));
if (rxCount < 2) if (iproc->rxCount < 2)
break; break;
rxCount = 0; iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum // If there is a payload get it, otherwise receive checksum
if (length > 0) if (iproc->length > 0)
state = STATE_DATA; iproc->state = UAVTALK_STATE_DATA;
else else
state = STATE_CS; iproc->state = UAVTALK_STATE_CS;
break; break;
case STATE_DATA: case UAVTALK_STATE_DATA:
// update the CRC // update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte); iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
rxBuffer[rxCount++] = rxbyte; connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (rxCount < length) if (iproc->rxCount < iproc->length)
break; break;
state = STATE_CS; iproc->state = UAVTALK_STATE_CS;
rxCount = 0; iproc->rxCount = 0;
break; break;
case STATE_CS: case UAVTALK_STATE_CS:
// the CRC byte // the CRC byte
csRx = rxbyte; if (rxbyte != iproc->cs)
if (csRx != cs)
{ // packet error - faulty CRC { // packet error - faulty CRC
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
if (rxPacketLength != (packet_size + 1)) if (iproc->rxPacketLength != (iproc->packet_size + 1))
{ // packet error - mismatched packet size { // packet error - mismatched packet size
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
} }
xSemaphoreTakeRecursive(lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
receiveObject(type, objId, instId, rxBuffer, length); receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
stats.rxObjectBytes += length; connection->stats.rxObjectBytes += iproc->length;
stats.rxObjects++; connection->stats.rxObjects++;
xSemaphoreGiveRecursive(lock); xSemaphoreGiveRecursive(connection->lock);
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
break; break;
default: default:
stats.rxErrors++; connection->stats.rxErrors++;
state = STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
} }
// Done // Done
@ -441,6 +448,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
/** /**
* Receive an object. This function process objects received through the telemetry stream. * Receive an object. This function process objects received through the telemetry stream.
* \param[in] connection UAVTalkConnection to be used
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK) * \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
* \param[in] objId ID of the object to work on * \param[in] objId ID of the object to work on
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
@ -449,9 +457,9 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length) static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length)
{ {
static UAVObjHandle obj; UAVObjHandle obj;
int32_t ret = 0; int32_t ret = 0;
// Get the handle to the Object. Will be zero // Get the handle to the Object. Will be zero
@ -467,7 +475,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
// Unpack object, if the instance does not exist it will be created! // Unpack object, if the instance does not exist it will be created!
UAVObjUnpack(obj, instId, data); UAVObjUnpack(obj, instId, data);
// Check if an ack is pending // Check if an ack is pending
updateAck(obj, instId); updateAck(connection, obj, instId);
} }
else else
{ {
@ -482,7 +490,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
if ( UAVObjUnpack(obj, instId, data) == 0 ) if ( UAVObjUnpack(obj, instId, data) == 0 )
{ {
// Transmit ACK // Transmit ACK
sendObject(obj, instId, TYPE_ACK); sendObject(connection, obj, instId, TYPE_ACK);
} }
else else
{ {
@ -497,9 +505,9 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
case TYPE_OBJ_REQ: case TYPE_OBJ_REQ:
// Send requested object if message is of type OBJ_REQ // Send requested object if message is of type OBJ_REQ
if (obj == 0) if (obj == 0)
sendNack(objId); sendNack(connection, objId);
else else
sendObject(obj, instId, TYPE_OBJ); sendObject(connection, obj, instId, TYPE_OBJ);
break; break;
case TYPE_NACK: case TYPE_NACK:
// Do nothing on flight side, let it time out. // Do nothing on flight side, let it time out.
@ -509,7 +517,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
if (instId != UAVOBJ_ALL_INSTANCES) if (instId != UAVOBJ_ALL_INSTANCES)
{ {
// Check if an ack is pending // Check if an ack is pending
updateAck(obj, instId); updateAck(connection, obj, instId);
} }
else else
{ {
@ -525,25 +533,29 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
/** /**
* Check if an ack is pending on an object and give response semaphore * Check if an ack is pending on an object and give response semaphore
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
*/ */
static void updateAck(UAVObjHandle obj, uint16_t instId) static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId)
{ {
if (respObj == obj && (respInstId == instId || respInstId == UAVOBJ_ALL_INSTANCES)) if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES))
{ {
xSemaphoreGive(respSema); xSemaphoreGive(connection->respSema);
respObj = 0; connection->respObj = 0;
} }
} }
/** /**
* Send an object through the telemetry link. * Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send * \param[in] obj Object handle to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances * \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
* \param[in] type Transaction type * \param[in] type Transaction type
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type) static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
{ {
uint32_t numInst; uint32_t numInst;
uint32_t n; uint32_t n;
@ -564,24 +576,24 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Send all instances // Send all instances
for (n = 0; n < numInst; ++n) for (n = 0; n < numInst; ++n)
{ {
sendSingleObject(obj, n, type); sendSingleObject(connection, obj, n, type);
} }
return 0; return 0;
} }
else else
{ {
return sendSingleObject(obj, instId, type); return sendSingleObject(connection, obj, instId, type);
} }
} }
else if (type == TYPE_OBJ_REQ) else if (type == TYPE_OBJ_REQ)
{ {
return sendSingleObject(obj, instId, TYPE_OBJ_REQ); return sendSingleObject(connection, obj, instId, TYPE_OBJ_REQ);
} }
else if (type == TYPE_ACK) else if (type == TYPE_ACK)
{ {
if ( instId != UAVOBJ_ALL_INSTANCES ) if ( instId != UAVOBJ_ALL_INSTANCES )
{ {
return sendSingleObject(obj, instId, TYPE_ACK); return sendSingleObject(connection, obj, instId, TYPE_ACK);
} }
else else
{ {
@ -596,13 +608,14 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
/** /**
* Send an object through the telemetry link. * Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send * \param[in] obj Object handle to send
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead) * \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead)
* \param[in] type Transaction type * \param[in] type Transaction type
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type) static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
{ {
int32_t length; int32_t length;
int32_t dataOffset; int32_t dataOffset;
@ -610,13 +623,13 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Setup type and object id fields // Setup type and object id fields
objId = UAVObjGetID(obj); objId = UAVObjGetID(obj);
txBuffer[0] = SYNC_VAL; // sync byte connection->txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = type; connection->txBuffer[1] = type;
// data length inserted here below // data length inserted here below
txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
// Setup instance ID if one is required // Setup instance ID if one is required
if (UAVObjIsSingleInstance(obj)) if (UAVObjIsSingleInstance(obj))
@ -625,8 +638,8 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
} }
else else
{ {
txBuffer[8] = (uint8_t)(instId & 0xFF); connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF); connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 10; dataOffset = 10;
} }
@ -641,7 +654,7 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
} }
// Check length // Check length
if (length >= MAX_PAYLOAD_LENGTH) if (length >= UAVTALK_MAX_PAYLOAD_LENGTH)
{ {
return -1; return -1;
} }
@ -649,26 +662,26 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Copy data (if any) // Copy data (if any)
if (length > 0) if (length > 0)
{ {
if ( UAVObjPack(obj, instId, &txBuffer[dataOffset]) < 0 ) if ( UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0 )
{ {
return -1; return -1;
} }
} }
// Store the packet length // Store the packet length
txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF); connection->txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF);
txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF); connection->txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF);
// Calculate checksum // Calculate checksum
txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset+length); connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length);
// Send buffer // Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH); if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+length+UAVTALK_CHECKSUM_LENGTH);
// Update stats // Update stats
++stats.txObjects; ++connection->stats.txObjects;
stats.txBytes += dataOffset+length+CHECKSUM_LENGTH; connection->stats.txBytes += dataOffset+length+UAVTALK_CHECKSUM_LENGTH;
stats.txObjectBytes += length; connection->stats.txObjectBytes += length;
// Done // Done
return 0; return 0;
@ -676,36 +689,37 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
/** /**
* Send a NACK through the telemetry link. * Send a NACK through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for * \param[in] objId Object ID to send a NACK for
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t sendNack(uint32_t objId) static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId)
{ {
int32_t dataOffset; int32_t dataOffset;
txBuffer[0] = SYNC_VAL; // sync byte connection->txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = TYPE_NACK; connection->txBuffer[1] = TYPE_NACK;
// data length inserted here below // data length inserted here below
txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF); connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF); connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
dataOffset = 8; dataOffset = 8;
// Store the packet length // Store the packet length
txBuffer[2] = (uint8_t)((dataOffset) & 0xFF); connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF); connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
// Calculate checksum // Calculate checksum
txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset); connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset);
// Send buffer // Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+CHECKSUM_LENGTH); if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+UAVTALK_CHECKSUM_LENGTH);
// Update stats // Update stats
stats.txBytes += dataOffset+CHECKSUM_LENGTH; connection->stats.txBytes += dataOffset+UAVTALK_CHECKSUM_LENGTH;
// Done // Done
return 0; return 0;